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

#include "libplump/hpyp_model.h"

#include <cmath>
#include <boost/bind.hpp>
#include <boost/shared_ptr.hpp>

#include "libplump/utils.h"
#include "libplump/subseq.h"
#include "libplump/stirling.h"
#include "libplump/random.h"
#include "libplump/hpyp_restaurants.h"

namespace gatsby { namespace libplump {

HPYPModel::HPYPModel(seq_type& seq,
                     INodeManager& nodeManager,
                     const IAddRemoveRestaurant& restaurant,
                     IParameters& parameters,
                     int numTypes)
    : seq(seq),
      contextTree_(new ContextTree(nodeManager, seq)),
      contextTree(*contextTree_),
      restaurant(restaurant),
      parameters(parameters),
      numTypes(numTypes) {
    baseProb = 1./((double) numTypes);
  }

void HPYPModel::insertRoot(e_type obs) {
  WrappedNodeList root_path = this->contextTree.findLongestSuffix(0,0);
  d_vec discount_path = this->parameters.getDiscounts(root_path);
  d_vec concentration_path = this->parameters.getConcentrations(root_path,
                                                                discount_path);
  d_vec prob_path = this->computeProbabilityPath(root_path,
                                                 discount_path,
                                                 concentration_path,
                                                 obs);
  this->updatePath(root_path,
                   prob_path,
                   discount_path,
                   concentration_path,
                   obs);
}

/**
 * Insert a context into the tree and handle a potentially occuring
 * split.
 */
WrappedNodeList HPYPModel::insertContext(l_type start, l_type stop) {
  typedef ContextTree::InsertionResult InsertionResult;

  // insert context
  InsertionResult insertionResult = contextTree.insert(start, stop);

  // handle split if one occurred
  if (insertionResult.action != InsertionResult::INSERT_ACTION_NO_SPLIT) {
    WrappedNode nodeA, nodeC;
    WrappedNodeList::iterator i = insertionResult.path.end();

    // move iterator to point to the split node which is either the
    // last or second to last element on the path, depending on
    // whether the inserted node was a suffix
    switch (insertionResult.action) {
      case InsertionResult::INSERT_ACTION_SPLIT :
        // split where inserted node is not a suffix of a node in
        // the tree
        i--; i--; // split node is above the newly inserted node
        break;
      case InsertionResult::INSERT_ACTION_SPLIT_SUFFIX :
        // split where inserted node is a suffix -> inserted node
        // is the shorter node
        i--; // split node is the newly inserted node
        break;
      case InsertionResult::INSERT_ACTION_NO_SPLIT :
        break;
    }
    nodeC = *i;
    i--; // move iterator to parent
    nodeA = *i;
    this->handleSplit(nodeA,
                      insertionResult.splitChild,
                      nodeC);
  }
  return insertionResult.path;
}

d_vec HPYPModel::insertContextAndObservation(l_type start,
                                             l_type stop,
                                             e_type obs) {
  // insert context (and handle a potential split)
  WrappedNodeList path = insertContext(start, stop);

  // insert observation
  d_vec discountPath = this->parameters.getDiscounts(path);
  d_vec concentrationPath = this->parameters.getConcentrations(path,
                                                               discountPath);
  d_vec probabilityPath = this->computeProbabilityPath(path,
                                                       discountPath,
                                                       concentrationPath,
                                                       obs);
  this->parameters.accumulateParameterGradient(this->restaurant,
      path, probabilityPath, discountPath, concentrationPath, obs);

  this->updatePath(path, probabilityPath, discountPath,
                   concentrationPath, obs);

  // static int j = 0;
  //if (j == 1) {
    this->parameters.stepParameterGradient(10e-4);
  //  j = 0;
  //}

  return probabilityPath;
}

d_vec HPYPModel::insertObservation(l_type start, l_type stop, e_type obs,
    WrappedNodeList* cached_path) {
  tracer << "HPYPModel::insertObservation(" << start << ", " << stop
         << ", " << obs << ")" << std::endl;

  WrappedNodeList path;
  if (cached_path != NULL) {
    path = *cached_path;
  } else {
    path = this->contextTree.findLongestSuffix(start,stop);
  }

  tracer << "  HPYPModel::insertObservation: longest suffix path: "
         << std::endl << this->contextTree.pathToString(path) << std::endl;

  d_vec discount_path = this->parameters.getDiscounts(path);
  d_vec concentration_path = this->parameters.getConcentrations(path,
                                                                discount_path);
  d_vec prob_path = this->computeProbabilityPath(path,
                                                 discount_path,
                                                 concentration_path,
                                                 obs);
  this->updatePath(path, prob_path, discount_path,
                   concentration_path, obs);
  return prob_path;
}

void HPYPModel::removeObservation(
    l_type start, l_type stop, e_type obs,
    const HPYPModel::PayloadDataPath& payloadDataPath,
    WrappedNodeList* cached_path) {
  tracer << "HPYPModel::removeObservation(" << start << ", " << stop
         << ", " << obs << ")" << std::endl;

  WrappedNodeList path;
  if (cached_path != NULL) {
    path = *cached_path;
  } else {
    path = this->contextTree.findLongestSuffix(start,stop);
  }
  assert(path.back().end == (*cached_path).back().end);

  tracer << "  HPYPModel::removeObservation: longest suffix path: "
         << std::endl << contextTree.pathToString(path) << std::endl;

  d_vec discountPath = this->parameters.getDiscounts(path);

  this->removeObservationFromPath(path,
                                  discountPath,
                                  obs,
                                  payloadDataPath);
}

void HPYPModel::removeAddSweep(l_type start, l_type stop) {
  // start timer
  clock_t start_t,end_t;
  start_t = clock();
  for (int i = start; i < stop; ++i) {
    HPYPModel::PayloadDataPath payloadDataPath;
    WrappedNodeList path = this->contextTree.findNode(start, i);
    //WrappedNodeList p2 = this->contextTree.findLongestSuffix(start, i);
    //std::cout << path.back().end << ", " << p2.back().end
    //          << ", " << path.size() << ", " << p2.size() <<std::endl;
    //assert(path.back().end == p2.back().end);
    this->removeObservation(start, i, this->seq[i], payloadDataPath, &path);
    this->insertObservation(start, i, this->seq[i], &path);

    if (i%10000==0) {
      end_t = clock();
      std::cerr << makeProgressBarString(i/(double)stop) << " "
                << ((double)i*CLOCKS_PER_SEC)/(end_t-start_t) << " chars/sec"
                <<  "\r";
    }
  }
}

d_vec HPYPModel::computeLosses(l_type start, l_type stop) {
  d_vec losses;

  // deal with first symbol: add loss and insert customer
  losses.push_back(log2((double) this->numTypes));
  insertRoot(this->seq[start]);

  // start timer
  clock_t start_t,end_t;
  start_t = clock();

  for (l_type i=start+1; i < stop; i++) {
    d_vec prob_path = this->insertContextAndObservation(start,i,this->seq[i]);
    double prob = prob_path[prob_path.size()-2];
    losses.push_back(-log2(prob));

    if (i%10000==0) {
      end_t = clock();
      std::cerr << makeProgressBarString(i/(double)stop) << " "
                << ((double)i*CLOCKS_PER_SEC)/(end_t-start_t) << " chars/sec"
                <<  "\r";
    }

  }
  end_t = clock();

  std::cerr << makeProgressBarString(1) << " "
            << ((double)stop*CLOCKS_PER_SEC)/(end_t-start_t) << " chars/sec"
            <<  std::endl;

  return losses;
}

d_vec HPYPModel::computeLossesWithDeletion(l_type start, l_type stop, l_type lag) {
  d_vec losses;

  // deal with first symbol: add loss and insert customer
  losses.push_back(log2((double) this->numTypes));
  insertRoot(this->seq[start]);

  // start timer
  clock_t start_t,end_t;
  start_t = clock();

  for (l_type i=start+1; i < stop; i++) {
    d_vec prob_path = this->insertContextAndObservation(start,i,this->seq[i]);
    double prob = prob_path[prob_path.size()-2];
    losses.push_back(-log2(prob));
    if (i - lag >= start) {
      HPYPModel::PayloadDataPath payloadDataPath;
      WrappedNodeList path = this->contextTree.findNode(start, i - lag);
      this->removeObservation(start, i - lag, this->seq[i - lag], payloadDataPath, &path);
    }

    if (i%10000==0) {
      end_t = clock();
      std::cerr << makeProgressBarString(i/(double)stop) << " "
        << ((double)i*CLOCKS_PER_SEC)/(end_t-start_t) << " chars/sec"
        <<  "\r";
    }

  }
  end_t = clock();

  std::cerr << makeProgressBarString(1) << " "
    << ((double)stop*CLOCKS_PER_SEC)/(end_t-start_t) << " chars/sec"
    <<  std::endl;

  return losses;
}

d_vec HPYPModel::predictSequence(l_type start, l_type stop, PredictMode mode) {
  d_vec probs;
  for (l_type i = start; i < stop; i++) {
    switch(mode) {
      case ABOVE:
        probs.push_back(this->predict(start, i, this->seq[i]));
        break;
      case FRAGMENT:
        probs.push_back(this->predictWithFragmentation(start, i, this->seq[i]));
        break;
      case BELOW:
        probs.push_back(this->predictBelow(start, i, this->seq[i]));
        break;
    }
  }
  return probs;
}

void HPYPModel::buildTree(l_type stop) {
  this->insertRoot(this->seq[0]);
  for (l_type i=1; i < stop; ++i) {
    this->insertContextAndObservation(0, i, this->seq[i]);
  }
}

void HPYPModel::updateTree(l_type start, l_type stop) {
  for (l_type i = start; i < stop; ++i) {
    this->insertContextAndObservation(0, i, this->seq[i]);
  }
}

double HPYPModel::predict(l_type start, l_type stop, e_type obs) {
  WrappedNodeList path = this->contextTree.findLongestSuffix(start,stop);
  d_vec discount_path = this->parameters.getDiscounts(path);
  d_vec concentration_path = this->parameters.getConcentrations(path,
                                                                discount_path);

  d_vec prob_path = this->computeProbabilityPath(path,
                                                 discount_path,
                                                 concentration_path,
                                                 obs);
  return prob_path.back();
}

/**
 * Predict prob; in the case of required fragmentation, predict form
 * _below_ the split point!
 */
double HPYPModel::predictBelow(l_type start, l_type stop, e_type obs) {
  WrappedNodeList path =
      this->contextTree.findLongestSuffixVirtual(start,stop).second;
  d_vec discount_path = this->parameters.getDiscounts(path);
  d_vec concentration_path = this->parameters.getConcentrations(path,
                                                                discount_path);

  d_vec prob_path = this->computeProbabilityPath(path,
                                                 discount_path,
                                                 concentration_path,
                                                 obs);
  return prob_path.back();
}

double HPYPModel::predictWithFragmentation(l_type start,
                                           l_type stop,
                                           e_type obs) {
  std::pair<int, WrappedNodeList> path = contextTree.findLongestSuffixVirtual(
      start, stop);

  d_vec discountPath = parameters.getDiscounts(path.second);
  d_vec concentrationPath = parameters.getConcentrations(path.second,
                                                         discountPath);
  d_vec probabilityPath = this->computeProbabilityPath(path.second,
                                                       discountPath,
                                                       concentrationPath,
                                                       obs);

  double probability = 0;
  if (path.first != 0) {
    // create a new payload for the node we are predicting from
    // fragmentation -- last probability on the path needs to be recomputed
    // by creating a new, split node of length path.second.
    void* splitNode = this->restaurant.getFactory().make();
    WrappedNodeList::iterator it = path.second.end();
    it--; it--; // one before last; parent of node we need to split
    int parentLength = it->end - it->start;
    // path.first is length of parent after split
    double discountAfter = parameters.getDiscount(path.first, it->end - it->start);
    it++; // last node
    double discountFragmented = this->parameters.getDiscount(parentLength,
                                                             path.first);
    this->restaurant.updateAfterSplit(
        it->payload,
        splitNode,
        discountPath.back(),
        discountFragmented,
        true); // update splitNode only
    double concentrationFragmented = this->parameters.getConcentration(
        discountFragmented, parentLength, path.first);
    probability = this->restaurant.computeProbability(
        splitNode, obs, probabilityPath[probabilityPath.size()-2],
        discountFragmented, concentrationFragmented);
    this->restaurant.getFactory().recycle(splitNode);
  } else {
    probability = probabilityPath.back();
  }
  return probability;
}

d_vec HPYPModel::predictiveDistribution(l_type start, l_type stop) {
  d_vec predictive;
  predictive.reserve(this->numTypes);
  WrappedNodeList path = this->contextTree.findLongestSuffix(start,stop);
  d_vec discount_path = this->parameters.getDiscounts(path);
  d_vec concentration_path = this->parameters.getConcentrations(path,
                                                                discount_path);

  for(int i = 0; i < this->numTypes; ++i) {
    d_vec prob_path = this->computeProbabilityPath(path,
                                                   discount_path,
                                                   concentration_path,
                                                   i);
    predictive.push_back(prob_path.back());
  }

  return predictive;
}

d_vec HPYPModel::predictiveDistributionWithMixing(l_type start,
                                                  l_type stop,
                                                  d_vec& mixingWeights) {
  d_vec predictive;
  predictive.reserve(this->numTypes);
  const WrappedNodeList path = this->contextTree.findLongestSuffix(start, stop);
  const d_vec discount_path = this->parameters.getDiscounts(path);
  const d_vec concentration_path
      = this->parameters.getConcentrations(path, discount_path);

  for(int i = 0; i < this->numTypes; ++i) {
    d_vec prob_path = this->computeProbabilityPath(path,
                                                   discount_path,
                                                   concentration_path,
                                                   i);
    double prob = 0;
    double sum = 0;
    for (int j = 0;
         j < std::min<int>(mixingWeights.size(), prob_path.size());
         ++j) {
      prob += mixingWeights[j]*prob_path[j];
      sum += mixingWeights[j];
    }
    predictive.push_back(prob + (1 - sum)*prob_path.back());
  }

  return predictive;
}

/**
 * Given a path from the root to some node (not a leaf),
 * perform add/remove Gibbs sampling of the last node by repeatedly
 * removing and adding customers, cus times for each type s.
 */
void HPYPModel::addRemoveSamplePath(
    const WrappedNodeList& path,
    const d_vec& discountPath,
    const d_vec& concentrationPath,
    const HPYPModel::PayloadDataPath& payloadDataPath,
    double baseProb) {
  assert(path.size() > 0);
  assert(path.size() == discountPath.size());
  assert(path.size() == concentrationPath.size());

  bool useAdditionalData = payloadDataPath.size() == path.size();
  void* main = path.back().payload;
  const IAddRemoveRestaurant& r = this->restaurant; // shortcut

  IHPYPBaseRestaurant::TypeVector types = r.getTypeVector(main);
  for(IHPYPBaseRestaurant::TypeVectorIterator it = types.begin();
      it != types.end(); ++it) { // for each type of customer

    e_type type = *it;
    l_type cw = r.getC(main, type);

    if (cw == 1) {
      continue; // no point in reseating in a 1 customer restaurant
    }

    d_vec probabilityPath = this->computeProbabilityPath(path,
                                                         discountPath,
                                                         concentrationPath,
                                                         type);
    WrappedNodeList::const_iterator current;
    for (l_type i = 0; i < cw; ++i) { // for each customer of this type
      current = --path.end(); // set current to last restaurant in path

      // index into d/alpha vectors for current restaurant
      int j = discountPath.size() - 1;

      bool goUp = true;
      while(goUp && j != -1) {
        void* additionalData = NULL;
        if (useAdditionalData) {
          additionalData = payloadDataPath[j].get();
        }
        bool removed = r.removeCustomer(current->payload,
                                        type,
                                        discountPath[j],
                                        additionalData);
        if (removed) {
          --current;
          --j;
        } else {
          goUp = false;
        }
      }

      // recompute probabilities back down; need not recompute last probability
      while(j < (int)probabilityPath.size() - 1) {
        if (j == -1) {
          // can't recompute base distribution probabilities at prob_path[0]
          j = 0; //
          ++current;
        }
        probabilityPath[j+1] = r.computeProbability(current->payload,
                                                    type,
                                                    probabilityPath[j],
                                                    discountPath[j],
                                                    concentrationPath[j]);
        ++j;
        ++current;
      }

      // set current and j to the last restaurant on path
      current = --path.end();
      j = discountPath.size() - 1;
      goUp = true;
      while(goUp && j != -1) {
        void* additionalData = NULL;
        if (useAdditionalData) {
          additionalData = payloadDataPath[j].get();
        }
        bool inserted = r.addCustomer(current->payload,
                                      type,
                                      probabilityPath[j],
                                      discountPath[j],
                                      concentrationPath[j],
                                      additionalData);
        if (inserted) {
          current--;
          j--;
        } else {
          goUp = false;
        }
      }

    }

  }

}

/**
 * Given a path from the root to some node (not a leaf),
 * perform add/remove Gibbs sampling of the last node by repeatedly
 * removing and adding customers, cus times for each type s.
 */
void HPYPModel::directGibbsSamplePath(
    const WrappedNodeList& path,
    const d_vec& discountPath,
    const d_vec& concentrationPath,
    const HPYPModel::PayloadDataPath& payloadDataPath,
    double baseProb) {
  assert(path.size() > 0);
  assert(path.size() == discountPath.size());
  assert(path.size() == concentrationPath.size());

  bool useAdditionalData = payloadDataPath.size() == path.size();
  // XXX: HACK! We assume this is the type of addData for the used restaurant
  stirling_generator_full_log *stirlingGenCurrent = NULL;
  stirling_generator_full_log *stirlingGenParent = NULL;
  void* main = path.back().payload;
  const BaseCompactRestaurant& r = (BaseCompactRestaurant&)this->restaurant; // shortcut

  IHPYPBaseRestaurant::TypeVector types = r.getTypeVector(main);
  for(IHPYPBaseRestaurant::TypeVectorIterator it = types.begin();
      it != types.end(); ++it) { // for each type of customer

    e_type type = *it;
    l_type cw = r.getC(main, type);

    if (cw == 1) {
      continue; // no point in reseating in a 1 customer restaurant
    }

    WrappedNodeList::const_iterator current;
    current = --path.end(); // set current to last restaurant in path

    // index into d/alpha vectors for current restaurant
    int j = discountPath.size() - 1;

    bool goUp = true;
    while(goUp && j != -1) {
      goUp = false;
      stirlingGenCurrent = (stirling_generator_full_log*)payloadDataPath[j].get();
      void* currentPayload = (*current).payload;
      void* parentPayload = NULL; // initialized below
      int currentCw = r.getC(currentPayload, type);
      int currentTw = r.getT(currentPayload, type);
      int otherT = r.getT(currentPayload) - currentTw;
      std::vector<double> logProbs(currentCw, 0);
      std::vector<double> logProbs1(currentCw, 0);
      std::vector<double> logProbs2(currentCw, 0);
      std::vector<double> logProbs3(currentCw, 0);
      std::vector<double> logProbs4(currentCw, 0);
      if (j > 0) { // not at the top, so we have a CRP parent
        stirlingGenParent = (stirling_generator_full_log*)payloadDataPath[j-1].get();
        current--; // move to parent
        parentPayload = (*current).payload;
        current++;

        int parentTw = r.getT(parentPayload, type);
        int parentCw = r.getC(parentPayload, type);
        int parentOtherC = r.getC(parentPayload) - currentTw;
        for (int tw = 1; tw <= currentCw; ++tw) {
          int newParentCw = parentCw - currentTw + tw;
          if (newParentCw < parentTw) {
            logProbs4[tw-1] = -INFINITY;
          } else {
            logProbs1[tw-1] = logKramp(concentrationPath[j] + discountPath[j], discountPath[j], otherT + tw - 1);
            logProbs2[tw-1] = - logKramp(concentrationPath[j-1] + 1, 1, parentOtherC + tw - 1);
            logProbs3[tw-1] = stirlingGenCurrent->getLog(currentCw, tw);
            logProbs4[tw-1] = stirlingGenParent->getLog(newParentCw, parentTw);
            //std::cerr <<  logKramp(concentrationPath[j] + discountPath[j], discountPath[j], otherT + tw - 1) << std::endl;
            //std::cerr <<  logKramp(concentrationPath[j-1], 1, parentOtherC + tw - 1)<< std::endl;
            //std::cerr <<  stirlingGenCurrent->getLog(cw, tw)<< std::endl;
            //std::cerr << cw << ", " << tw << std::endl;
            //std::cerr <<  stirlingGenParent->getLog(newParentCw, parentTw)<< std::endl;
            //std::cerr << newParentCw << ", " << parentTw << std::endl;
          }
        }
        //std::cerr << parentCw << ", " << parentTw << ", " << parentOtherC << ", " << currentTw << ", " << otherT << std::endl;
      } else { // at the root, take base prob into account
        for (int tw = 1; tw <= currentCw; ++tw) {
          logProbs1[tw-1] = logKramp(concentrationPath[j] + discountPath[j], discountPath[j], otherT + tw - 1);
          logProbs2[tw-1] = stirlingGenCurrent->getLog(currentCw, tw);
          logProbs3[tw-1] = tw * log(baseProb);
        }
      }
      // subtract max for stability
      //std::cerr << iterableToString(logProbs1) << std::endl;
      //std::cerr << iterableToString(logProbs2) << std::endl;
      //std::cerr << iterableToString(logProbs3) << std::endl;
      //std::cerr << iterableToString(logProbs4) << std::endl;
      subMax_vec(logProbs1);
      subMax_vec(logProbs2);
      subMax_vec(logProbs3);
      subMax_vec(logProbs4);
      add_vec(logProbs, logProbs1);
      add_vec(logProbs, logProbs2);
      add_vec(logProbs, logProbs3);
      add_vec(logProbs, logProbs4);
      subMax_vec(logProbs);

      //std::cerr << iterableToString(logProbs1) << std::endl;
      //std::cerr << iterableToString(logProbs2) << std::endl;
      //std::cerr << iterableToString(logProbs3) << std::endl;
      //std::cerr << iterableToString(logProbs4) << std::endl;
      //std::cerr << iterableToString(logProbs) << std::endl;
      exp_vec(logProbs);
      //std::cerr << iterableToString(logProbs) << std::endl;
      // if (max == -INFINITY) { // if all choices are improbable, choose uniformly
      //     // we can do better by normalizing the component individually
      //   for (int ii = 0; ii < logProbs.size(); ++ii)
      //     logProbs[ii] = 1.0;
      // }
      //std::cerr << iterableToString(logProbs) << std::endl;
      int sampledTw = sample_unnormalized_pdf(logProbs, 0) + 1;
      //std::cerr << "Old tw: " << currentTw << ", newTw: " << sampledTw << std::endl;

      r.setT(currentPayload, type, sampledTw);
      if (j > 0) {
        int newCw =  r.getC(parentPayload, type) - currentTw + sampledTw;
        assert(newCw >= r.getT(parentPayload, type));
        r.setC(parentPayload, type, newCw);
      }

      if (sampledTw != currentTw) {
        --current;
        --j;
      } else {
        goUp = false;
      }
    }

  }

}

boost::shared_ptr<void> HPYPModel::makeAdditionalDataPtr(void* payload,
                                                         double discount,
                                                         double concentration) const {
  return boost::shared_ptr<void>(
      this->restaurant.createAdditionalData(payload,
        discount,
        concentration),
      boost::bind(&IAddRemoveRestaurant::freeAdditionalData,
        &(this->restaurant),
        _1));
}

void HPYPModel::runGibbsSampler(bool directGibbs) {
  ContextTree::DFSPathIterator pathIterator = contextTree.getDFSPathIterator();
  d_vec discountPath = parameters.getDiscounts(*pathIterator);
  d_vec concentrationPath = parameters.getConcentrations(*pathIterator,
                                                         discountPath);

  // initialize payloadDataPath; by using shared_ptr with the proper
  // destruction function, all clean-up should be automatic.
  HPYPModel::PayloadDataPath payloadDataPath;
  int j = 0;
  for (WrappedNodeList::const_iterator it = (*pathIterator).begin();
       it != (*pathIterator).end(); ++it) {
      payloadDataPath.push_back(this->makeAdditionalDataPtr(
            it->payload, discountPath[j], concentrationPath[j]));
    j++;
  }

  if (directGibbs) {
    this->directGibbsSamplePath(*pathIterator, discountPath, concentrationPath,
                                payloadDataPath, baseProb);
  } else {
    this->addRemoveSamplePath(*pathIterator, discountPath, concentrationPath,
                              payloadDataPath, baseProb);
  }

  size_t pathLength = (*pathIterator).size();

  while(pathIterator.hasMore()) { // loop over all paths in the tree
    ++pathIterator;
    if ((*pathIterator).size() == 0) {
      break;
    }

    if ((*pathIterator).size() == pathLength) {
      // sibling
      discountPath.pop_back();
      parameters.extendDiscounts(*pathIterator, discountPath);
      concentrationPath.pop_back();
      parameters.extendConcentrations(*pathIterator,
                                      discountPath,
                                      concentrationPath);
      payloadDataPath.pop_back();
      payloadDataPath.push_back(
          this->makeAdditionalDataPtr((*pathIterator).back().payload,
                                      discountPath.back(),
                                      concentrationPath.back()));

    } else {
      if ((*pathIterator).size() == pathLength - 1) {
        // we went up -- just drop the last term
        discountPath.pop_back();
        concentrationPath.pop_back();
        payloadDataPath.pop_back();
      } else {
        // we went up one and then down some number of levels -- recompute
        discountPath.pop_back();
        concentrationPath.pop_back();
        parameters.extendDiscounts(*pathIterator, discountPath);
        parameters.extendConcentrations(*pathIterator,
                                        discountPath,
                                        concentrationPath);
        payloadDataPath.pop_back();
        WrappedNodeList::const_iterator it = (*pathIterator).begin();
        // move it to the first item not covered by the payloadDataPath
        for (size_t i = 0; i < payloadDataPath.size(); ++i) {
          ++it;
        }
        for (size_t i = payloadDataPath.size(); i < discountPath.size(); ++i) {
          payloadDataPath.push_back(
              this->makeAdditionalDataPtr(it->payload,
                                          discountPath[i],
                                          concentrationPath[i]));

          ++it;
        }
        assert(it == (*pathIterator).end());
      }
    }
    pathLength = (*pathIterator).size();

    tracer << (*pathIterator).size() << " " << discountPath.size() << " "
           << concentrationPath.size() << " " <<  payloadDataPath.size()
           << std::endl;

    if (directGibbs) {
      this->directGibbsSamplePath(*pathIterator, discountPath, concentrationPath,
                                  payloadDataPath, baseProb);
    } else {
      this->addRemoveSamplePath(*pathIterator, discountPath, concentrationPath,
                                payloadDataPath, baseProb);
    }
  }
}

d_vec HPYPModel::computeProbabilityPath(const WrappedNodeList& path,
                                        const d_vec& discount_path,
                                        const d_vec& concentration_path,
                                        e_type obs) {
  d_vec out;
  out.reserve(path.size() + 1);

  double prob = this->baseProb; // base distribution
  out.push_back(prob);

  int j = 0;
  for(WrappedNodeList::const_iterator it = path.begin();
      it!= path.end();
      ++it) {
    prob = this->restaurant.computeProbability(it->payload,
                                               obs,
                                               prob,
                                               discount_path[j],
                                               concentration_path[j]);
    out.push_back(prob);
    j++;
  }
  return out;
}

void HPYPModel::updatePath(const WrappedNodeList& path,
                           const d_vec& prob_path,
                           const d_vec& discount_path,
                           const d_vec& concentration_path,
                           e_type obs) {

  unsigned int j=path.size()-1;
  double newTable = 1;
  for(WrappedNodeList::const_reverse_iterator it = path.rbegin();
      it != path.rend();
      ++it) {
    newTable = this->restaurant.addCustomer(it->payload,
                                            obs,
                                            prob_path[j],
                                            discount_path[j],
                                            concentration_path[j],
                                            NULL,
                                            newTable);
    if (newTable==0) {
      break;
    }
    j--;
  }
}

void HPYPModel::removeObservationFromPath(
    const WrappedNodeList& path,
    const d_vec& discountPath,
    e_type obs,
    const HPYPModel::PayloadDataPath& payloadDataPath) {
  int j = path.size()-1;

  double frac_t = 1;

  for(WrappedNodeList::const_reverse_iterator it = path.rbegin();
    it != path.rend(); it++) {

    void* payloadData = NULL;
    if (payloadDataPath.size() == path.size()) {
      payloadData = payloadDataPath[j].get();
    }

    frac_t = this->restaurant.removeCustomer(it->payload,
                                             obs,
                                             discountPath[j],
                                             payloadData, frac_t);
    if (frac_t == 0.)
      break;
    j--;
  }
}

void HPYPModel::handleSplit(const WrappedNode& nodeA,
                            const WrappedNode& nodeB,
                            const WrappedNode& nodeC) {
    int lengthA = nodeA.end - nodeA.start;
    int lengthB = nodeB.end - nodeB.start;
    int lengthC = nodeC.end - nodeC.start;

    // parent context should be shorter than both its children
    assert(lengthA < lengthB && lengthA < lengthC);
    // length of node that required splitting should be longer than the result
    assert(lengthC < lengthB);

    double discBBeforeSplit = this->parameters.getDiscount(lengthA, lengthB);
    double discBAfterSplit  = this->parameters.getDiscount(lengthC, lengthB);
    this->restaurant.updateAfterSplit(nodeB.payload,
                                      nodeC.payload,
                                      discBBeforeSplit,
                                      discBAfterSplit);
}

bool HPYPModel::checkConsistency(const WrappedNode& node,
                      const std::list<WrappedNode>& children) const {
  bool consistent = this->restaurant.checkConsistency(node.payload);
  std::map<e_type, int> table_counts;
  for(std::list<WrappedNode>::const_iterator it = children.begin();
      it != children.end(); ++it) {
    IHPYPBaseRestaurant::TypeVector keys =
        this->restaurant.getTypeVector(it->payload);
    for(IHPYPBaseRestaurant::TypeVectorIterator key_it = keys.begin();
        key_it != keys.end(); ++key_it) {
      table_counts[*key_it] += this->restaurant.getT(it->payload, *key_it);
    }
  }

  for(std::map<e_type, int>::iterator it = table_counts.begin();
      it != table_counts.end(); ++it) {
    consistent = (this->restaurant.getC(node.payload, (*it).first)
                  >= (*it).second) && consistent;
    if (!consistent) {
      std::cerr << "Child table sum is: " << (*it).second
                << ", parent customers is: "
                << this->restaurant.getC(node.payload, (*it).first)
                << std::endl;
    }
  }
  return consistent;
}

void HPYPModel::prunePath(WrappedNodeList& path) {

}

bool HPYPModel::checkConsistency() const {
  CheckConsistencyVisitor v(*this);
  this->contextTree.visitDFSWithChildren(v);
  return v.consistent;
}

double HPYPModel::computeLogRestaurantProb(
    const WrappedNodeList& path,
    const d_vec& discountPath,
    const d_vec& concentrationPath,
    const HPYPModel::PayloadDataPath& payloadDataPath,
    double baseProb) const {
  assert(path.size() > 0);
  assert(path.size() == discountPath.size());
  assert(path.size() == concentrationPath.size());

  void* payload = path.back().payload;
  const BaseCompactRestaurant& r = (BaseCompactRestaurant&)this->restaurant; // shortcut
  double logProb = 0;
  int j = discountPath.size() - 1;
  l_type c = r.getC(payload);
  if (c == 1) {
      // deterministic restaurant
      return 0;
  }
  l_type t = r.getT(payload);
  //std::cerr << "c: " << c << ", t: " << t << std::endl;
  //std::cerr << "a: " << concentrationPath[j] << ", d: " << discountPath[j] << std::endl;
  logProb += logKramp(concentrationPath[j] + discountPath[j], discountPath[j], t - 1);
  //std::cerr << logProb << std::endl;
  logProb -= logKramp(concentrationPath[j] + 1, 1, c - 1);
  //std::cerr << logProb << std::endl;

  stirling_generator_full_log *stirlingGen = (stirling_generator_full_log*)payloadDataPath[j].get();
  IHPYPBaseRestaurant::TypeVector types = r.getTypeVector(payload);
  for(IHPYPBaseRestaurant::TypeVectorIterator it = types.begin();
      it != types.end(); ++it) { // for each type of customer

    e_type type = *it;
    l_type cw = r.getC(payload, type);
    l_type tw = r.getT(payload, type);

    logProb += stirlingGen->getLog(cw, tw);

    if (j == 0) { // at the root, take base prob into account
      logProb += tw * log(baseProb);
    }
  }
  //std::cerr << logProb << std::endl;
  return logProb;
}

double HPYPModel::computeLogJoint() const {
  double logJoint = 0;
  ContextTree::DFSPathIterator pathIterator = contextTree.getDFSPathIterator();
  d_vec discountPath = parameters.getDiscounts(*pathIterator);
  d_vec concentrationPath = parameters.getConcentrations(*pathIterator,
                                                         discountPath);

  // initialize payloadDataPath; by using shared_ptr with the proper
  // destruction function, all clean-up should be automatic.
  HPYPModel::PayloadDataPath payloadDataPath;
  int j = 0;
  for (WrappedNodeList::const_iterator it = (*pathIterator).begin();
       it != (*pathIterator).end(); ++it) {
      payloadDataPath.push_back(this->makeAdditionalDataPtr(
            it->payload, discountPath[j], concentrationPath[j]));
    j++;
  }

  logJoint += computeLogRestaurantProb(*pathIterator, discountPath, concentrationPath, payloadDataPath, baseProb);

  size_t pathLength = (*pathIterator).size();

  while(pathIterator.hasMore()) { // loop over all paths in the tree
    ++pathIterator;
    if ((*pathIterator).size() == 0) {
      break;
    }

    if ((*pathIterator).size() == pathLength) {
      // sibling
      discountPath.pop_back();
      parameters.extendDiscounts(*pathIterator, discountPath);
      concentrationPath.pop_back();
      parameters.extendConcentrations(*pathIterator,
                                      discountPath,
                                      concentrationPath);
      payloadDataPath.pop_back();
      payloadDataPath.push_back(
          this->makeAdditionalDataPtr((*pathIterator).back().payload,
                                      discountPath.back(),
                                      concentrationPath.back()));

    } else {
      if ((*pathIterator).size() == pathLength - 1) {
        // we went up -- just drop the last term
        discountPath.pop_back();
        concentrationPath.pop_back();
        payloadDataPath.pop_back();
      } else {
        // we went up one and then down some number of levels -- recompute
        discountPath.pop_back();
        concentrationPath.pop_back();
        parameters.extendDiscounts(*pathIterator, discountPath);
        parameters.extendConcentrations(*pathIterator,
                                        discountPath,
                                        concentrationPath);
        payloadDataPath.pop_back();
        WrappedNodeList::const_iterator it = (*pathIterator).begin();
        // move it to the first item not covered by the payloadDataPath
        for (size_t i = 0; i < payloadDataPath.size(); ++i) {
          ++it;
        }
        for (size_t i = payloadDataPath.size(); i < discountPath.size(); ++i) {
          payloadDataPath.push_back(
              this->makeAdditionalDataPtr(it->payload,
                                          discountPath[i],
                                          concentrationPath[i]));

          ++it;
        }
        assert(it == (*pathIterator).end());
      }
    }
    pathLength = (*pathIterator).size();

    logJoint += computeLogRestaurantProb(*pathIterator, discountPath, concentrationPath, payloadDataPath, baseProb);

  }
  return logJoint;
}

HPYPModel::ToStringVisitor::ToStringVisitor(seq_type& seq,
    const IHPYPBaseRestaurant& restaurant)
    : outstream(), seq(seq), restaurant(restaurant) {}

void HPYPModel::ToStringVisitor::operator()(const WrappedNode& n) {
  for (l_type i = 0; i < n.depth; ++i) {
    this->outstream << " ";
  }
  this->outstream << SubSeq::toString(n.start, n.end, seq);
  this->outstream << " " << this->restaurant.toString(n.payload);
  this->outstream << std::endl;
}

HPYPModel::CheckConsistencyVisitor::CheckConsistencyVisitor(
    const HPYPModel& model) : consistent(true), model(model) {}

void HPYPModel::CheckConsistencyVisitor::operator()(
    WrappedNode& n, std::list<WrappedNode>& children) {
  bool nodeConsistent = this->model.checkConsistency(n, children);
  if (!nodeConsistent) {
    std::cerr << "Node " << n.toString() << " not consistent!" << std::endl;
  }
  consistent = consistent && nodeConsistent;
}

HPYPModel::LogJointVisitor::LogJointVisitor(
    const HPYPModel& model) : logJoint(0), model(model) {}

void HPYPModel::LogJointVisitor::operator()(
    // FIXME
    WrappedNode& n, std::list<WrappedNode>& children) {
    int c = model.restaurant.getC(n.payload);
    int t = model.restaurant.getT(n.payload);
    IHPYPBaseRestaurant::TypeVector keys =
        model.restaurant.getTypeVector(n.payload);
    for(IHPYPBaseRestaurant::TypeVectorIterator key_it = keys.begin();
        key_it != keys.end(); ++key_it) {
      int cw = model.restaurant.getC(n.payload, *key_it);
      int tw = model.restaurant.getT(n.payload, *key_it);
    }
}

std::string HPYPModel::toString() {
  HPYPModel::ToStringVisitor visitor(this->seq, this->restaurant);
  this->contextTree.visitDFS(visitor);
  return visitor.outstream.str();
}

}} // namespace gatsby::libplump


/// Causal Dynamical Triangulations in C++ using CGAL
///
/// Copyright © 2016-2017 Adam Getchell
///
/// Data structures for operations on simplicial manifolds
///
/// \done Classify cells as (3,1), (2,2), or (1,3) based on their foliation.
/// A tuple of vectors contain cell handles to the simplices of type (3,1),
/// (2,2), and (1,3) respectively.
/// \done Classify edges as timelike or spacelike so that action can be
/// calculated.
/// \done SimplicialManifold data structure holding a std::unique_ptr to
/// the Delaunay triangulation and a std::tuple of geometry information.
/// \done Move constructor recalculates geometry.
///
/// @file  SimplicialManifold.hpp
/// @brief Data structures for simplicial manifolds
/// @author Adam Getchell
/// @todo Deprecated in favor of Manifold.hpp

#ifndef INCLUDE_SIMPLICIALMANIFOLD_HPP_
#define INCLUDE_SIMPLICIALMANIFOLD_HPP_

#include <S3Triangulation.hpp>
#include <boost/optional.hpp>
#include <map>
#include <memory>
#include <set>
#include <utility>
#include <vector>

using Facet = Delaunay3::Facet;

/// @brief A tuple of the geometric values of the Simplicial Manifold
///
/// The first element is the vector of (3,1) simplices
/// The second element is the vector of (2,2) simplices
/// The third element is the vector of (1,3) simplices
/// The fourth element is the vector of timelike edges
/// The fifth element is the vector of spacelike edges
/// The sixth element is the vector of vertices
///
/// Useful for constructing GeometryInfo, which contains this and other
/// information, and in comparing the results of moves, which will change one or
/// more elements of the Geometry_tuple.
using Geometry_tuple =
    std::tuple<std::vector<Cell_handle>, std::vector<Cell_handle>,
               std::vector<Cell_handle>, std::vector<Edge_handle>,
               std::vector<Edge_handle>, std::vector<Vertex_handle>>;

// Non-member non-friend functions

/// @brief Classifies edges
///
/// This function iterates over all edges in the triangulation
/// and classifies them as timelike or spacelike.
/// Timelike edges are stored in the **timelike_edges** vector as an Edge_handle
/// (tuple of Cell_handle, std::size_t, std::size_t) for later use by
/// ergodic moves on timelike edges. Spacelike edges are also stored as a
/// vector of Edge_handle **spacelike_edges**, for use by (4,4) moves as
/// well as the distance-finding algorithms.
/// @param[in] universe_ptr A std::unique_ptr<Delaunay> to the triangulation
/// @returns A std::pair<std::vector<Edge_handle>, std::vector<Edge_handle>> of
/// timelike edges and spacelike edges
template <typename T>
[[deprecated]] auto classify_edges(T&& universe_ptr)
{
#ifndef NDEBUG
  std::cout << "Classifying edges....\n";
#endif
  Delaunay3::Finite_edges_iterator eit;
  std::vector<Edge_handle>         timelike_edges;
  std::vector<Edge_handle>         spacelike_edges;

  // Iterate over all edges in the Delaunay triangulation
  for (eit = universe_ptr->finite_edges_begin();
       eit != universe_ptr->finite_edges_end(); ++eit)
  {
    Cell_handle ch = eit->first;
    // Get timevalues of vertices at the edge ends
    auto time1 = ch->vertex(eit->second)->info();
    auto time2 = ch->vertex(eit->third)->info();

    // Make Edge_handle
    Edge_handle thisEdge{
        ch, static_cast<std::size_t>(ch->index(ch->vertex(eit->second))),
        static_cast<std::size_t>(ch->index(ch->vertex(eit->third)))};

    if (time1 != time2)
    {  // We have a timelike edge
      timelike_edges.emplace_back(thisEdge);

#ifdef DETAILED_DEBUGGING
      std::cout << "First vertex of edge is " << std::get<1>(thisEdge)
                << " and second vertex of edge is " << std::get<2>(thisEdge)
                << std::endl;
#endif
    }
    else
    {  // We have a spacelike edge
      spacelike_edges.emplace_back(thisEdge);
    }  // endif
  }    // Finish iterating over edges

// Display results if debugging
#ifndef NDEBUG
  std::cout << "There are " << timelike_edges.size() << " timelike edges and "
            << spacelike_edges.size() << " spacelike edges.\n";
#endif
  return std::make_pair(timelike_edges, spacelike_edges);
}  // classify_edges()

/// @brief Classify simplices as (3,1), (2,2), or (1,3)
///
/// This function iterates over all cells in the triangulation
/// and classifies them as:
/// \f{eqnarray*}{
///   31 &=& (3, 1) \\
///   22 &=& (2, 2) \\
///   13 &=& (1, 3)
/// \f}
/// The vectors **three_one**, **two_two**, and **one_three** contain
/// Cell_handles to all the simplices in the triangulation of that corresponding
/// type.
///
/// @param[in] universe_ptr A std::unique_ptr<Delaunay> to the triangulation
/// @returns A std::tuple<std::vector, std::vector, std::vector> of
/// **three_one**, **two_two**, and **one_three**
template <typename T>
[[deprecated]] auto classify_simplices(T&& universe_ptr)
{
#ifndef NDEBUG
  std::cout << "Classifying simplices....\n";
#endif
  Delaunay3::Finite_cells_iterator cit;
  std::vector<Cell_handle>         three_one;
  std::vector<Cell_handle>         two_two;
  std::vector<Cell_handle>         one_three;

  // Iterate over all cells in the Delaunay triangulation
  for (cit = universe_ptr->finite_cells_begin();
       cit != universe_ptr->finite_cells_end(); ++cit)
  {
    int max_values{0};
    int min_values{0};
    // Push every time value of every vertex into a list
    int timevalues[4] = {
        cit->vertex(0)->info(),
        cit->vertex(1)->info(),
        cit->vertex(2)->info(),
        cit->vertex(3)->info(),
    };
    int max_time =
        *std::max_element(std::begin(timevalues), std::end(timevalues));
    for (auto elt : timevalues)
    {
      if (elt == max_time) { ++max_values; }
      else
      {
        ++min_values;
      }
    }

    // Classify simplex using max_values and write to cit->info()
    if (min_values == 1 && max_values == 3)
    {
      cit->info() = 13;
      one_three.emplace_back(cit);
    }
    else if (min_values == 2 && max_values == 2)
    {
      cit->info() = 22;
      two_two.emplace_back(cit);
    }
    else if (min_values == 3 && max_values == 1)
    {
      cit->info() = 31;
      three_one.emplace_back(cit);
    }
    else
    {
      throw std::runtime_error("Invalid simplex in classify_simplices()!");
    }  // endif
  }    // Finish iterating over cells

// Display results if debugging
#ifndef NDEBUG
  std::cout << "There are " << three_one.size() << " (3,1) simplices and "
            << two_two.size() << " (2,2) simplices\n";
  std::cout << "and " << one_three.size() << " (1,3) simplices.\n";
#endif
  return std::make_tuple(three_one, two_two, one_three);
}  // classify_simplices()

template <typename T>
[[deprecated]] auto classify_all_simplices(T&& universe_ptr)
{
#ifndef NDEBUG
  std::cout << "Classifying all simplices....\n";
#endif

  auto                       cells = classify_simplices(universe_ptr);
  auto                       edges = classify_edges(universe_ptr);
  std::vector<Vertex_handle> vertices;
  for (auto vit = universe_ptr->finite_vertices_begin();
       vit != universe_ptr->finite_vertices_end(); ++vit)
  { vertices.emplace_back(vit); }
  return std::make_tuple(std::get<0>(cells), std::get<1>(cells),
                         std::get<2>(cells), edges.first, edges.second,
                         vertices);
}

/// @struct
/// @brief A struct containing detailed geometry information
///
/// GeometryInfo contains information about the geometry of
/// a triangulation. In addition, it defines convenient functions to
/// retrieve commonly used values. This is to save the expense of
/// calculating manually from the triangulation. GeometryInfo() is
/// recalculated using the move assignment operator anytime a
/// SimplicialManifold() is move constructed.
/// The default constructor, destructor, move constructor, copy
/// constructor, and copy assignment operator are explicitly defaulted.
/// See http://en.cppreference.com/w/cpp/language/rule_of_three
struct [[deprecated]] GeometryInfo
{
 private:
  /// @brief (3,1) cells in the foliation
  std::vector<Cell_handle> three_one;

  /// @brief (2,2) cells in the foliation
  std::vector<Cell_handle> two_two;

  /// @brief (1,3) cells in the foliation
  std::vector<Cell_handle> one_three;

  /// @brief Edges spanning two adjacent time slices in the foliation
  std::vector<Edge_handle> timelike_edges;

  /// @brief Non-spanning edges in the foliation
  std::vector<Edge_handle> spacelike_edges;

  /// @brief Vertices of the foliation
  std::vector<Vertex_handle> vertices;

  /// @brief Spacelike facets for each timeslice
  boost::optional<std::multimap<std::size_t, Facet>> spacelike_facets;

  /// @brief Actual timevalues of simulation
  boost::optional<std::set<std::size_t>> timevalues;

 public:
  /// @brief Getter for spacelike facets
  /// @return The multimap of facets
  const boost::optional<std::multimap<std::size_t, Facet>>&
  getSpacelike_facets() const
  {
    return spacelike_facets;
  }

  /// @brief Setter for spacelike facets
  /// @param spacelike_facets The multimap of facets
  void setSpacelike_facets(
      const boost::optional<std::multimap<std::size_t, Facet>>&
          spacelike_facets)
  {
    GeometryInfo::spacelike_facets = spacelike_facets;
  }

  /// @brief Getter for timevalues
  /// @return The set of timevalues
  const boost::optional<std::set<std::size_t>>& getTimevalues() const
  {
    return timevalues;
  }

  /// @brief Setter for timevalues
  /// @param timevalues The set of timevalues
  void setTimevalues(const boost::optional<std::set<std::size_t>>& timevalues)
  {
    GeometryInfo::timevalues = timevalues;
  }

  /// @brief Default constructor
  GeometryInfo() = default;

  /// @brief Constructor from Geometry_tuple
  ///
  /// This is usually called as a result of classify_all_simplices(),
  /// which itself takes a std::unique_ptr<Delaunay>
  /// @param geometry Geometry_tuple initializing values
  /// @return A populated GeometryInfo{}
  explicit GeometryInfo(const Geometry_tuple&& geometry) noexcept
      : three_one{std::get<0>(geometry)}
      , two_two{std::get<1>(geometry)}
      , one_three{std::get<2>(geometry)}
      , timelike_edges{std::get<3>(geometry)}
      , spacelike_edges{std::get<4>(geometry)}
      , vertices{std::get<5>(geometry)}
  {}

  /// @brief Default destructor
  ~GeometryInfo() = default;

  /// @brief Default move constructor
  GeometryInfo(GeometryInfo&&) = default;

  /// @brief Default move assignment operator
  //  GeometryInfo& operator=(GeometryInfo&&) = default;
  GeometryInfo& operator=(Geometry_tuple&& other)
  {
#ifndef NDEBUG
    std::cout << "GeometryInfo move assignment operator." << std::endl;
#endif
    three_one       = std::move(std::get<0>(other));
    two_two         = std::move(std::get<1>(other));
    one_three       = std::move(std::get<2>(other));
    timelike_edges  = std::move(std::get<3>(other));
    spacelike_edges = std::move(std::get<4>(other));
    vertices        = std::move(std::get<5>(other));
    return *this;
  }

  /// @brief Default copy constructor
  GeometryInfo(const GeometryInfo&) = default;

  /// @brief Default copy assignment operator
  GeometryInfo& operator=(const GeometryInfo&) = default;

  /// @brief Timelike edges
  /// @return The number of edges spanning timeslices
  auto N1_TL() { return static_cast<std::size_t>(timelike_edges.size()); }

  /// @brief Spacelike edges
  /// @return The number of edges on same timeslice
  auto N1_SL() { return static_cast<std::size_t>(spacelike_edges.size()); }

  /// @brief (3,1) simplices
  /// @return The total number of simplices with 3 vertices on the t
  /// timeslice and 1 vertex on the t+1 timeslice
  auto N3_31() { return static_cast<std::size_t>(three_one.size()); }

  /// @brief (1,3) simplices
  /// @return The total number of simplices with 1 vertex on the t timeslice
  /// and 3 vertices on the t+1 timeslice
  auto N3_13() { return static_cast<std::size_t>(one_three.size()); }

  /// @brief (3,1) and (1,3) simplices
  /// @return The total number of simplices with 3 vertices on one
  /// timeslice and 1 vertex on the adjacent timeslice. Used to
  /// calculate the change in action.
  auto N3_31_13() { return N3_31() + N3_13(); }

  /// @brief (2,2) simplices
  /// @return The total number of simplices with 2 vertices on one
  /// timeslice and 2 vertices on the adjacent timeslice. Used to
  /// calculate the change in action.
  auto N3_22() { return static_cast<std::size_t>(two_two.size()); }

  /// @brief Number of cells
  ///
  /// This should be the equivalent of
  /// SimplicialManifold::triangulation->number_of_finite_cells(),
  /// and is used as a check to ensure that GeometryInfo{} matches.
  /// @return The number of cells in GeometryInfo{}
  auto number_of_cells() { return N3_31() + N3_22() + N3_13(); }

  /// @brief Number of edges
  ///
  /// This should be the equivalent of
  /// SimplicialManifold::triangulation->number_of_finite_edges(),
  /// and is used as a check to ensure that GeometryInfo{} matches.
  /// @return The number of edges in the triangulation
  auto number_of_edges() { return N1_TL() + N1_SL(); }

  //  auto max_timevalue() { return *timevalues.crbegin();}
  boost::optional<std::size_t> max_timevalue()
  {
    return timevalues ? *timevalues->crbegin() : 0;
  }

  boost::optional<std::size_t> min_timevalue()
  {
    return timevalues ? *timevalues->begin() : 0;
  }
  /// @brief Number of vertices
  /// @return The number of vertices in the triangulation
  auto N0() { return static_cast<std::size_t>(vertices.size()); }

  template <typename T1, typename T2>
  friend auto make_23_move(T1&& universe, T2&& attempted_moves)
      -> decltype(universe);

  template <typename T1, typename T2>
  friend auto make_32_move(T1&& universe, T2&& attempted_moves)
      -> decltype(universe);

  template <typename T1, typename T2>
  friend auto make_26_move(T1&& universe, T2&& attempted_moves)
      -> decltype(universe);

  template <typename T1, typename T2>
  friend auto make_62_move(T1&& universe, T2&& attempted_moves)
      -> decltype(universe);

  template <typename T1, typename T2>
  friend auto make_44_move(T1&& universe, T2&& attempted_moves)
      -> decltype(universe);

  template <typename T>
  friend auto VolumePerTimeslice(T&& manifold) -> decltype(manifold);
};

/// @struct
/// @brief A struct to hold triangulation and geometry information
///
/// SimplicialManifold contains information about the triangulation and
/// its geometry. In addition, it defines convenient constructors.
struct [[deprecated]] SimplicialManifold
{
  /// @brief Owning pointer to the Delaunay triangulation
  std::unique_ptr<Delaunay3> triangulation;

  /// @brief Owning pointer to GeometryInfo
  std::unique_ptr<GeometryInfo> geometry;

  /// @brief Default constructor
  SimplicialManifold()
      : triangulation{std::make_unique<Delaunay3>()}
      , geometry{std::make_unique<GeometryInfo>()}
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold default ctor.\n";
#endif
  }

  /// @brief Constructor with std::unique_ptr<Delaunay>
  ///
  /// Constructor taking a std::unique_ptr<Delaunay> which should be created
  /// using make_triangulation(). If you wish to default initialize a
  /// SimplicialManifold with no values, use the default
  /// constructor SimplicialManifold::SimplicialManifold() instead.
  /// Non-static data members are initialized in the order they are declared,
  /// (see http://open-std.org/JTC1/SC22/WG21/docs/papers/2016/n4594.pdf,
  ///  \f$\S\f$ 12.6.2.13.3), so **geometry** depending upon **triangulation**
  /// is fine.
  /// @param manifold A std::unique_ptr<Delaunay>
  /// @return A SimplicialManifold{}
  explicit SimplicialManifold(std::unique_ptr<Delaunay3>&& manifold)
      : triangulation{std::move(manifold)}
      , geometry{std::make_unique<GeometryInfo>(
            classify_all_simplices(triangulation))}
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold std::unique_ptr<Delaunay> ctor.\n";
#endif
  }

  /// @brief make_triangulation constructor
  ///
  /// Constructor that initializes **triangulation** by calling
  /// make_triangulation() and **geometry** by calling
  /// classify_all_simplices().
  /// @param simplices The number of desired simplices in the triangulation
  /// @param timeslices The number of timeslices in the triangulation
  /// @return A populated SimplicialManifold{}
  SimplicialManifold(std::size_t simplices, std::size_t timeslices)
      : triangulation{make_triangulation(simplices, timeslices)}
      , geometry{std::make_unique<GeometryInfo>(
            classify_all_simplices(triangulation))}
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold make_triangulation ctor.\n";
#endif
  }

  /// @brief Destructor
  ~SimplicialManifold()
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold dtor.\n";
#endif
    this->triangulation = nullptr;
    this->geometry      = nullptr;
  }

  /// @brief Move constructor
  /// @param other The SimplicialManifold to be move-constructed from
  /// @return A moved-to SimplicialManifold{}
  SimplicialManifold(SimplicialManifold&& other)
      : triangulation{std::move(other.triangulation)}
      , geometry{std::make_unique<GeometryInfo>(
            classify_all_simplices(triangulation))}
  //      , geometry{std::move(other.geometry)}
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold move ctor.\n";
#endif
  }
  //  SimplicialManifold(SimplicialManifold&&) = default;

  /// @brief Move assignment operator
  /// @param other The SimplicialManifold to be moved from
  /// @return A moved-assigned SimplicialManifold{}
  SimplicialManifold& operator=(SimplicialManifold&& other) noexcept
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold move assignment operator.\n";
#endif
    triangulation = std::move(other.triangulation);
    geometry      = std::make_unique<GeometryInfo>(
        classify_all_simplices(std::move(triangulation)));
    //      geometry = std::move(other.geometry);
    return *this;
  }
  //  SimplicialManifold& operator=(SimplicialManifold&&) = default;

  /// @brief SimplicialManifold copy constructor
  /// @param other The SimplicialManifold to copy
  /// @return A copied SimplicialManifold{}
  SimplicialManifold(const SimplicialManifold& other)
      : triangulation{std::make_unique<Delaunay3>(*(other.triangulation))}
      , geometry{std::make_unique<GeometryInfo>(*(other.geometry))}
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold copy ctor.\n";
#endif
  }

  /// @brief Copy assignment operator
  /// @return A copy-assigned SimplicialManifold{}
  //    SimplicialManifold& operator=(const SimplicialManifold&) = default;
  SimplicialManifold& operator=(const SimplicialManifold& other) noexcept
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold copy assignment operator.\n";
#endif
    SimplicialManifold temp(other);
    swap(triangulation, temp.triangulation);
    swap(geometry, temp.geometry);
    return *this;
  }

  /// @brief Exception-safe swap
  /// @param first  The first SimplicialManifold to be swapped
  /// @param second The second SimplicialManifold to be swapped with.
  friend void swap(SimplicialManifold& first,
                   SimplicialManifold& second) noexcept
  {
#ifndef NDEBUG
    std::cout << "SimplicialManifold swapperator.\n";
#endif
    using std::swap;
    swap(first.triangulation, second.triangulation);
    swap(first.geometry, second.geometry);
  }

  bool reconcile()
  {
    return (this->triangulation->number_of_vertices() == this->geometry->N0() &&
            this->triangulation->number_of_finite_edges() ==
                this->geometry->number_of_edges() &&
            this->triangulation->number_of_finite_cells() ==
                this->geometry->number_of_cells());
  }

  void update()
  {
    geometry = std::make_unique<GeometryInfo>(
        classify_all_simplices(std::move(triangulation)));
  }
};

#endif  // INCLUDE_SIMPLICIALMANIFOLD_HPP_


/*
 *  Copyright 2010 CNRS
 *
 *  Florent Lamiraux
 */

#include <boost/format.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-setter.h>
#include <dynamic-graph/command-getter.h>
#include "dynamic-graph/tutorial/inverted-pendulum.hh"
#include "command-increment.hh"
#include "constant.hh"

using namespace dynamicgraph;
using namespace dynamicgraph::tutorial;

const double Constant::gravity = 9.81;

// Register new Entity type in the factory
// Note that the second argument is the type name of the python class
// that will be created when importing the python module.
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(InvertedPendulum, "InvertedPendulum");

InvertedPendulum::InvertedPendulum(const std::string& inName)
    : Entity(inName),
      forceSIN(NULL, "InvertedPendulum(" + inName + ")::input(double)::force"),
      stateSOUT("InvertedPendulum(" + inName + ")::output(vector)::state"),
      cartMass_(1.0),
      pendulumMass_(1.0),
      pendulumLength_(1.0),
      viscosity_(0.1) {
  // Register signals into the entity.
  signalRegistration(forceSIN);
  signalRegistration(stateSOUT);

  // Set signals as constant to size them
  Vector state(4);
  state.fill(0.);
  double input = 0.;
  stateSOUT.setConstant(state);
  forceSIN.setConstant(input);

  // Commands
  std::string docstring;

  // Incr
  docstring =
      "\n"
      "    Integrate dynamics for time step provided as input\n"
      "\n"
      "      take one floating point number as input\n"
      "\n";
  addCommand(std::string("incr"), new command::Increment(*this, docstring));

  // setCartMass
  docstring =
      "\n"
      "    Set cart mass\n"
      "\n";
  addCommand(std::string("setCartMass"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
                                             *this, &InvertedPendulum::setCartMass, docstring));

  // getCartMass
  docstring =
      "\n"
      "    Get cart mass\n"
      "\n";
  addCommand(std::string("getCartMass"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
                                             *this, &InvertedPendulum::getCartMass, docstring));

  // setPendulumMass
  docstring =
      "\n"
      "    Set pendulum mass\n"
      "\n";
  addCommand(std::string("setPendulumMass"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
                                                 *this, &InvertedPendulum::setPendulumMass, docstring));

  // getPendulumMass
  docstring =
      "\n"
      "    Get pendulum mass\n"
      "\n";
  addCommand(std::string("getPendulumMass"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
                                                 *this, &InvertedPendulum::getPendulumMass, docstring));

  // setPendulumLength
  docstring =
      "\n"
      "    Set pendulum length\n"
      "\n";
  addCommand(std::string("setPendulumLength"), new ::dynamicgraph::command::Setter<InvertedPendulum, double>(
                                                   *this, &InvertedPendulum::setPendulumLength, docstring));

  // getPendulumLength
  docstring =
      "\n"
      "    Get pendulum length\n"
      "\n";
  addCommand(std::string("getPendulumLength"), new ::dynamicgraph::command::Getter<InvertedPendulum, double>(
                                                   *this, &InvertedPendulum::getPendulumLength, docstring));
}

InvertedPendulum::~InvertedPendulum() {}

Vector InvertedPendulum::computeDynamics(const Vector& inState, const double& inControl, double inTimeStep) {
  if (inState.size() != 4)
    throw dynamicgraph::ExceptionSignal(dynamicgraph::ExceptionSignal::GENERIC, "state signal size is ",
                                        "%d, should be 4.", inState.size());

  double dt = inTimeStep;
  double dt2 = dt * dt;
  double g = Constant::gravity;
  double x = inState(0);
  double th = inState(1);
  double dx = inState(2);
  double dth = inState(3);
  double F = inControl;
  double m = pendulumMass_;
  double M = cartMass_;
  double l = pendulumLength_;
  double lambda = viscosity_;
  double l2 = l * l;
  double dth2 = dth * dth;
  double sth = sin(th);
  double cth = cos(th);
  double sth2 = sth * sth;

  double b1 = F - m * l * dth2 * sth - lambda * dx;
  double b2 = m * l * g * sth - lambda * dth;

  double det = m * l2 * (M + m * sth2);

  double ddx = (b1 * m * l2 + b2 * m * l * cth) / det;
  double ddth = ((M + m) * b2 + m * l * cth * b1) / det;

  Vector nextState(4);
  nextState(0) = x + dx * dt + .5 * ddx * dt2;
  nextState(1) = th + dth * dt + .5 * ddth * dt2;
  nextState(2) = dx + dt * ddx;
  nextState(3) = dth + dt * ddth;

  return nextState;
}

void InvertedPendulum::incr(double inTimeStep) {
  int t = stateSOUT.getTime();
  Vector nextState = computeDynamics(stateSOUT(t), forceSIN(t), inTimeStep);
  stateSOUT.setConstant(nextState);
  stateSOUT.setTime(t + 1);
  forceSIN(t + 1);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2014-2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP

#include <cstddef>

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>

#include <boost/geometry/algorithms/validity_failure_type.hpp>
#include <boost/geometry/algorithms/dispatch/is_valid.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace is_valid
{

template <typename Box, std::size_t I>
struct has_valid_corners
{
    template <typename VisitPolicy>
    static inline bool apply(Box const& box, VisitPolicy& visitor)
    {
        if (math::equals(geometry::get<geometry::min_corner, I-1>(box),
                         geometry::get<geometry::max_corner, I-1>(box)))
        {
            return
                visitor.template apply<failure_wrong_topological_dimension>();
        }
        else if (geometry::get<geometry::min_corner, I-1>(box)
                 >
                 geometry::get<geometry::max_corner, I-1>(box))
        {
            return visitor.template apply<failure_wrong_corner_order>();
        }
        return has_valid_corners<Box, I-1>::apply(box, visitor);
    }
};

template <typename Box>
struct has_valid_corners<Box, 0>
{
    template <typename VisitPolicy>
    static inline bool apply(Box const&, VisitPolicy& visitor)
    {
        geofeatures_boost::ignore_unused(visitor);

        return visitor.template apply<no_failure>();
    }
};

}} // namespace detail::is_valid
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

// A box is always simple
// A box is a Polygon, and it satisfies the conditions for Polygon validity.
//
// The only thing we have to check is whether the max corner lies in
// the upper-right quadrant as defined by the min corner
//
// Reference (for polygon validity): OGC 06-103r4 (6.1.11.1)
template <typename Box>
struct is_valid<Box, box_tag>
    : detail::is_valid::has_valid_corners<Box, dimension<Box>::value>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_IS_VALID_BOX_HPP


#include <armadillo>
#include <iostream>
#include "gtest/gtest.h"

#include "svm_helpers.h"
#include "svm_basic.h"
#include "utils/logger.h"

namespace {

int log_level = LogLevel::DEBUG_LEVEL;

// The fixture
class SVMConfigurationTest: public ::testing::Test {

protected:

    SVMConfigurationTest() {
        std::cout << "Creating SVMConfiguration..." << std::endl << std::flush;
        svm_config = SVMConfiguration();
        std::cout << "Setting logger..." << std::endl << std::flush;
        svm_config.log.verbosity = log_level;

        // Exemplary sparse matrix
        sparse_matrix_csc_01_row_indices =
            helper_sparse_matrix_csc_01_row_indices();
        sparse_matrix_csc_01_column_pointers =
            helper_sparse_matrix_csc_01_column_pointers();
        sparse_matrix_csc_01_values =
            helper_sparse_matrix_csc_01_values();
        sparse_matrix_csc_01_nrow =
            helper_sparse_matrix_csc_01_nrow();
        sparse_matrix_csc_01_ncol =
            helper_sparse_matrix_csc_01_ncol();
        sparse_matrix_csc_01_sp_mat =
            helper_sparse_matrix_csc_01_sp_mat();

        std::cout << "Starting test..." << std::endl << std::flush;
    }

    virtual ~SVMConfigurationTest() {}

    /// Called immediately after the constructor (righ before each test)
    virtual void SetUp() {}

    /// Called immediately after each test (right before the destructor)
    virtual void TearDown() {}

    /* Objects declared here can be used by all tests in the test case */

    SVMConfiguration svm_config;

	arma::uvec sparse_matrix_csc_01_row_indices;
    arma::uvec sparse_matrix_csc_01_column_pointers;
    arma::vec sparse_matrix_csc_01_values;
    size_t sparse_matrix_csc_01_nrow;
    size_t sparse_matrix_csc_01_ncol;
    arma::sp_mat sparse_matrix_csc_01_sp_mat;
};

} // end namespace

/* Fixture tests */

TEST_F(SVMConfigurationTest, setSparseData) {
    std::cout << "SVMConfiguration sparse data..." << std::endl;
    svm_config.setSparseData(
        sparse_matrix_csc_01_column_pointers,
        sparse_matrix_csc_01_row_indices,
        sparse_matrix_csc_01_values,
        sparse_matrix_csc_01_nrow,
        sparse_matrix_csc_01_ncol,
        true
    );

    for (size_t i = 0; i < svm_config.sparse_data.n_cols; ++i) {
        for (size_t j = 0; j < svm_config.sparse_data.n_rows; ++j) {
            // Sparse matrix is currently being held as transposed
            ASSERT_EQ(svm_config.sparse_data(i, j), sparse_matrix_csc_01_sp_mat(i, j));
        }
    }
}


#include <stdio.h>
#include <time.h>
#include <float.h>
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cstdint>

#include <unistd.h>

#include <boost/make_shared.hpp>
#include <boost/algorithm/string.hpp>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/pcd_io.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include "MyCalibration.h"
#include "myutility.h"

extern Eigen::Vector2i g_points[4];

// Variables for debug
bool g_DebugFlg = false;
double d_roll, d_pitch, d_yaw;
double d_tx, d_ty, d_tz;
int d_convertion_method = 2;
int main(int argc, char **argv)
{
  int option;
  float pattern_width = 45.0;
  double convertion_threshold = 0.0001;
  while ((option = getopt(argc, argv, "dht:w:0:1")) != -1)
  {
    char tmp = '\0';
    if (optarg == NULL)
    {
      optarg = &tmp;
    }
    std::string args(optarg);
    std::string delim(",");
    std::vector< std::string > list_args;
    switch (option)
    {
      case 'h':
        std::cout << "-- Usage" << std::endl
                  << "  -d flag for debug" << std::endl
                  << "In Debug Mode: " << std::endl
                  << "  -0\t"
                  << "[roll],[pitch],[yaw],[tx],[ty],[tz]" << std::endl
                  << "  -1\t"
                  << "Switch convertion method. (Perturbation method -> Steepest descent method)" << std::endl
                  << std::endl;
        return 0;
        break;
      case 'd':
        g_DebugFlg = true;
        break;
      case 't':
        convertion_threshold = atof(args.c_str());
        break;
      case 'w':
        pattern_width = atof(args.c_str());
        break;
      case '0':
        if (g_DebugFlg)
        {
          boost::split(list_args, args, boost::is_any_of(delim));
          d_roll = atof(list_args[0].c_str());
          d_pitch = atof(list_args[1].c_str());
          d_yaw = atof(list_args[2].c_str());
          d_tx = atof(list_args[3].c_str());
          d_ty = atof(list_args[4].c_str());
          d_tz = atof(list_args[5].c_str());
        }
        break;
      case '1':
        if (g_DebugFlg)
        {
          d_convertion_method = 1;
        }
        break;
      case ':':
        std::cerr << "Need some values" << std::endl;
        break;
      case '?':
        std::cerr << "unknown option" << std::endl;
        break;
    }
  }

  ros::init(argc, argv, "ExtrinsicParameterEstimation");
  ros::NodeHandle ros_nh;

  cv::namedWindow("Depth image");
  cv::setMouseCallback("Depth image", mouseCallback, 0);

  // For Debug
  g_points[0] << 295, 215;
  g_points[1] << 345, 215;
  g_points[2] << 295, 265;
  g_points[3] << 345, 265;

  pcl::visualization::CloudViewer viewer("OpenNI Viewer");

  // initialization
  char key;
  uint8_t *pdepth;
  uint8_t *pcolor;
  pdepth = new uint8_t[CAMERA_RESOLUTION_X * CAMERA_RESOLUTION_Y * 2];
  pcolor = new uint8_t[CAMERA_RESOLUTION_X * CAMERA_RESOLUTION_Y * 3];

  MyCalibration my_calib;
  my_calib.initialize(viewer, pdepth, pcolor);

  ros::Subscriber sub_depth =
      ros_nh.subscribe("/camera/depth/image_raw", 1, &MyCalibration::getDepthFrameCallback, &my_calib);
  ros::Subscriber sub_color =
      ros_nh.subscribe("/camera/rgb/image_raw", 1, &MyCalibration::getColorFrameCallback, &my_calib);
  ros::Subscriber sub_points =
      ros_nh.subscribe("/camera/depth/points", 1, &MyCalibration::getPointsCallback, &my_calib);

  ros::Rate loop_late(100);

  KeyboardEventReader keyboard;

  while (ros::ok() && key != KEYCODE_q)  // key == 'q' ?
  {
    ros::spinOnce();
    if (key == KEYCODE_r)  // key == 'r' ?
    {
      my_calib.extractPlanePoints();
      my_calib.viewPoints();
      my_calib.calcurateExtrinsicParameters(convertion_threshold, d_convertion_method, pattern_width);
    }
    if (key == KEYCODE_p)  // key == 'p' ?
    {
      my_calib.startPickingPoints();
    }
    if (key == KEYCODE_d)  // key == 'd' ?
    {
      while (key != 0 && key != KEYCODE_q)
      {
        ros::spinOnce();
        key = my_calib.pickPointsAutomatically(5, 6) & 0x00ff;
      }
    }
    key = cv::waitKey(10) & 0x00ff;
    char tmp_key;
  }

  delete[] pdepth;
  delete[] pcolor;

  return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/curvatures_simple.h>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace cctbx { namespace xray { namespace structure_factors {
namespace curvatures_simple {
namespace boost_python {

namespace {

  struct grads_and_curvs_target_wrappers
  {
    typedef grads_and_curvs_target<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<return_by_value> rbv;
      class_<w_t>(
          "structure_factors_curvatures_simple_grads_and_curvs_target",
          no_init)
        .def(init<
          uctbx::unit_cell const&,
          sgtbx::space_group const&,
          af::const_ref<scatterer<> > const&,
          xray::scattering_type_registry const&,
          sgtbx::site_symmetry_table const&,
          af::const_ref<miller::index<> > const&,
          af::const_ref<std::complex<double> > const&,
          af::const_ref<scitbx::vec3<double> > const&>((
            arg("unit_cell"),
            arg("space_group"),
            arg("scatterers"),
            arg("scattering_type_registry"),
            arg("site_symmetry_table"),
            arg("miller_indices"),
            arg("da_db"),
            arg("daa_dbb_dab"))))
        .add_property("grads", make_getter(&w_t::grads, rbv()))
        .add_property("curvs", make_getter(&w_t::curvs, rbv()))
      ;
    }
  };

} // namespace <anoymous>

}}} // namespace structure_factors::curvatures_simple::boost_python

namespace boost_python {

  void wrap_curvatures_simple()
  {
    structure_factors::curvatures_simple::boost_python
      ::grads_and_curvs_target_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ored/portfolio/builders/fxoption.hpp>
#include <ored/portfolio/enginefactory.hpp>
#include <ored/portfolio/fxoption.hpp>
#include <ored/utilities/log.hpp>
#include <ql/errors.hpp>
#include <ql/exercise.hpp>
#include <ql/instruments/compositeinstrument.hpp>
#include <ql/instruments/vanillaoption.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

void FxOption::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    Currency boughtCcy = parseCurrency(boughtCurrency_);
    Currency soldCcy = parseCurrency(soldCurrency_);

    QL_REQUIRE(tradeActions().empty(), "TradeActions not supported for FxOption");

    // Payoff
    Real strike = soldAmount_ / boughtAmount_;
    Option::Type type = parseOptionType(option_.callPut());
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type, strike));

    // Only European Vanilla supported for now
    QL_REQUIRE(option_.style() == "European", "Option Style unknown: " << option_.style());
    QL_REQUIRE(option_.exerciseDates().size() == 1, "Invalid number of excercise dates");
    Date expiryDate = parseDate(option_.exerciseDates().front());

    // Exercise
    boost::shared_ptr<Exercise> exercise = boost::make_shared<EuropeanExercise>(expiryDate);

    // Vanilla European/American.
    // If price adjustment is necessary we build a simple EU Option
    boost::shared_ptr<QuantLib::Instrument> instrument;

    // QL does not have an FXOption, so we add a vanilla one here and wrap
    // it in a composite to get the notional in.
    boost::shared_ptr<Instrument> vanilla = boost::make_shared<VanillaOption>(payoff, exercise);

    // we buy foriegn with domestic(=sold ccy).
    boost::shared_ptr<EngineBuilder> builder = engineFactory->builder(tradeType_);
    QL_REQUIRE(builder, "No builder found for " << tradeType_);
    boost::shared_ptr<FxOptionEngineBuilder> fxOptBuilder = boost::dynamic_pointer_cast<FxOptionEngineBuilder>(builder);

    vanilla->setPricingEngine(fxOptBuilder->engine(boughtCcy, soldCcy));

    Position::Type positionType = parsePositionType(option_.longShort());
    Real bsInd = (positionType == QuantLib::Position::Long ? 1.0 : -1.0);
    Real mult = boughtAmount_ * bsInd;

    // If premium data is provided
    // 1) build the fee trade and pass it to the instrument wrapper for pricing
    // 2) add fee payment as additional trade leg for cash flow reporting
    std::vector<boost::shared_ptr<Instrument>> additionalInstruments;
    std::vector<Real> additionalMultipliers;
    if (option_.premiumPayDate() != "" && option_.premiumCcy() != "") {
        Real premiumAmount = -bsInd * option_.premium(); // pay if long, receive if short
        Currency premiumCurrency = parseCurrency(option_.premiumCcy());
        Date premiumDate = parseDate(option_.premiumPayDate());
        // soldCcy is the NpvCcy, see below
        addPayment(additionalInstruments, additionalMultipliers, premiumDate, premiumAmount, premiumCurrency, soldCcy,
                   engineFactory, fxOptBuilder->configuration(MarketContext::pricing));
        DLOG("option premium added for fx option " << id());
    }

    instrument_ = boost::shared_ptr<InstrumentWrapper>(
        new VanillaInstrument(vanilla, mult, additionalInstruments, additionalMultipliers));

    npvCurrency_ = soldCurrency_; // sold is the domestic
    notional_ = soldAmount_;
    maturity_ = expiryDate;
}

void FxOption::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* fxNode = XMLUtils::getChildNode(node, "FxOptionData");
    QL_REQUIRE(fxNode, "No FxOptionData Node");
    option_.fromXML(XMLUtils::getChildNode(fxNode, "OptionData"));
    boughtCurrency_ = XMLUtils::getChildValue(fxNode, "BoughtCurrency", true);
    soldCurrency_ = XMLUtils::getChildValue(fxNode, "SoldCurrency", true);
    boughtAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "BoughtAmount", true);
    soldAmount_ = XMLUtils::getChildValueAsDouble(fxNode, "SoldAmount", true);
}

XMLNode* FxOption::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* fxNode = doc.allocNode("FxOptionData");
    XMLUtils::appendNode(node, fxNode);

    XMLUtils::appendNode(fxNode, option_.toXML(doc));
    XMLUtils::addChild(doc, fxNode, "BoughtCurrency", boughtCurrency_);
    XMLUtils::addChild(doc, fxNode, "BoughtAmount", boughtAmount_);
    XMLUtils::addChild(doc, fxNode, "SoldCurrency", soldCurrency_);
    XMLUtils::addChild(doc, fxNode, "SoldAmount", soldAmount_);

    return node;
}
} // namespace data
} // namespace ore


/*******************************************************************************
*
*  Filename    : RooFitMgr.cc
*  Description : Implementation for RooFitMgr Class
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "ManagerUtils/RootMgr/interface/RooFitMgr.hpp"
#include <boost/algorithm/string.hpp>

using namespace mgr;
using namespace std;

/*******************************************************************************
*   Static variable declaration
*******************************************************************************/
RootObjMgr<RooRealVar> RooFitMgr::_staticvarmgr( "" );// nameless container

/*******************************************************************************
*   Constructor and destructor
*******************************************************************************/
RooFitMgr::RooFitMgr( const string& name ) :
  Named( name ),
  _varmgr( name ),
  _setmgr( name ),
  _pdfmgr( name ),
  _funcmgr( name )
{
}

/******************************************************************************/

RooFitMgr::~RooFitMgr()
{
}

/*******************************************************************************
*   Static RooRealVar access functions
*******************************************************************************/
RooRealVar*
RooFitMgr::StaticNewVar(
  const string& varname,
  const string& title,
  const string& unit,
  const double  min,
  const double  max
  )
{
  if( StaticVar( varname ) ){
    RooRealVar* var = StaticVar( varname );
    var->setConstant( kFALSE );
    var->setMin( min );
    var->setMax( max );
    var->setUnit( unit.c_str() );
    var->SetTitle( title.c_str() );
    return var;
  } else {
    RooRealVar* newvar = new RooRealVar(
      varname.c_str(),
      title.c_str(),
      min,
      max,
      unit.c_str()
      );
    _staticvarmgr.AddObj( newvar );
    return _staticvarmgr.GetObj( varname );
  }
}

/******************************************************************************/

RooRealVar*
RooFitMgr::StaticVar( const string& name )
{
  return _staticvarmgr.GetObj( name );
}

/******************************************************************************/

vector<string>
RooFitMgr::StaticVarNameList()
{
  return _staticvarmgr.ObjNameList();
}

/*******************************************************************************
*   RooRealVar access functions
*******************************************************************************/
RooRealVar*
RooFitMgr::NewVar( const string& varname, const double min, const double max )
{
  if( Var( varname ) ){// if already exists variable
    RooRealVar* var = Var( varname );
    var->setConstant( kFALSE );
    var->setMin( min );
    var->setMax( max );
    return var;
  } else {
    RooRealVar* newvar = new RooRealVar( varname.c_str(), "", min, max );
    _varmgr.AddObj( newvar );
    return _varmgr.GetObj( varname );
  }
}

/******************************************************************************/

RooRealVar*
RooFitMgr::NewVar( const string& varname, const double cen, const double min, const double max )
{
  if( Var( varname ) ){// if already exists variable
    RooRealVar* var = Var( varname );
    var->setConstant( kFALSE );
    var->setMin( min );
    var->setMax( max );
    *var = cen;
    return var;
  } else {
    RooRealVar* newvar = new RooRealVar( varname.c_str(), "", cen, min, max );
    _varmgr.AddObj( newvar );
    return _varmgr.GetObj( varname );
  }
}

/******************************************************************************/

RooRealVar*
RooFitMgr::Var( const string& name )
{
  return _varmgr.GetObj( name );
}

/******************************************************************************/

const RooRealVar*
RooFitMgr::Var( const string& name ) const
{
  return _varmgr.GetObj( name );
}

/******************************************************************************/

vector<string>
RooFitMgr::VarNameList() const
{
  return _varmgr.ObjNameList();
}

/******************************************************************************/

vector<RooRealVar*>
RooFitMgr::VarContains( const string& substring )
{
  return _varmgr.GetObjContains( substring );
}

/******************************************************************************/

void
RooFitMgr::SetConstant( const bool state )
{
  for( const auto& name  : _varmgr.ObjNameList() ){
    _varmgr.GetObj( name )->setConstant( state );
  }
}

/******************************************************************************/

void RooFitMgr::RemoveVar( const std::string& name )
{
  _varmgr.RemoveObj(name);
}

/*******************************************************************************
*   RooAbsData access functions
*******************************************************************************/
RooAbsData*
RooFitMgr::DataSet( const std::string& name )
{
  return _setmgr.GetObj( name );
}

/******************************************************************************/

const RooAbsData*
RooFitMgr::DataSet( const std::string& name ) const
{
  return _setmgr.GetObj( name );
}

/******************************************************************************/

vector<std::string>
RooFitMgr::SetNameList() const
{
  return _setmgr.ObjNameList();
}

/******************************************************************************/

void
RooFitMgr::AddDataSet( RooAbsData* set )
{
  _setmgr.AddObj( set );
}

/******************************************************************************/

void
RooFitMgr::RemoveDataSet( const std::string& name )
{
  _setmgr.RemoveObj( name );
}

/*******************************************************************************
*   RooAbsPdf access functions
*******************************************************************************/
void
RooFitMgr::AddPdf( RooAbsPdf* pdf )
{
  _pdfmgr.AddObj( pdf );
}

/******************************************************************************/

RooAbsPdf*
RooFitMgr::Pdf( const std::string& name )
{
  return _pdfmgr.GetObj( name );
}

/******************************************************************************/

const RooAbsPdf*
RooFitMgr::Pdf( const std::string& name ) const
{
  return _pdfmgr.GetObj( name );
}

/******************************************************************************/

vector<std::string>
RooFitMgr::PdfNameList() const
{
  return _pdfmgr.ObjNameList();
}

/******************************************************************************/

void
RooFitMgr::RemovePdf( const std::string& name )
{
  _pdfmgr.RemoveObj(name);
}

/*******************************************************************************
*   RooAbsReal access functions
*******************************************************************************/
void
RooFitMgr::AddFunc( RooAbsReal* func )
{
  _funcmgr.AddObj( func );
}

RooAbsReal*
RooFitMgr::Func( const std::string& name )
{
  return _funcmgr.GetObj( name );
}

const RooAbsReal*
RooFitMgr::Func( const std::string& name ) const
{
  return _funcmgr.GetObj( name );
}

vector<std::string>
RooFitMgr::FuncNameList() const
{
  return _funcmgr.ObjNameList();
}


#include <boost/thread/tss.hpp>

#include "externals/cuda/host.hpp"
#include "externals/cuda/types.hpp"

#include <rysq/cuda.hpp>
#include "cuda/detail.hpp"
#include "index.hpp"

using namespace rysq::cuda;
namespace host = cuda::host;

namespace {

    using rysq::State;

    size_t coefficients(const State &bra, const State &ket, double2 *AB) {
	// size_t L = bra.L() + ket.L();
	// ket
#define COEFFICIENT(shell,index) ((shell)(index,(shell).is_hybrid()))
	for(int l = 0, ijkl = 0; l < ket[1].K; ++l) {
	    for(int k = 0; k < ket[0].K; ++k) {
		double B = ket[0](k) + ket[1](l);
		double Ckl = COEFFICIENT(ket[0], k)*COEFFICIENT(ket[1], l);
		// bra
		for(int j = 0; j < bra[1].K; ++j) {
		    for(int i = 0; i < bra[0].K; ++i, ++ijkl) {
			// compute values
			double A = bra[0](i) + bra[1](j);
			double Cij = COEFFICIENT(bra[0], i)*COEFFICIENT(bra[1], j);
			// double Cij = bra[0](i,0)*bra[1](j,0);
			double C = Cij*Ckl;
			double AB2 = 1.0/(sqrt(A+B)*A*B);
			AB[ijkl].x = C*AB2;
			AB[ijkl].y = 1.0/(A + B);
			// std::cout << AB[ijkl].x << AB[ijkl].y << "\n";
			// if (L == 0) AB[ijkl].y *= A*B;
			// if (L == 1) AB[ijkl].y *= A;
		    }
		}
	    }
	}
	return bra.K()*ket.K()*sizeof(double2);
#undef COEFFICIENT
    }

    size_t exponents(const State &state, double2 *dest) {
	for(int j = 0, ij = 0; j < state[1].K; ++j) {
	    for(int i = 0; i < state[0].K; ++i, ++ij) {
		dest[ij].x = state[0](i);
		dest[ij].y = state[1](j);
	    }
	}
	return state.K()*sizeof(double2);
    }

    size_t sp_coefficients(const rysq::Shell &shell, double *Cps) {
	if (!shell.is_hybrid()) return 0;
	for (int i = 0; i < shell.K; ++i) {
	    Cps[i] = shell(i,0)/shell(i,1);
	}
	return shell.K*sizeof(double);
    }

    size_t sp_coefficients(const State &state, double *Cps) {
	double *Cps0 = Cps;
	double *Cps1 = Cps + (state[0].type < 0)*state[0].K;
	for(int j = 0, ij = 0; j < state[1].K; ++j) {
	    double Cj = state[1](j,0);
	    for(int i = 0; i < state[0].K; ++i, ++ij) {
		double Ci = state[0](i,0);
		if (state[0].type < 0) Cps0[ij] = (state[0](i,1))/Ci;
		if (state[1].type < 0) Cps1[ij] = (state[1](j,1))/Cj;
	    }
	}
	return (state.K()*state.hybrid())*sizeof(double);
    }

    struct Index {
	static const Index& object() {
	    if (!Index::instance_.get()) {
		Index::instance_ .reset(new Index());
	    }
	    return *Index::instance_;
	}
	typedef host::array_ref<const ushort3> array_ref;
	Index() {
	    index host;
	    map_ = host.map();
	    const index::vector_type &data = host.data();
	    size_t size = data.size();
	    index_.reserve(size);
	    host::copy(data, index_);
	}
	array_ref find(rysq::Quartet<rysq::Shell> quartet) const {
	    map_type::const_iterator it = map_.find(index::key(quartet));
	    size_t size = quartet.size();
	    if (it == map_.end())
		return array_ref(host::wrap<ushort3>(NULL),  size);
	    // std::cout << index_.data().data() << " "
	    // 	  << (*it).second << " " << size << std::endl;
	    return array_ref(index_ + (*it).second, size);
	}

    private:
	static boost::thread_specific_ptr<Index> instance_;
	// static const Index *instance_;
	typedef rysq::Index<ushort3> index;
	typedef index::map_type map_type;
	map_type map_;
	host::array<ushort3> index_;
    };
    boost::thread_specific_ptr<Index> Index::instance_;

}

rysq::cuda::detail::Quartet::Quartet(const rysq::Quartet<Shell> &quartet)
    : base(quartet) {
    index2d_ = ::Index::object().find(quartet);

    const rysq::Quartet<Shell>::Bra &bra = quartet.bra();
    const rysq::Quartet<Shell>::Ket &ket = quartet.ket();
    size_t size = 0;

    size = (bra.K()*ket.K())*sizeof(double2);
    size += (bra.K() + ket.K())*sizeof(double2);
    for (int i = 0; i < 4; ++i)
	size += quartet[i].is_hybrid()*quartet[i].K*sizeof(double);

    char *data = new char[size];
    size_t bytes = 0;
    bytes += exponents(bra, (double2*)(data + bytes));
    bytes += exponents(ket, (double2*)(data + bytes));
    bytes += coefficients(bra, ket, (double2*)(data + bytes));

    for (int i = 0; i < 4; ++i)
	bytes += sp_coefficients(quartet[i], (double*)(data + bytes));

    assert(bytes == size);

    this->data_.reserve(size);
    host::copy(data, this->data_.data(), size);
    delete[] data;
}