/*
 * SimpleFeatures.cpp
 * This creates features on the netflow features without any
 * operators added in.
 * The base netflow features are:
 * 1) duration
 * 2) source app bytes
 * 3) dest app bytes
 * 4) source total bytes
 * 5) dest total bytes
 * 6) source packets
 * 7) dest packets
 *
 * The types of features created are
 * 1) average
 * 2) variance
 *  Created on: March 15, 2017
 *      Author: elgood
 */

#include <string>
#include <vector>
#include <stdlib.h>
#include <iostream>
#include <chrono>

#include <boost/program_options.hpp>

#include <sam/VastNetflow.hpp>
#include <sam/sam.hpp>

using std::string;
using std::vector;
using std::cout;
using std::endl;

namespace po = boost::program_options;

using namespace sam;
using namespace std::chrono;

typedef TupleStringHashFunction<VastNetflow, SourceIp> SourceHash;
typedef TupleStringHashFunction<VastNetflow, DestIp> TargetHash;
typedef ZeroMQPushPull<VastNetflow, VastNetflowTuplizer, SourceHash, TargetHash>
        PartitionType;
typedef ReadCSV<VastNetflow, VastNetflowTuplizer> ReadCSVType;

//zmq::context_t context(1);

void createPipeline(
                 std::shared_ptr<ReadCSVType> readCSV,
                 std::shared_ptr<FeatureMap> featureMap,
                 std::shared_ptr<FeatureSubscriber> subscriber,
                 std::shared_ptr<PartitionType> pushpull,
                 std::size_t queueLength,
                 std::size_t numNodes,
                 std::size_t nodeId,
                 std::vector<std::string> const& hostnames,
                 std::size_t hwm,
                 std::size_t N,
                 std::size_t b,
                 std::size_t k)
{
  // An operator to get the label from each netflow and add it to the
  // subscriber.
  string identifier = "label";

  // Doesn't really need a key, but provide one anyway to the template.
  auto label = std::make_shared<Identity<VastNetflow, SamLabel, DestIp>>
                (nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(label);
  } else {
    pushpull->registerConsumer(label);
  }
  if (subscriber != NULL) {
    label->registerSubscriber(subscriber, identifier);
  }

  /** Dest Ip as key **/
  identifier = "averageSrcTotalBytes";
  auto averageSrcTotalBytes = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 SrcTotalBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);

  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcTotalBytes);
  } else {
    pushpull->registerConsumer(averageSrcTotalBytes);
  }
  if (subscriber != NULL) {
    averageSrcTotalBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcTotalBytes";
  auto varSrcTotalBytes = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 SrcTotalBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcTotalBytes);
  } else {
    pushpull->registerConsumer(varSrcTotalBytes);
  }
  if (subscriber != NULL) {
    varSrcTotalBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestTotalBytes";
  auto averageDestTotalBytes = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DestTotalBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestTotalBytes);
  } else {
    pushpull->registerConsumer(averageDestTotalBytes);
  }
  if (subscriber != NULL) {
    averageDestTotalBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestTotalBytes";
  auto varDestTotalBytes = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DestTotalBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestTotalBytes);
  } else {
    pushpull->registerConsumer(varDestTotalBytes);
  }
  if (subscriber != NULL) {
    varDestTotalBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDuration";
  auto averageDuration = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DurationSeconds,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDuration);
  } else {
    pushpull->registerConsumer(averageDuration);
  }
  if (subscriber != NULL) {
    averageDuration->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDuration";
  auto varDuration = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DurationSeconds,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDuration);
  } else {
    pushpull->registerConsumer(varDuration);
  }
  if (subscriber != NULL) {
    varDuration->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageSrcPayloadBytes";
  auto averageSrcPayloadBytes = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 SrcPayloadBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcPayloadBytes);
  } else {
    pushpull->registerConsumer(averageSrcPayloadBytes);
  }
  if (subscriber != NULL) {
    averageSrcPayloadBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcPayloadBytes";
  auto varSrcPayloadBytes = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 SrcPayloadBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcPayloadBytes);
  } else {
    pushpull->registerConsumer(varSrcPayloadBytes);
  }
  if (subscriber != NULL) {
    varSrcPayloadBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestPayloadBytes";
  auto averageDestPayloadBytes = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DestPayloadBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestPayloadBytes);
  } else {
    pushpull->registerConsumer(averageDestPayloadBytes);
  }
  if (subscriber != NULL) {
    averageDestPayloadBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestPayloadBytes";
  auto varDestPayloadBytes = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DestPayloadBytes,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestPayloadBytes);
  } else {
    pushpull->registerConsumer(varDestPayloadBytes);
  }
  if (subscriber != NULL) {
    varDestPayloadBytes->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageSrcPacketCount";
  auto averageSrcPacketCount = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 FirstSeenSrcPacketCount,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcPacketCount);
  } else {
    pushpull->registerConsumer(averageSrcPacketCount);
  }
  if (subscriber != NULL) {
    averageSrcPacketCount->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcPacketCount";
  auto varSrcPacketCount = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 FirstSeenSrcPacketCount,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcPacketCount);
  } else {
    pushpull->registerConsumer(varSrcPacketCount);
  }
  if (subscriber != NULL) {
    varSrcPacketCount->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestPacketCount";
  auto averageDestPacketCount = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 FirstSeenDestPacketCount,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestPacketCount);
  } else {
    pushpull->registerConsumer(averageDestPacketCount);
  }
  if (subscriber != NULL) {
    averageDestPacketCount->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestPacketCount";
  auto varDestPacketCount = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 FirstSeenDestPacketCount,
                                                 DestIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestPacketCount);
  } else {
    pushpull->registerConsumer(varDestPacketCount);
  }
  if (subscriber != NULL) {
    varDestPacketCount->registerSubscriber(subscriber, identifier);
  }

  /** SourceIp as key **/
  identifier = "averageSrcTotalBytesSourceIp";
  auto averageSrcTotalBytesSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 SrcTotalBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcTotalBytesSourceIp);
  } else {
    pushpull->registerConsumer(averageSrcTotalBytesSourceIp);
  }
  if (subscriber != NULL) {
    averageSrcTotalBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcTotalBytesSourceIp";
  auto varSrcTotalBytesSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 SrcTotalBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcTotalBytesSourceIp);
  } else {
    pushpull->registerConsumer(varSrcTotalBytesSourceIp);
  }
  if (subscriber != NULL) {
    varSrcTotalBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestTotalBytesSourceIp";
  auto averageDestTotalBytesSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DestTotalBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestTotalBytesSourceIp);
  } else {
    pushpull->registerConsumer(averageDestTotalBytesSourceIp);
  }
  if (subscriber != NULL) {
    averageDestTotalBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestTotalBytesSourceIp";
  auto varDestTotalBytesSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DestTotalBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestTotalBytesSourceIp);
  } else {
    pushpull->registerConsumer(varDestTotalBytesSourceIp);
  }
  if (subscriber != NULL) {
    varDestTotalBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDurationSourceIp";
  auto averageDurationSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DurationSeconds,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDurationSourceIp);
  } else {
    pushpull->registerConsumer(averageDurationSourceIp);
  }
  if (subscriber != NULL) {
    averageDurationSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDurationSourceIp";
  auto varDurationSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DurationSeconds,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDurationSourceIp);
  } else {
    pushpull->registerConsumer(varDurationSourceIp);
  }
  if (subscriber != NULL) {
    varDurationSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageSrcPayloadBytesSourceIp";
  auto averageSrcPayloadBytesSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 SrcPayloadBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcPayloadBytesSourceIp);
  } else {
    pushpull->registerConsumer(averageSrcPayloadBytesSourceIp);
  }
  if (subscriber != NULL) {
    averageSrcPayloadBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcPayloadBytesSourceIp";
  auto varSrcPayloadBytesSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 SrcPayloadBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcPayloadBytesSourceIp);
  } else {
    pushpull->registerConsumer(varSrcPayloadBytesSourceIp);
  }
  if (subscriber != NULL) {
    varSrcPayloadBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestPayloadBytesSourceIp";
  auto averageDestPayloadBytesSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 DestPayloadBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestPayloadBytesSourceIp);
  } else {
    pushpull->registerConsumer(averageDestPayloadBytesSourceIp);
  }
  if (subscriber != NULL) {
    averageDestPayloadBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestPayloadBytesSourceIp";
  auto varDestPayloadBytesSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 DestPayloadBytes,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestPayloadBytesSourceIp);
  } else {
    pushpull->registerConsumer(varDestPayloadBytesSourceIp);
  }
  if (subscriber != NULL) {
    varDestPayloadBytesSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageSrcPacketCountSourceIp";
  auto averageSrcPacketCountSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 FirstSeenSrcPacketCount,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageSrcPacketCountSourceIp);
  } else {
    pushpull->registerConsumer(averageSrcPacketCountSourceIp);
  }
  if (subscriber != NULL) {
    averageSrcPacketCountSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varSrcPacketCountSourceIp";
  auto varSrcPacketCountSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 FirstSeenSrcPacketCount,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varSrcPacketCountSourceIp);
  } else {
    pushpull->registerConsumer(varSrcPacketCountSourceIp);
  }
  if (subscriber != NULL) {
    varSrcPacketCountSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "averageDestPacketCountSourceIp";
  auto averageDestPacketCountSourceIp = std::make_shared<
                      ExponentialHistogramAve<double, VastNetflow,
                                                 FirstSeenDestPacketCount,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(averageDestPacketCountSourceIp);
  } else {
    pushpull->registerConsumer(averageDestPacketCountSourceIp);
  }
  if (subscriber != NULL) {
    averageDestPacketCountSourceIp->registerSubscriber(subscriber, identifier);
  }

  identifier = "varDestPacketCountSourceIp";
  auto varDestPacketCountSourceIp = std::make_shared<
                      ExponentialHistogramVariance<double, VastNetflow,
                                                 FirstSeenDestPacketCount,
                                                 SourceIp>>
                          (N, 2, nodeId, featureMap, identifier);
  if (readCSV != NULL) {
    readCSV->registerConsumer(varDestPacketCountSourceIp);
  } else {
    pushpull->registerConsumer(varDestPacketCountSourceIp);
  }
  if (subscriber != NULL) {
    varDestPacketCountSourceIp->registerSubscriber(subscriber, identifier);
  }

}

int main(int argc, char** argv) {

  string ip; ///> The ip to read the nc data from.
  std::size_t ncPort; ///> The port to read the nc data from.
  std::size_t numNodes; ///> The number of nodes in the cluster
  std::size_t nodeId; ///> The node id of this node
  string prefix; ///> The prefix to the nodes
  std::size_t startingPort; ///> The starting port number
  std::size_t hwm; ///> The high-water mark (zeromq parameter)
  std::size_t queueLength; ///> The length of the input queue
  std::size_t N; ///> The total number of elements in a sliding window
  std::size_t b; ///> The number of elements in a dormant or active window
  std::size_t k; ///> The number of elements to keep track of
  std::size_t nop; //not used
  string inputfile = "";
  string outputfile = "";
  std::size_t capacity = 10000;////> Capacity of FeatureMap and subscriber

  // The training data if learning the classifier
  //arma::mat trainingData;

  // The model that can be trained from example data or if a trained model
  // exists, can be loaded from the filesystem.
  //NBCModel model;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "help message")
    ("ip", po::value<string>(&ip)->default_value("localhost"),
      "The ip to receive the data from nc")
    ("ncPort", po::value<std::size_t>(&ncPort)->default_value(9999),
      "The port to receive the data from nc")
    ("numNodes", po::value<std::size_t>(&numNodes)->default_value(1),
      "The number of nodes involved in the computation")
    ("nodeId", po::value<std::size_t>(&nodeId)->default_value(0),
      "The node id of this node")
    ("prefix", po::value<string>(&prefix)->default_value("node"),
      "The prefix common to all nodes")
    ("startingPort", po::value<std::size_t>(&startingPort)->default_value(
      10000),  "The starting port for the zeromq communications")
    ("hwm", po::value<std::size_t>(&hwm)->default_value(10000),
      "The high water mark (how many items can queue up before we start "
      "dropping)")
    ("queueLength", po::value<std::size_t>(&queueLength)->default_value(10000),
      "We fill a queue before sending things in parallel to all consumers."
      "  This controls the size of that queue.")
    ("N", po::value<std::size_t>(&N)->default_value(10000),
      "The total number of elements in a sliding window")
    ("b", po::value<std::size_t>(&b)->default_value(1000),
      "The number of elements per block (active or dynamic window)")
    ("nop", po::value<std::size_t>(&nop)->default_value(1),
      "The number of simultaneous operators")
    ("create_features", "If specified, will read a netflow feature file "
     "from --inputfile and output to --outputfile a csv feature file")
    ("train", "If specified, will read a csv feature file from --inputfile"
     " and output to --outputfile a learned model.")
    ("test", "If specified, will read a learned model from --inputfile"
     " and apply it to the data.")
    ("inputfile", po::value<string>(&inputfile),
      "If --create_features is specified, the input should be a file with"
      " netflow.  If --train is specified, the input should be a csv file"
      " of features (the output of --create_features).  If --test is specified,"
      " the input should be a model (the output of --train).")
    ("outputfile", po::value<string>(&outputfile),
      "If --create_features is specified, the produced file will be a csv"
      " file of features.  If --train is specified, the produced file will be"
      " a learned model.")
    ("capacity", po::value<std::size_t>(&capacity)->default_value(10000),
      "The capacity of the FeatureMap and FeatureSubcriber")
  ;

  // Parse the command line variables
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  // Print out the help and exit if --help was specified.
  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }

  vector<string> hostnames(numNodes); // A vector of hosts in the cluster

  if (numNodes == 1) { // Case when we are operating on one node
    hostnames[0] = "127.0.0.1";
  } else {
    for (int i = 0; i < numNodes; i++) {
      // Assumes all the host names can be composed by adding prefix with
      // [0,numNodes).
      hostnames[i] = prefix + boost::lexical_cast<string>(i);

    }
  }

  // The global featureMap (global for all features generated for this node;
  // each node has it's own featuremap.
  std::cout << "About to create feature Map " << std::endl;
  auto featureMap = std::make_shared<FeatureMap>(capacity);

  /********************** Creating features ******************************/
  if (vm.count("create_features"))
  {
    if (inputfile == "") {
      std::cout << "--create_features was specified but no input file"
                << " was listed with --inputfile." << std::endl;
      return -1;
    }
    if (outputfile == "") {
      std::cout << "--create_features was specified but no output file"
                << " was listed with --outputfile." << std::endl;
      return -1;
    }

    // We read the netflow data from a file.  It assumes each netflow
    // has a label at the beginning.
    auto receiver = std::make_shared<ReadCSVType>(inputfile);

    // subscriber collects the features for each netflow
    auto subscriber = std::make_shared<FeatureSubscriber>(outputfile, capacity);

    std::cout << "Creating Pipeline " << std::endl;
    // createPipeline creates all the operators and ties them together.  It
    // also notifies the designated feature producers of the subscriber.
    createPipeline(receiver, featureMap, subscriber, NULL,
                   queueLength,
                   numNodes,
                   nodeId,
                   hostnames,
                   hwm,
                   N, b, k);

    std::cout << "Created Pipeline " << std::endl;

    // You must call init before starting the pipeline.
    subscriber->init();

    // Connects the receiver to the input data but doesn't start ingestion.
    if (!receiver->connect()) {
      std::cout << "Problems opening file " << inputfile << std::endl;
      return -1;
    }

    milliseconds ms1 = duration_cast<milliseconds>(
      system_clock::now().time_since_epoch()
    );
    // Starts the pipeline
    receiver->receive();
    milliseconds ms2 = duration_cast<milliseconds>(
      system_clock::now().time_since_epoch()
    );
    std::cout << "Seconds for Node" << nodeId << ": "
      << static_cast<double>(ms2.count() - ms1.count()) / 1000 << std::endl;

    std::cout << "Finished" << std::endl;
    return 0;
  }
  /********************* Learning Model *********************************/
  else if (vm.count("train"))
  {
    /*if (inputfile == "") {
      std::cout << "--train was specified but no input file"
                << " was listed with --inputfile." << std::endl;
      return -1;
    }
    if (outputfile == "") {
      std::cout << "--train was specified but no output file"
                << " was listed with --outputfile." << std::endl;
      return -1;
    }

    // The true parameter transposes the data.  In mlpack, rows are features
    // and columns are observations, which makes things confusing.
    data::Load(inputfile, trainingData, true);

    arma::Row<double> labels = trainingData.row(0);

    //data::NormalizeLabels(trainingData.row(0), labels, model.mappings);

    // Remove the label row
    trainingData.shed_row(0);

    Timer::Start("nbc_training");
    std::cout << "About to train " << std::endl;
    model.nbc = NaiveBayesClassifier<>(trainingData, labels,
      model.mappings.n_elem, true);

    data::Save(outputfile, "model", model, true);
    std::cout << "Saved Model " << std::endl;
    Timer::Stop("nbc_training");
    return 0;
    */
  }
  /******************** Applying model *********************************/
  else if (vm.count("test"))
  {
    /*if (inputfile == "") {
      std::cout << "--test was specified but no input file"
                << " was listed with --inputfile." << std::endl;
      return -1;
    }
    data::Load(inputfile, "model", model);
    cout << "model.mappings " << model.mappings << std::endl;
    */
  }
  /******************* Running pipeline without model ******************/
  else
  {

    auto receiver = std::make_shared<ReadSocket>(ip, ncPort);

    // Make a commandline argument
    size_t timeout = 1000;

    // Creating the ZeroMQPushPull consumer.  This consumer is responsible for
    // getting the data from the receiver (e.g. a socket or a file) and then
    // publishing it in a load-balanced way to the cluster.
    auto consumer = std::make_shared<PartitionType>(queueLength,
                                   numNodes,
                                   nodeId,
                                   hostnames,
                                   startingPort, timeout, false,
                                   hwm);

    receiver->registerConsumer(consumer);

    createPipeline(NULL, featureMap, NULL, consumer,
                   queueLength,
                   numNodes,
                   nodeId,
                   hostnames,
                   hwm,
                   N, b, k);

    if (!receiver->connect()) {
      std::cout << "Couldn't connected to " << ip << ":" << ncPort << std::endl;
      return -1;
    }

    milliseconds ms1 = duration_cast<milliseconds>(
      system_clock::now().time_since_epoch()
    );
    receiver->receive();
    milliseconds ms2 = duration_cast<milliseconds>(
      system_clock::now().time_since_epoch()
    );
    std::cout << "Seconds for Node" << nodeId << ": "
      << static_cast<double>(ms2.count() - ms1.count()) / 1000 << std::endl;
  }
}


/* Copyright 2017 The sfcpp Authors. All Rights Reserved.

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.
==============================================================================*/

#pragma once

#include <math/NatSet.hpp>

#include <Eigen/Dense>

#include <memory>
#include <ostream>
#include <vector>

namespace sfcpp {
namespace geo {

/**
 * Stores the combinatorial information of a polytope, i. e. the indices of
 * vertices contained in each face.
 */
class ConvexPolytope {
 public:
  struct Face {
    math::NatSet vertices;
    std::vector<size_t> parentIndexes;
    std::vector<size_t> childIndexes;
    size_t dim;

    friend std::ostream &operator<<(std::ostream &stream,
                                    ConvexPolytope::Face const &face);
  };

  std::vector<std::vector<Face>> faces;

  ConvexPolytope(size_t d);

  bool tryFindFace(size_t dim, math::NatSet const &vertexSet,
                   size_t &faceIndex) const;

  static std::shared_ptr<ConvexPolytope> convexHull(Eigen::MatrixXd points);

  friend std::ostream &operator<<(std::ostream &stream,
                                  ConvexPolytope const &polytope);

  void sort();

  size_t getDimension();

  // TODO: method for comparing two ConvexPolytopes?
};

} /* namespace geo */
} /* namespace sfcpp */


#pragma once
#include <variant>

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/Eigenvalues>

#include <tbb/tbb.h>

#include "ED/ConstructSparseMat.hpp"
#include "Utilities/Utility.hpp"
#include "./utils.hpp"

namespace yannq
{
//! \addtogroup GroundState

template<typename Machine>
class SamplingResultExact
{
private:
	const Machine& qs_;
	const uint32_t N_;
	const tbb::concurrent_vector<uint32_t>& basis_;

public:
	SamplingResultExact(const Machine& qs,
			const tbb::concurrent_vector<uint32_t>& basis)
		: qs_{qs}, N_{qs.getN()}, basis_{basis}
	{
	}

	typename Machine::DataT operator[](uint32_t idx) const
	{
		return qs_.makeData(toSigma(N_, basis_[idx]));
	}

	std::size_t size() const
	{
		return basis_.size();
	}

};

//! \ingroup GroundState
//! This class calculate the quantum Fisher matrix by exactly constructing the quantum state.
template<typename Machine>
class SRMatExact
{
public:
	using Scalar = typename Machine::Scalar;
	using RealScalar = typename remove_complex<Scalar>::type;

	using Matrix = typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
	using MatrixRowMajor = typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
	using Vector = typename Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

private:
	const uint32_t n_;
	const Machine& qs_;
	tbb::concurrent_vector<uint32_t> basis_;

	std::variant<Eigen::SparseMatrix<RealScalar>, Eigen::SparseMatrix<Scalar>> ham_;

	Matrix deltas_;
	Matrix deltasPsis_;
	Vector oloc_;
	Vector grad_;

	RealScalar energy_;
	RealScalar energyVar_;

public:

	RealScalar eloc() const
	{
		return energy_;
	}

	RealScalar elocVar() const
	{
		return energyVar_;
	}

	void clear()
	{
		deltas_ = Matrix{};
		deltasPsis_ = Matrix{};
		oloc_ = Vector{};
		grad_ = Vector{};

		energy_ = 0.0;
		energyVar_ = 0.0;
	}

	void constructExact()
	{
		Vector st = getPsi(qs_, basis_, true);

		Vector k = std::visit([&st](auto&& arg) -> Vector { return arg*st; }, ham_);

		Scalar t = st.adjoint()*k;
		energy_ = std::real(t);
		energyVar_ = static_cast<Scalar>(k.adjoint()*k).real();
		energyVar_ -= energy_*energy_;

		SamplingResultExact srex(qs_, basis_);
		//deltas_ = constructDelta(qs_, srex);
		constructDelta(qs_, srex, deltas_);

		deltasPsis_ = st.cwiseAbs2().asDiagonal()*deltas_;
		oloc_ = deltasPsis_.colwise().sum();
		grad_ = (st.asDiagonal()*deltas_).adjoint()*k;
		grad_ -= t*oloc_.conjugate();
	}

	const Vector& oloc() const&
	{
		return oloc_;
	}
	Vector oloc() &&
	{
		return oloc_;
	}

	Matrix corrMat() const
	{
		Matrix res = deltas_.adjoint()*deltasPsis_;
		res -= oloc_.conjugate()*oloc_.transpose();
		return res;
	}

	const Vector& energyGrad() const&
	{
		return grad_;
	}

	Vector erengyGrad() &&
	{
		return grad_;
	}

	Vector apply(const Vector& rhs)
	{
		Vector res = deltas_.adjoint()*(deltasPsis_*rhs);
		res -= oloc_.conjugate()*(oloc_.transpose()*rhs);
		return res;
	}

	template<class Iterable, class ColFunc>
	SRMatExact(const Machine& qs, Iterable&& basis, ColFunc&& col)
	  : n_{qs.getN()}, qs_(qs)
	{
		tbb::parallel_for_each(basis.begin(), basis.end(),
				[&](uint32_t elt)
		{
			basis_.emplace_back(elt);
		});
		tbb::parallel_sort(basis_.begin(), basis_.end());
		ham_ = edp::constructSubspaceMat(std::forward<ColFunc>(col), basis_);
	}
};
} //namespace yannq


//   Copyright (c) 2014-2016 SSPA Sweden AB

#pragma once

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
namespace pysim {
    typedef boost::numeric::ublas::vector<double> vector;
    typedef boost::numeric::ublas::matrix<double> matrix;
};


#include <stdexcept>
#include "say.h"
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <iostream>
#include <algorithm>

using namespace std;
using namespace boost;

namespace say {

    std::map<unsigned long long int, std::string> num999 =
        {
        {1, "one"},
        {2, "two"},
        {3, "three"},
        {4, "four"},
        {5, "five"},
        {6, "six"},
        {7, "seven"},
        {8, "eight"},
        {9, "nine"},
        {10, "ten"},
        {11, "eleven"},
        {12, "twelve"},
        {13, "thirdteen"},
        {14, "fourteen"},
        {15, "fifteen"},
        {16, "sixteen"},
        {17, "seventeen"},
        {18, "eighteen"},
        {19, "nineteen"},
        {20, "twenty"},
        {30, "thirty"},
        {40, "forty"},
        {50, "fifty"},
        {60, "sixty"},
        {70, "seventy"},
        {80, "eighty"},
        {90, "ninety"},
        };

    string in_english_999(int num){
        if(num > 999)
            throw domain_error("");
        if(num == 0)
            return "zero";

        string result = "";
        auto it = num999.find(num);
        if(it == num999.end()){
            int h = int(num/100);
            int d = int((num%100)/10)*10;
            int s = num % 10;

            string hs = "";
            if(num >= 100){
                hs = num999[h] + " hundred";
            }
            string ds = num999[d];
            string ss = num999[s];

            result += hs;
            result += " " + ds;
            if(ss != ""){
                result += "-" + ss;
            }

            algorithm::trim(result);
            //cout << format("num: %d,  h=%d d=%d s=%d -> %s\n") % num % h % d % s % result;
        } else {
            result = it->second;
        }
        return result;
    }

    string in_english(unsigned long long num){
        if(num >= 1e+12){
            throw domain_error("");
        }
        int rest;
        int thousand;
        int million;
        int billion;
        unsigned long long r;

        billion = int(num / 1e9);
        r = num % int(1e9);
        million = int(r / 1e6);
        r = r % int(1e6);
        thousand = r / int(1e3);
        rest = num%1000;
        //cout << format("snum: %d, r %d, t %d, m %d, b %d\n") % num % rest % thousand % million % billion;

        string result = "";
        if(billion > 0){
            result += in_english_999(billion) + " billion";
        }
        if(million > 0){
            result += " " + in_english_999(million) + " million";
        }
         if(thousand > 0){
            result += " " +in_english_999(thousand) + " thousand";
        }
        if((result == "") or (rest > 0))
           result += " " + in_english_999(rest);

        algorithm::trim(result);
        return result;
    }
}


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

#include <iostream>
#include <algorithm>
#include <boost/numeric/ublas/tensor.hpp>

#include <boost/test/unit_test.hpp>

#include "utility.hpp"

BOOST_AUTO_TEST_SUITE ( test_einstein_notation, * boost::unit_test::depends_on("test_multi_index") )

using test_types = zip<int,long,float,double,std::complex<float>>::with_t<boost::numeric::ublas::first_order, boost::numeric::ublas::last_order>;

//using test_types = zip<int>::with_t<boost::numeric::ublas::first_order>;

BOOST_AUTO_TEST_CASE_TEMPLATE( test_einstein_multiplication, value,  test_types )
{
	using namespace boost::numeric::ublas;
	using value_type   = typename value::first_type;
	using layout_type  = typename value::second_type;
	using tensor_type  = tensor<value_type,layout_type>;
	using namespace boost::numeric::ublas::index;

	{
		auto A = tensor_type{5,3};
		auto B = tensor_type{3,4};
		//		auto C = tensor_type{4,5,6};

		for(auto j = 0u; j < A.extents().at(1); ++j)
			for(auto i = 0u; i < A.extents().at(0); ++i)
				A.at( i,j ) = value_type(i+1);

		for(auto j = 0u; j < B.extents().at(1); ++j)
			for(auto i = 0u; i < B.extents().at(0); ++i)
				B.at( i,j ) = value_type(i+1);

		auto AB = A(_,_e) * B(_e,_);

		//		std::cout << "A = " << A << std::endl;
		//		std::cout << "B = " << B << std::endl;
		//		std::cout << "AB = " << AB << std::endl;

		for(auto j = 0u; j < AB.extents().at(1); ++j)
			for(auto i = 0u; i < AB.extents().at(0); ++i)
				BOOST_CHECK_EQUAL( AB.at( i,j ) , value_type(A.at( i,0 ) * ( B.extents().at(0) * (B.extents().at(0)+1) / 2 )) );

	}

	{
		auto A = tensor_type{4,5,3};
		auto B = tensor_type{3,4,2};

		for(auto k = 0u; k < A.extents().at(2); ++k)
			for(auto j = 0u; j < A.extents().at(1); ++j)
				for(auto i = 0u; i < A.extents().at(0); ++i)
					A.at( i,j,k ) = value_type(i+1);

		for(auto k = 0u; k < B.extents().at(2); ++k)
			for(auto j = 0u; j < B.extents().at(1); ++j)
				for(auto i = 0u; i < B.extents().at(0); ++i)
					B.at( i,j,k ) = value_type(i+1);

		auto AB = A(_d,_,_f) * B(_f,_d,_);

		//		std::cout << "A = " << A << std::endl;
		//		std::cout << "B = " << B << std::endl;
		//		std::cout << "AB = " << AB << std::endl;
		// n*(n+1)/2;
		auto const nf = ( B.extents().at(0) * (B.extents().at(0)+1) / 2 );
		auto const nd = ( A.extents().at(0) * (A.extents().at(0)+1) / 2 );

		for(auto j = 0u; j < AB.extents().at(1); ++j)
			for(auto i = 0u; i < AB.extents().at(0); ++i)
				BOOST_CHECK_EQUAL( AB.at( i,j ) ,  value_type(nf * nd) );

	}

	{
		auto A = tensor_type{4,3};
		auto B = tensor_type{3,4,2};

		for(auto j = 0u; j < A.extents().at(1); ++j)
			for(auto i = 0u; i < A.extents().at(0); ++i)
				A.at( i,j ) = value_type(i+1);

		for(auto k = 0u; k < B.extents().at(2); ++k)
			for(auto j = 0u; j < B.extents().at(1); ++j)
				for(auto i = 0u; i < B.extents().at(0); ++i)
					B.at( i,j,k ) = value_type(i+1);

		auto AB = A(_d,_f) * B(_f,_d,_);

		// n*(n+1)/2;
		auto const nf = ( B.extents().at(0) * (B.extents().at(0)+1) / 2 );
		auto const nd = ( A.extents().at(0) * (A.extents().at(0)+1) / 2 );

		for(auto i = 0u; i < AB.extents().at(0); ++i)
			BOOST_CHECK_EQUAL ( AB.at( i  ) ,  value_type(nf * nd) );

	}
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright John Maddock 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_EXPM1_INCLUDED
#define BOOST_MATH_EXPM1_INCLUDED

#include <cmath>
#include <math.h> // platform's ::expm1
#include <boost/limits.hpp>
#include <boost/math/special_functions/detail/series.hpp>

#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
#  include <boost/static_assert.hpp>
#else
#  include <boost/assert.hpp>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::exp; using ::fabs; }
#endif

namespace boost{ namespace math{

namespace detail{
//
// Functor expm1_series returns the next term in the Taylor series
// x^k / k!
// each time that operator() is invoked.
//
template <class T>
struct expm1_series
{
   typedef T result_type;

   expm1_series(T x)
      : k(0), m_x(x), m_term(1) {}

   T operator()()
   {
      ++k;
      m_term *= m_x;
      m_term /= k;
      return m_term;
   }

   int count()const
   {
      return k;
   }

private:
   int k;
   const T m_x;
   T m_term;
   expm1_series(const expm1_series&);
   expm1_series& operator=(const expm1_series&);
};

} // namespace

//
// Algorithm expm1 is part of C99, but is not yet provided by many compilers.
//
// This version uses a Taylor series expansion for 0.5 > |x| > epsilon.
//
template <class T>
T expm1(T x)
{
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   BOOST_STATIC_ASSERT(::std::numeric_limits<T>::is_specialized);
#else
   BOOST_ASSERT(std::numeric_limits<T>::is_specialized);
#endif

   T a = std::fabs(x);
   if(a > T(0.5L))
      return std::exp(x) - T(1);
   if(a < std::numeric_limits<T>::epsilon())
      return x;
   detail::expm1_series<T> s(x);
   T result = detail::kahan_sum_series(s, std::numeric_limits<T>::digits + 2);
   return result;
}
#if BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x564))
inline float expm1(float z)
{
   return expm1<float>(z);
}
inline double expm1(double z)
{
   return expm1<double>(z);
}
inline long double expm1(long double z)
{
   return expm1<long double>(z);
}
#endif

#ifdef expm1
#  ifndef BOOST_HAS_expm1
#     define BOOST_HAS_expm1
#  endif
#  undef expm1
#endif

#ifdef BOOST_HAS_EXPM1
#  if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901)
inline float expm1(float x){ return ::expm1f(x); }
inline long double expm1(long double x){ return ::expm1l(x); }
#else
inline float expm1(float x){ return ::expm1(x); }
#endif
inline double expm1(double x){ return ::expm1(x); }
#endif

} } // namespaces

#endif // BOOST_MATH_HYPOT_INCLUDED


/* The Image Registration Toolkit (IRTK)
 *
 * Copyright 2008-2015 Imperial College London
 *
 * 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 <irtkImage.h>

#include <irtkNoise.h>

#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>

template <class VoxelType> irtkGaussianNoise<VoxelType>::irtkGaussianNoise() : irtkNoise<VoxelType>()
{
  _Mean   = 0;
  _Sigma  = 1;
  _MinVal = VoxelType(MIN_GREY);
  _MaxVal = VoxelType(MAX_GREY);

  long temp = -1 * this->_Init;

  boost::mt19937 rng;
  rng.seed(temp);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);
  (void) var_nor();
}

template <class VoxelType> irtkGaussianNoise<VoxelType>::irtkGaussianNoise(double Mean, double Sigma, VoxelType MinVal, VoxelType MaxVal) : irtkNoise<VoxelType>()
{
  this->_Mean   = Mean;
  this->_Sigma  = Sigma;
  this->_MinVal = MinVal;
  this->_MaxVal = MaxVal;

  long temp = -1 * this->_Init;

  boost::mt19937 rng;
  rng.seed(temp);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);
  (void) var_nor();
}

template <class VoxelType> double irtkGaussianNoise<VoxelType>::Run(int x, int y, int z, int t)
{
  boost::mt19937 rng;
  rng.seed(this->_Init);

  boost::normal_distribution<> nd(0, 1);
  boost::variate_generator<boost::mt19937&,
                           boost::normal_distribution<> > var_nor(rng, nd);

  double tmp = this->_input->Get(x, y, z, t) + this->_Sigma * var_nor() + this->_Mean;
  if (tmp < this->_MinVal) return this->_MinVal;
  if (tmp > this->_MaxVal) return this->_MaxVal;
  return tmp;
}

template class irtkGaussianNoise<irtkBytePixel>;
template class irtkGaussianNoise<irtkGreyPixel>;
template class irtkGaussianNoise<float>;
template class irtkGaussianNoise<double>;


// -----------------------------------------------------------
// integer_log2.hpp
//
//   Gives the integer part of the logarithm, in base 2, of a
// given number. Behavior is undefined if the argument is <= 0.
//
//        Copyright (c) 2003-2004, 2008 Gennaro Prota
//            Copyright (c) 2022 Andrey Semashev
//
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)
//
// -----------------------------------------------------------

#ifndef BOOST_INTEGER_INTEGER_LOG2_HPP
#define BOOST_INTEGER_INTEGER_LOG2_HPP

#include <climits>
#include <limits>
#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <boost/cstdint.hpp>
#include <boost/core/bit.hpp>
#include <boost/core/enable_if.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/make_unsigned.hpp>

namespace boost {
namespace detail {

// helper to find the maximum power of two
// less than p
template< unsigned int p, unsigned int n, bool = ((2u * n) < p) >
struct max_pow2_less :
    public max_pow2_less< p, 2u * n >
{
};

template< unsigned int p, unsigned int n >
struct max_pow2_less< p, n, false >
{
    BOOST_STATIC_CONSTANT(unsigned int, value = n);
};

template< typename T >
inline typename boost::disable_if< boost::is_integral< T >, int >::type integer_log2_impl(T x)
{
    unsigned int n = detail::max_pow2_less<
        std::numeric_limits< T >::digits,
        CHAR_BIT / 2u
    >::value;

    int result = 0;
    while (x != 1)
    {
        T t(x >> n);
        if (t)
        {
            result += static_cast< int >(n);
#if !defined(BOOST_NO_CXX11_RVALUE_REFERENCES)
            x = static_cast< T&& >(t);
#else
            x = t;
#endif
        }
        n >>= 1u;
    }

    return result;
}

template< typename T >
inline typename boost::enable_if< boost::is_integral< T >, int >::type integer_log2_impl(T x)
{
    // We could simply rely on numeric_limits but sometimes
    // Borland tries to use numeric_limits<const T>, because
    // of its usual const-related problems in argument deduction
    // - gps
    return static_cast< int >((sizeof(T) * CHAR_BIT - 1u) -
        boost::core::countl_zero(static_cast< typename boost::make_unsigned< T >::type >(x)));
}

#if defined(BOOST_HAS_INT128)
// We need to provide explicit overloads for __int128 because (a) boost/core/bit.hpp currently does not support it and
// (b) std::numeric_limits are not specialized for __int128 in some standard libraries.
inline int integer_log2_impl(boost::uint128_type x)
{
    const boost::uint64_t x_hi = static_cast< boost::uint64_t >(x >> 64u);
    if (x_hi != 0u)
        return 127 - boost::core::countl_zero(x_hi);
    else
        return 63 - boost::core::countl_zero(static_cast< boost::uint64_t >(x));
}

inline int integer_log2_impl(boost::int128_type x)
{
    return detail::integer_log2_impl(static_cast< boost::uint128_type >(x));
}
#endif // defined(BOOST_HAS_INT128)

} // namespace detail

// ------------
// integer_log2
// ------------
template< typename T >
inline int integer_log2(T x)
{
    BOOST_ASSERT(x > 0);
    return detail::integer_log2_impl(x);
}

} // namespace boost

#endif // BOOST_INTEGER_INTEGER_LOG2_HPP


#include <frovedis.hpp>
#include <frovedis/ml/glm/linear_regression_with_sgd.hpp>
#include <frovedis/ml/glm/ridge_regression_with_sgd.hpp>
#include <frovedis/ml/glm/lasso_with_sgd.hpp>
#include <boost/lexical_cast.hpp>

double parse(const std::string& s){return boost::lexical_cast<double>(s);}

int main(int argc, char* argv[]){
  frovedis::use_frovedis use(argc, argv);

  auto samples = frovedis::make_crs_matrix_load<double>("./train.mat");
  auto label = frovedis::make_dvector_loadline("./train.label").map(parse);

  int num_iteration = 1000;
  double alpha = 0.00000001;
  double minibatch_fraction = 1.0;
  bool intercept = true;
  double convTol = 0;
  frovedis::MatType mType = frovedis::CRS;
  auto model = frovedis::linear_regression_with_sgd::
    train(samples, label, num_iteration, alpha, minibatch_fraction,
          intercept, convTol, mType);
  /*
  // L2 regularization
  double regParam = 0.001;
  auto model = frovedis::ridge_regression_with_sgd::
    train(samples, label, num_iteration, alpha, minibatch_fraction,
          regParam, intercept, convTol, mType);
  */
  /*
  // L1 regularization
  double regParam = 0.0001;
  auto model = frovedis::lasso_with_sgd::
    train(samples, label, num_iteration, alpha, minibatch_fraction,
          regParam, intercept, convTol, mType);
  */
  model.save("./model");

  frovedis::linear_regression_model<double> lm;
  lm.load("./model");
  auto test = frovedis::make_crs_matrix_local_load<double>("./test.mat");
  auto result = lm.predict(test);
  for(auto i: result) std::cout << i << std::endl;
}


/**
 * \file libsanm/tensor_linalg.cpp
 * This file is part of SANM, a symbolic asymptotic numerical solver.
 */

#include "libsanm/tensor_impl_helper.h"

#include <Eigen/Dense>

using namespace sanm;

namespace {
/*!
 * \brief compute cofactor of a single matrix
 * \param s_prod_rev a workspace of size dim+1, and the last value must be
 *      initialized to one
 */
template <int mat_dim>
void compute_cofactor(EigenMat<mat_dim, mat_dim>& mdst,
                      EigenMat<mat_dim, mat_dim>& msrc, fp_t* s_prod_rev) {
    Eigen::Index dim = msrc.rows();

    Eigen::JacobiSVD<Eigen::Matrix<fp_t, mat_dim, mat_dim>> svd{
            msrc, Eigen::ComputeFullU | Eigen::ComputeFullV};

    if (svd.rank() + 2 <= dim) {
        mdst.setZero();
        return;
    }

    // det(S) * S.inv()
    Eigen::Matrix<fp_t, mat_dim, 1> sinvd = svd.singularValues();
    if constexpr (mat_dim == 2) {
        fp_t a = sinvd(0), b = sinvd(1);
        sinvd(0) = b;
        sinvd(1) = a;
    } else if constexpr (mat_dim == 3) {
        fp_t a = sinvd(0), b = sinvd(1), c = sinvd(2);
        sinvd(0) = b * c;
        sinvd(1) = a * c;
        sinvd(2) = a * b;
    } else {
        for (int i = dim - 1; i >= 0; --i) {
            s_prod_rev[i] = s_prod_rev[i + 1] * sinvd[i];
        }
        fp_t prod = 1;
        for (Eigen::Index i = 0; i < dim; ++i) {
            fp_t orig = sinvd(i);
            sinvd(i) = prod * s_prod_rev[i + 1];
            prod *= orig;
        }
    }

    fp_t sign = (svd.matrixU() * svd.matrixV().transpose()).determinant();
    if (sign < 0) {
        sinvd = -sinvd;
    }
    mdst = svd.matrixU() * sinvd.asDiagonal() * svd.matrixV().transpose();
}

template <typename T>
constexpr T get_from_pair(int d, T x, T y) {
    return d == 0 ? x : y;
}

CBLAS_TRANSPOSE cblas_trans(bool t) {
    return t ? CblasTrans : CblasNoTrans;
}
}  // anonymous namespace

TensorND& TensorND::as_mm(const TensorND& lhs, const TensorND& rhs, bool accum,
                          bool trans_lhs, bool trans_rhs) {
    SANM_SCOPED_PROFILER("mm");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(lhs.rank() == 2 && rhs.rank() == 2);
    TensorShape dst_shape{
            get_from_pair(trans_lhs, lhs.shape(0), lhs.shape(1)),
            get_from_pair(trans_rhs ^ 1, rhs.shape(0), rhs.shape(1))};

    if (accum) {
        sanm_assert(shape() == dst_shape, "mm accum: expect shape %s, got %s",
                    dst_shape.str().c_str(), shape().str().c_str());

        if (lhs.is_zero() || rhs.is_zero()) {
            return *this;
        }
    } else {
        if (lhs.is_zero() || rhs.is_zero()) {
            // do not use set_shape to avoid unnecessary memory allocation
            m_shape = dst_shape;
            return fill_with_inplace(0);
        }
        set_shape(dst_shape);
    }

    static_assert(std::is_same_v<fp_t, double>, "unhandled fp_t");
    if constexpr (std::is_same_v<fp_t, double>) {
        cblas_dgemm(CblasRowMajor, cblas_trans(trans_lhs),
                    cblas_trans(trans_rhs), dst_shape[0], dst_shape[1],
                    get_from_pair(trans_lhs ^ 1, lhs.shape(0), lhs.shape(1)), 1,
                    lhs.ptr(), lhs.shape(1), rhs.ptr(), rhs.shape(1),
                    accum ? 1 : 0, rwptr(), dst_shape[1]);
    }
    return *this;
}

TensorND& TensorND::as_batched_mm(const TensorND& lhs, const TensorND& rhs,
                                  bool accum, bool trans_lhs, bool trans_rhs) {
    SANM_SCOPED_PROFILER("batched_mm");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(
            lhs.rank() == 3 && rhs.rank() == 3 && lhs.shape(0) == rhs.shape(0),
            "batched mm shape mismatch %s vs %s", lhs.shape().str().c_str(),
            rhs.shape().str().c_str());
    const size_t ls0 = lhs.shape(1), ls1 = lhs.shape(2), rs0 = rhs.shape(1),
                 rs1 = rhs.shape(2), batch = lhs.shape(0);
    sanm_assert(get_from_pair(trans_lhs ^ 1, ls0, ls1) ==
                        get_from_pair(trans_rhs, rs0, rs1),
                "matmul shape mismatch: %s vs %s, trans=%d,%d",
                lhs.shape().str().c_str(), rhs.shape().str().c_str(), trans_lhs,
                trans_rhs);

    TensorShape dst_shape{batch, get_from_pair(trans_lhs, ls0, ls1),
                          get_from_pair(trans_rhs ^ 1, rs0, rs1)};

    if (accum) {
        sanm_assert(shape() == dst_shape,
                    "batched_mm accum: expect shape %s, got %s",
                    dst_shape.str().c_str(), shape().str().c_str());

        if (lhs.is_zero() || rhs.is_zero()) {
            return *this;
        }
    } else {
        if (lhs.is_zero() || rhs.is_zero()) {
            // do not use set_shape to avoid unnecessary memory allocation
            m_shape = dst_shape;
            return fill_with_inplace(0);
        }
        set_shape(dst_shape);
    }

    // As of 2020, MKL batched dgemm is still slow for small matrices ...
    // So we roll out our simple accelerated impl using eigen

    auto pa = const_cast<fp_t*>(lhs.ptr()), pb = const_cast<fp_t*>(rhs.ptr()),
         pc = rwptr();
    auto run_static_shape = [pa, pb, pc, batch, accum, trans_lhs,
                             trans_rhs]<int ls0, int ls1, int rs0, int rs1>() {

    // eigen uses col-major; so we compute on the transformed matrices
#define FOREACH_8(cb)                                                       \
    cb(0, 0, 0) cb(0, 0, 1) cb(0, 1, 0) cb(0, 1, 1) cb(1, 0, 0) cb(1, 0, 1) \
            cb(1, 1, 0) cb(1, 1, 1)
#define ACC0 =
#define ACC1 +=
#define TR0(x) x
#define TR1(x) x.transpose()
#define CB(acc, tra, trb)                                               \
    if constexpr (get_from_pair(tra ^ 1, ls0, ls1) ==                   \
                  get_from_pair(trb, rs0, rs1)) {                       \
        constexpr int cs0 = get_from_pair(tra, ls0, ls1),               \
                      cs1 = get_from_pair(trb ^ 1, rs0, rs1);           \
        if (accum == acc && trans_lhs == tra && trans_rhs == trb) {     \
            for (size_t ib = 0; ib < batch; ++ib) {                     \
                EigenMat<ls1, ls0> ta{pa + ib * (ls0 * ls1), ls1, ls0}; \
                EigenMat<rs1, rs0> tb{pb + ib * (rs0 * rs1), rs1, rs0}; \
                EigenMat<cs1, cs0> tc{pc + ib * (cs0 * cs1), cs1, cs0}; \
                tc.noalias() ACC##acc TR##trb(tb) * TR##tra(ta);        \
            }                                                           \
            return;                                                     \
        }                                                               \
    }
        FOREACH_8(CB)
        sanm_assert(0, "impossible");
#undef CB
#undef TR1
#undef TR0
#undef ACC1
#undef ACC0
#undef FOREACH_8
    };

#define CASE(ls0_, ls1_, rs0_, rs1_)                                    \
    do {                                                                \
        if (ls0_ == ls0 && ls1_ == ls1 && rs0_ == rs0 && rs1_ == rs1) { \
            run_static_shape.operator()<ls0_, ls1_, rs0_, rs1_>();      \
            return *this;                                               \
        }                                                               \
    } while (0)

    CASE(1, 1, 1, 1);
    CASE(2, 2, 2, 2);
    CASE(3, 3, 3, 3);
    CASE(4, 4, 4, 4);

    // special handling of shapes can be added here
#undef CASE

    static_assert(std::is_same_v<fp_t, double>, "unhandled fp_t");
    if constexpr (std::is_same_v<fp_t, double>) {
        cblas_dgemm_batch_strided(
                CblasRowMajor, cblas_trans(trans_lhs), cblas_trans(trans_rhs),
                dst_shape[1], dst_shape[2],
                get_from_pair(trans_lhs ^ 1, ls0, ls1), 1, lhs.ptr(), ls1,
                ls0 * ls1, rhs.ptr(), rs1, rs0 * rs1, accum ? 1 : 0, rwptr(),
                dst_shape[2], dst_shape[1] * dst_shape[2], batch);
    }
    return *this;
}

TensorND& TensorND::as_batched_transpose(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_transpose");
    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3);
    {
        TensorShape oshp = src.shape();
        std::swap(oshp.dim[1], oshp.dim[2]);
        set_shape(oshp);
    }
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim0, int mat_dim1>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim0 = src.shape(1), dim1 = src.shape(2);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim0, mat_dim1> mdst{dptr + i * dim0 * dim1, dim0,
                                              dim1};
            EigenMat<mat_dim1, mat_dim0> msrc{sptr + i * dim0 * dim1, dim1,
                                              dim0};
            mdst = msrc.transpose();
        }
    };

    auto work_dispatch = [&work, &src]<int mat_dim0>() {
        switch (src.shape(2)) {
#define ON(x)                           \
    case x:                             \
        work.operator()<mat_dim0, x>(); \
        break
            ON(1);
            ON(2);
            ON(3);
            ON(4);
#undef ON
            default:
                work.operator()<mat_dim0, Eigen::Dynamic>();
                break;
        }
    };

    switch (src.shape(1)) {
#define ON(x)                          \
    case x:                            \
        work_dispatch.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work_dispatch.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_transpose(const TensorND& src) {
    sanm_assert(src.rank() == 2);
    size_t m = src.shape(0), n = src.shape(1);
    set_shape({n, m});
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto ei = [](size_t x) -> Eigen::Index { return x; };
    EigenMatDyn mdst{woptr(), ei(m), ei(n)},
            msrc{const_cast<fp_t*>(src.ptr()), ei(n), ei(m)};
    mdst = msrc.transpose();
    return *this;
}

TensorND& TensorND::as_batched_matinv(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_matinv");
    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2));
    sanm_assert(!src.is_zero());
    set_shape(src.shape());
    auto work = [this, &src]<int mat_dim>() {
        ScopedAllowMalloc scoped_allow_malloc;
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> mdst{dptr + i * dim * dim, dim, dim},
                    msrc{sptr + i * dim * dim, dim, dim};
            mdst = msrc.inverse();
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_determinant(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_det");
    ScopedAllowMalloc allow_mem_alloc;

    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2));
    set_shape({src.shape(0), 1});
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> msrc{sptr + i * dim * dim, dim, dim};
            dptr[i] = msrc.determinant();
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(1);
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_cofactor(const TensorND& src) {
    SANM_SCOPED_PROFILER("batched_cofactor");
    ScopedAllowMalloc allow_mem_alloc;

    sanm_assert(this != &src);
    sanm_assert(src.rank() == 3 && src.shape(1) == src.shape(2) &&
                src.shape(1) >= 2);
    set_shape(src.shape());
    if (src.is_zero()) {
        return fill_with_inplace(0);
    }
    auto work = [this, &src]<int mat_dim>() {
        auto sptr = const_cast<fp_t*>(src.ptr()), dptr = this->woptr();
        const size_t batch = src.shape(0);
        const Eigen::Index dim = src.shape(1);
        std::unique_ptr<fp_t[]> s_prod_rev{new fp_t[dim + 1]};
        s_prod_rev[dim] = 1;
        for (size_t i = 0; i < batch; ++i) {
            EigenMat<mat_dim, mat_dim> mdst{dptr + i * dim * dim, dim, dim},
                    msrc{sptr + i * dim * dim, dim, dim};
            compute_cofactor(mdst, msrc, s_prod_rev.get());
        }
    };
    switch (src.shape(1)) {
#define ON(x)                 \
    case x:                   \
        work.operator()<x>(); \
        break
        ON(2);
        ON(3);
        ON(4);
#undef ON
        default:
            work.operator()<Eigen::Dynamic>();
            break;
    }
    return *this;
}

TensorND& TensorND::as_batched_mm_vecitem_left(const TensorND& lhs,
                                               const TensorND& rhs,
                                               bool accum) {
    SANM_SCOPED_PROFILER("batched_mm_vecitem");
    sanm_assert(this != &lhs && this != &rhs);
    sanm_assert(lhs.rank() == 3 && rhs.rank() == 3);
    size_t BATCH = lhs.shape(0), M = lhs.shape(1), P = lhs.shape(2),
           K = rhs.shape(1), N = rhs.shape(2);
    sanm_assert(M % K == 0 && BATCH == rhs.shape(0));
    M /= K;

    TensorShape expect_shape{BATCH, M * N, P};
    if (accum) {
        sanm_assert(m_shape == expect_shape);
    } else {
        set_shape(expect_shape);
    }

    if (lhs.is_zero() || rhs.is_zero()) {
        return fill_with_inplace(0);
    }

    auto ei = [](size_t x) -> Eigen::Index { return x; };

    auto lptr = const_cast<fp_t*>(lhs.ptr()),
         rptr = const_cast<fp_t*>(rhs.ptr()), optr = this->woptr();
    for (size_t b = 0; b < BATCH; ++b) {
        EigenMatDyn mat_rhs{rptr + b * K * N, ei(N), ei(K)};
        for (size_t m = 0; m < M; ++m) {
            EigenMatDyn mat_lhs{lptr + (b * M + m) * K * P, ei(P), ei(K)},
                    mat_dst{optr + (b * M + m) * N * P, ei(P), ei(N)};
            if (accum) {
                mat_dst.noalias() += mat_lhs * mat_rhs.transpose();
            } else {
                mat_dst.noalias() = mat_lhs * mat_rhs.transpose();
            }
        }
    }

    return *this;
}


#include <cstdlib>
#include <iostream>
#include <complex>
#include <type_traits>
#include <algorithm>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/bindings/trans.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/banded.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::banded_matrix<complex, ublas::column_major> matrix;
    typedef typename std::make_signed<vector::size_type>::type size_type;
    rand_normal<complex>::reset();
    size_type m=6, n=8;
    matrix A(m, n, 1, 2);
    for (size_type j=0; j<n; ++j)
      for (size_type i=std::max(j-2, size_type(0)); i<std::min(j+1+1, m); ++i)
 	A(i, j)=rand_normal<complex>::get();
    // A banded ublas matrux and its transpose have very different
    // memory layouts, thus the following line does not work.
    // matrix A_t(ublas::trans(A));
    matrix A_t(n, m, 2, 1);
    for (size_type j=0; j<m; ++j)
      for (size_type i=std::max(j-1, size_type(0)); i<std::min(j+2+1, n); ++i)
    	A_t(i, j)=A(j, i);
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(m);
    for (size_type i=0; i<m; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    complex beta(rand_normal<complex>::get());
    vector y1(alpha*ublas::prod(A, x)+beta*y);
    vector y2(y);
    blas::gbmv(alpha, A, x, beta, y2);
    vector y3(y);
    blas::gbmv(alpha, blas::trans(A_t), x, beta, y3);
    std::cout << "testing boost::ublas containers\n"
	      << "using ublas           : " << print_vec(y1) << '\n'
	      << "using blas            : " << print_vec(y2) << '\n'
	      << "using blas (tranposed): " << print_vec(y3) << '\n'
	      << '\n';
  }
  return EXIT_SUCCESS;
}


#include <boost/numeric/ublas/vector.hpp>
#include <iostream>
#include <boost/numeric/ublas/io.hpp>

int main()
{
	boost::numeric::ublas::vector<float> vector(3);
	vector = boost::numeric::ublas::zero_vector<float> ();

	std::cerr << vector << std::endl;

	return 0;
}


/*
 * phase_oscillator_ensemble.cpp
 *
 * Demonstrates the phase transition from an unsynchronized to an synchronized state.
 *
 * Copyright 2011-2012 Karsten Ahnert
 * Copyright 2011-2012 Mario Mulansky
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 */

#include <iostream>
#include <utility>

#include <boost/numeric/odeint.hpp>

#ifndef M_PI //not there on windows
#define M_PI 3.141592653589793 //...
#endif

#include <boost/random.hpp>

using namespace std;
using namespace boost::numeric::odeint;

//[ phase_oscillator_ensemble_system_function
typedef vector< double > container_type;

pair< double , double > calc_mean_field( const container_type &x )
{
    size_t n = x.size();
    double cos_sum = 0.0 , sin_sum = 0.0;
    for( size_t i=0 ; i<n ; ++i )
    {
        cos_sum += cos( x[i] );
        sin_sum += sin( x[i] );
    }
    cos_sum /= double( n );
    sin_sum /= double( n );

    double K = sqrt( cos_sum * cos_sum + sin_sum * sin_sum );
    double Theta = atan2( sin_sum , cos_sum );

    return make_pair( K , Theta );
}

struct phase_ensemble
{
    container_type m_omega;
    double m_epsilon;

    phase_ensemble( const size_t n , double g = 1.0 , double epsilon = 1.0 )
    : m_omega( n , 0.0 ) , m_epsilon( epsilon )
    {
        create_frequencies( g );
    }

    void create_frequencies( double g )
    {
        boost::mt19937 rng;
        boost::cauchy_distribution<> cauchy( 0.0 , g );
        boost::variate_generator< boost::mt19937&, boost::cauchy_distribution<> > gen( rng , cauchy );
        generate( m_omega.begin() , m_omega.end() , gen );
    }

    void set_epsilon( double epsilon ) { m_epsilon = epsilon; }

    double get_epsilon( void ) const { return m_epsilon; }

    void operator()( const container_type &x , container_type &dxdt , double /* t */ ) const
    {
        pair< double , double > mean = calc_mean_field( x );
        for( size_t i=0 ; i<x.size() ; ++i )
            dxdt[i] = m_omega[i] + m_epsilon * mean.first * sin( mean.second - x[i] );
    }
};
//]

//[ phase_oscillator_ensemble_observer
struct statistics_observer
{
    double m_K_mean;
    size_t m_count;

    statistics_observer( void )
    : m_K_mean( 0.0 ) , m_count( 0 ) { }

    template< class State >
    void operator()( const State &x , double t )
    {
        pair< double , double > mean = calc_mean_field( x );
        m_K_mean += mean.first;
        ++m_count;
    }

    double get_K_mean( void ) const { return ( m_count != 0 ) ? m_K_mean / double( m_count ) : 0.0 ; }

    void reset( void ) { m_K_mean = 0.0; m_count = 0; }
};
//]

int main( int argc , char **argv )
{
    //[ phase_oscillator_ensemble_integration
    const size_t n = 16384;
    const double dt = 0.1;

    container_type x( n );

    boost::mt19937 rng;
    boost::uniform_real<> unif( 0.0 , 2.0 * M_PI );
    boost::variate_generator< boost::mt19937&, boost::uniform_real<> > gen( rng , unif );

    // gamma = 1, the phase transition occurs at epsilon = 2
    phase_ensemble ensemble( n , 1.0 );
    statistics_observer obs;

    for( double epsilon = 0.0 ; epsilon < 5.0 ; epsilon += 0.1 )
    {
        ensemble.set_epsilon( epsilon );
        obs.reset();

        // start with random initial conditions
        generate( x.begin() , x.end() , gen );

        // calculate some transients steps
        integrate_const( runge_kutta4< container_type >() , boost::ref( ensemble ) , x , 0.0 , 10.0 , dt );

        // integrate and compute the statistics
        integrate_const( runge_kutta4< container_type >() , boost::ref( ensemble ) , x , 0.0 , 100.0 , dt , boost::ref( obs ) );
        cout << epsilon << "\t" << obs.get_K_mean() << endl;
    }

    //]

    return 0;
}


#include <iostream>
#include <vector>
using namespace std;

#include <boost/generator_iterator.hpp>
#include <boost/random.hpp>
#include <boost/timer/timer.hpp>

#include <cpp_algs.hpp>

template <typename T>
void init_matrices(vector<vector<vector<T>>> &, const vector<int> &, const T &, bool = false);

void time_test(const vector<int> &);

int main() {
    // Sorting
    vector<int> v = {541, 12, 56, 62, 1234, 656547, 123, 1, 6546, 51, 1334, 56612};
    al::heapSort(v);
    BOOST_ASSERT_MSG(std::is_sorted(v.begin(), v.end()), "sorting buggy");

    // Stack
    ds::Stack<int> s1;
    s1.push(10);
    s1.push(20);

    ds::Stack<int> s_copy;
    s_copy = s1;

    ds::Stack<int> s_copy2(s1);

    // Singly Linked List
    ds::SinglyLinkedList<int> sing;
    BOOST_ASSERT_MSG(sing.size() == 0, "Singly linked list size() is buggy");
    sing.insertNode(10);
    BOOST_ASSERT_MSG(sing.contains(10), "Singly linked list insertion is buggy");
    sing.insertNode(20);
    BOOST_ASSERT_MSG(sing.contains(20), "Singly linked list search is buggy");
    sing.deleteNode(20);
    BOOST_ASSERT_MSG(!sing.contains(20), "Singly linked list deletion is buggy");

    vector<int> vec = {43, 44, 45, 46, 47};
    sing.insertArray(vec);
    for (const auto &v : vec) {
        BOOST_ASSERT_MSG(sing.contains(v), "Singly linked list insertion of std::vector<int> is buggy");
    }
    BOOST_ASSERT_MSG(sing.size() == 6, "Singly linked list size() is buggy");

    ds::SinglyLinkedList<string> s = ds::SinglyLinkedList<string>();
    BOOST_ASSERT_MSG(s.size() == 0, "Singly linked list size() is buggy");
    s.insertNode("hello");
    BOOST_ASSERT_MSG(s.contains("hello"), "Singly linked list std::string insertion is buggy");
    s.insertNode("world");
    BOOST_ASSERT_MSG(s.contains("world"), "Singly linked list std::string contains is buggy");
    s.deleteNode("hello");
    BOOST_ASSERT_MSG(!s.contains("hello"), "Singly linked list std::string deletion is buggy");
    s.insertNode("!");
    BOOST_ASSERT_MSG(s.search("!")->data == "!", "Singly linked list std::string search is buggy");
    BOOST_ASSERT_MSG(s.size() == 2, "Singly linked list size() is buggy");

    // Matrix Chain Multiplication
    vector<int> dims = {6, 4, 2, 1};

    vector<vector<vector<float>>> matrices;
    init_matrices<float>(matrices, dims, 1.0f);

    vector<vector<float>> out = al::chain_matmul<float>(matrices);

    for (int i = 0; i < out.size(); i++) {
        for (int j = 0; j < out[0].size(); j++) {
            BOOST_ASSERT_MSG(out[i][j] == 8, "Matrix chain multiplication is buggy");
        }
    }

    // Trie dictionary
    ds::Trie trie;

    trie.insertWord("hello");
    BOOST_ASSERT_MSG(trie.containsWord("hello"), "Trie std::string insertion is buggy");
    trie.insertWord("help");
    BOOST_ASSERT_MSG(trie.containsWord("help"), "Trie std::string insertion is buggy");
    trie.insertWord("hell");
    BOOST_ASSERT_MSG(trie.containsWord("hell"), "Trie std::string insertion is buggy");

    char c[] = {'a', 'b', 'c', '\0'};
    trie.insertWord(c);
    BOOST_ASSERT_MSG(trie.containsWord("abc"), "Trie (char *) insertion is buggy");

    trie.removeWord("hello");
    BOOST_ASSERT_MSG(!trie.containsWord("hello"), "Trie std::string removal is buggy");
    trie.removeWord(c);
    BOOST_ASSERT_MSG(!trie.containsWord(c), "Trie (char *) removal is buggy");

    cout << "Verified installation of library!" << '\n';

    dims = {1000, 1234, 152, 13, 542, 122, 11, 424};
    time_test(dims);

    // cleanup
    return 0;
}

template <typename T>
void init_matrices(vector<vector<vector<T>>> &matrices, const vector<int> &dims, const T &data, bool random) {
    typedef boost::mt19937 RNGType;
    RNGType rng;
    boost::uniform_int<> random_iter(1, 10);
    boost::variate_generator<RNGType, boost::uniform_int<>> dice(rng, random_iter);

    for (int i = 0; i < dims.size() - 1; i++) {
        if (random) {
            vector<vector<T>> matrix(dims[i], vector<T>(dims[i + 1], dice()));
            matrices.push_back(matrix);
        } else {
            vector<vector<T>> matrix(dims[i], vector<T>(dims[i + 1], data));
            matrices.push_back(matrix);
        }
    }
}

void time_test(const vector<int> &dims) {
    vector<vector<vector<int>>> matrices;
    init_matrices<int>(matrices, dims, 1, true);
    vector<vector<int>> out;
    boost::timer::cpu_timer t;

    // matrix chain using DP
    t.start();
    out = al::chain_matmul<int>(matrices);
    t.stop();
    boost::timer::cpu_times const et1(t.elapsed());
    cout << boost::timer::format(et1) << endl;

    out.clear();

    // naive matrix chain
    out = matrices[0];
    t.start();
    for (int i = 1; i < matrices.size(); i++) {
        out = al::matmul(out, matrices[i]);
    }
    t.stop();
    boost::timer::cpu_times const et2(t.elapsed());
    cout << boost::timer::format(et2);
}


//  Boost static_min_max.hpp test program  -----------------------------------//

//  (C) Copyright Daryle Walker 2001.  Permission to copy, use, modify, sell
//  and distribute this software is granted provided this copyright
//  notice appears in all copies.  This software is provided "as is" without
//  express or implied warranty, and with no claim as to its suitability for
//  any purpose.

//  See http://www.boost.org for most recent version including documentation.

//  Revision History
//  23 Sep 2001  Initial version (Daryle Walker)

#define  BOOST_INCLUDE_MAIN
#include <boost/test/test_tools.hpp>  // for main, BOOST_TEST

#include <boost/cstdlib.hpp>                 // for boost::exit_success
#include <boost/integer/static_min_max.hpp>  // for boost::static_signed_min, etc.

#include <iostream>  // for std::cout (std::endl indirectly)

// Main testing function
int
test_main
(
    int         ,   // "argc" is unused
    char *      []  // "argv" is unused
)
{
    using std::cout;
    using std::endl;
    using boost::static_signed_min;
    using boost::static_signed_max;
    using boost::static_unsigned_min;
    using boost::static_unsigned_max;

    // Two positives
    cout << "Doing tests with two positive values." << endl;

    BOOST_TEST( (static_signed_min< 9, 14>::value) ==  9 );
    BOOST_TEST( (static_signed_max< 9, 14>::value) == 14 );
    BOOST_TEST( (static_signed_min<14,  9>::value) ==  9 );
    BOOST_TEST( (static_signed_max<14,  9>::value) == 14 );

    BOOST_TEST( (static_unsigned_min< 9, 14>::value) ==  9 );
    BOOST_TEST( (static_unsigned_max< 9, 14>::value) == 14 );
    BOOST_TEST( (static_unsigned_min<14,  9>::value) ==  9 );
    BOOST_TEST( (static_unsigned_max<14,  9>::value) == 14 );

    // Two negatives
    cout << "Doing tests with two negative values." << endl;

    BOOST_TEST( (static_signed_min<  -8, -101>::value) == -101 );
    BOOST_TEST( (static_signed_max<  -8, -101>::value) ==   -8 );
    BOOST_TEST( (static_signed_min<-101,   -8>::value) == -101 );
    BOOST_TEST( (static_signed_max<-101,   -8>::value) ==   -8 );

    // With zero
    cout << "Doing tests with zero and a positive or negative value." << endl;

    BOOST_TEST( (static_signed_min< 0, 14>::value) ==  0 );
    BOOST_TEST( (static_signed_max< 0, 14>::value) == 14 );
    BOOST_TEST( (static_signed_min<14,  0>::value) ==  0 );
    BOOST_TEST( (static_signed_max<14,  0>::value) == 14 );

    BOOST_TEST( (static_unsigned_min< 0, 14>::value) ==  0 );
    BOOST_TEST( (static_unsigned_max< 0, 14>::value) == 14 );
    BOOST_TEST( (static_unsigned_min<14,  0>::value) ==  0 );
    BOOST_TEST( (static_unsigned_max<14,  0>::value) == 14 );

    BOOST_TEST( (static_signed_min<   0, -101>::value) == -101 );
    BOOST_TEST( (static_signed_max<   0, -101>::value) ==    0 );
    BOOST_TEST( (static_signed_min<-101,    0>::value) == -101 );
    BOOST_TEST( (static_signed_max<-101,    0>::value) ==    0 );

    // With identical
    cout << "Doing tests with two identical values." << endl;

    BOOST_TEST( (static_signed_min<0, 0>::value) == 0 );
    BOOST_TEST( (static_signed_max<0, 0>::value) == 0 );
    BOOST_TEST( (static_unsigned_min<0, 0>::value) == 0 );
    BOOST_TEST( (static_unsigned_max<0, 0>::value) == 0 );

    BOOST_TEST( (static_signed_min<14, 14>::value) == 14 );
    BOOST_TEST( (static_signed_max<14, 14>::value) == 14 );
    BOOST_TEST( (static_unsigned_min<14, 14>::value) == 14 );
    BOOST_TEST( (static_unsigned_max<14, 14>::value) == 14 );

    BOOST_TEST( (static_signed_min< -101, -101>::value) == -101 );
    BOOST_TEST( (static_signed_max< -101, -101>::value) == -101 );

    return boost::exit_success;
}


/*
 * Copyright (c) 2011-2014 Burkhard Ritter
 * This code is distributed under the two-clause BSD License.
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE system test
#include <boost/test/unit_test.hpp>

#include <ctime>
#include "basis.hpp"
#include "system.hpp"

template<class System>
class HubbardHamiltonian : public Hamiltonian<System>
{
public:
    SMatrix& H;
    System& s;

public:
    HubbardHamiltonian (System& s_)
        : Hamiltonian<System>(s_), H(Hamiltonian<System>::H), s(s_)
    {}

    void construct ()
    {
        H = SMatrix(s.basis.size(), s.basis.size());
        H.setZero();
        for (size_t i=0; i<s.N_sites; i++)
        {
            for (int spin=0; spin<2; spin++)
            {
                H += s.t * s.creator(2*i+spin) * s.annihilator(2* ((i+1)%s.N_sites) +spin);
                H += s.t * s.creator(2* ((i+1)%s.N_sites) +spin) * s.annihilator(2*i+spin);
            }
            H += s.U * s.creator(2*i) * s.annihilator(2*i) *
                       s.creator(2*i+1) * s.annihilator(2*i+1);
        }
    }
};

class HubbardSystem
{
public:
    typedef HubbardSystem Self;

    size_t N_sites;
    size_t N_orbitals;

    Basis basis;
    Creator<Self> creator;
    Annihilator<Self> annihilator;
    HubbardHamiltonian<Self> H;
    EnsembleAverage<Self> measure;

    ParticleNumberSymmetryOperator N;
    SpinSymmetryOperator S;
    bool exploitSymmetries;
    double t, U;

public:
    HubbardSystem (size_t N_sites_, bool exploitSymmetries_ = false)
        : N_sites(N_sites_), N_orbitals(2*N_sites_),
          creator(*this), annihilator(*this), H(*this), measure(*this),
          exploitSymmetries(exploitSymmetries_), t(1), U(10)
    {
        if (exploitSymmetries)
        {
            basis.addSymmetryOperator(&N);
            basis.addSymmetryOperator(&S);
        }
        construct();
    }

    void construct ()
    {
        basis.construct(N_orbitals);
        creator.construct();
        annihilator.construct();
    }

    void update()
    {
        H.construct();
        H.diagonalize();
    }
};

template<class System>
class DoubleOccupancy
{
private:
    const System& s;

public:
    DoubleOccupancy (const System& s_)
    : s(s_)
    {}

    SMatrix operator() (size_t site) const
    {
        return
            s.creator(2*site) * s.annihilator(2*site) *
            s.creator(2*site+1) * s.annihilator(2*site+1);
    }
};

bool epsilonEqual (double v, double w, double epsilon = 10E-10)
{
    return fabs(v-w) < epsilon;
}

BOOST_AUTO_TEST_CASE ( construct_system_without_symmetries )
{
    HubbardSystem s(2);
    s.H.construct();
    BOOST_CHECK (s.basis.getRanges().size() == 1);

    s.H.diagonalize();
    DVector eigenvalues = s.H.eigenvalues();
    auto& eigenvectors = s.H.eigenvectorsBySector();

    BOOST_CHECK (eigenvalues.size() == 16);
    BOOST_CHECK (eigenvectors.size() == 1);
    BOOST_CHECK (eigenvectors[0].cols() == 16);
}

BOOST_AUTO_TEST_CASE ( construct_system_with_symmetries )
{
    HubbardSystem s(4, true);
    s.H.construct();
    BOOST_CHECK (s.basis.getRanges().size() > 1);

    s.H.diagonalize();
    DVector eigenvalues = s.H.eigenvalues();
    auto& eigenvectors = s.H.eigenvectorsBySector();

    BOOST_CHECK (eigenvalues.size() == 256);
    BOOST_CHECK (eigenvectors.size() == 25);
    int n = 0;
    for (auto i=eigenvectors.begin(); i!=eigenvectors.end(); i++)
        n += i->cols();
    BOOST_CHECK (n == 256);
}

BOOST_AUTO_TEST_CASE ( construct_and_diagonalize_system_multiple_times )
{
    HubbardSystem s(4, true);
    s.H.construct();
    size_t n_b1 = s.basis.size();
    size_t n_r1 = s.basis.getRanges().size();

    s.H.diagonalize();
    DVector ev1 = s.H.eigenvalues();

    s.H.construct();
    size_t n_b2 = s.basis.size();
    size_t n_r2 = s.basis.getRanges().size();
    BOOST_CHECK (n_b1 == n_b2);
    BOOST_CHECK (n_r1 == n_r2);

    s.H.diagonalize();
    DVector ev2 = s.H.eigenvalues();

    BOOST_CHECK (ev1 == ev2);
}

BOOST_AUTO_TEST_CASE ( test_by_sector_eigenvalue_and_eigenvectors_accessors )
{
    HubbardSystem s(4, true);
    s.H.construct();

    s.H.diagonalize();
    auto eigenvalues1 = s.H.eigenvaluesBySector();
    auto energies1 = s.H.eigenvalues();

    BOOST_CHECK (eigenvalues1.size() > 1);
    BOOST_CHECK (eigenvalues1[0].size() > 0);
    BOOST_CHECK (energies1.size() == 256);

    DVector ev(256);
    int k = 0;
    for (size_t j=0; j<eigenvalues1.size(); j++)
        for (int i=0; i<eigenvalues1[j].size(); i++)
        {
            ev(k) = eigenvalues1[j](i);
            k++;
        }
    BOOST_CHECK (k == 256);
    BOOST_CHECK (energies1 == ev);

    // test that eigenvectors get properly reset and reconstructed
    s.H.diagonalize();
    auto energies2 = s.H.eigenvalues();
    BOOST_CHECK (energies2.size() == 256);

    s.t = 1000;
    s.U = 20000;
    s.H.construct();
    s.H.diagonalize();
    auto energies3 = s.H.eigenvalues();

    double diff = 0;
    for (int i=0; i<256; i++)
        diff += std::abs(energies2(i) - energies3(i));
    BOOST_CHECK ( diff > 1 );
}

BOOST_AUTO_TEST_CASE ( measure_double_occupancy )
{
    HubbardSystem s(4, true);
    DoubleOccupancy<HubbardSystem> DO(s);
    s.H.construct();

    s.H.diagonalize();
    double doLowT = s.measure(1000, DO(0));
    double doMidT = s.measure(1, DO(0));
    double doHighT = s.measure(0.00001, DO(0));

    BOOST_CHECK (epsilonEqual(doLowT, 0, 10E-2));
    BOOST_CHECK (epsilonEqual(doMidT, 0.00453281, 10E-6));
    BOOST_CHECK (epsilonEqual(doHighT, 0.25, 10E-2));
}

BOOST_AUTO_TEST_CASE ( performance_of_diagonalization )
{
    std::clock_t startCPUTime, endCPUTime;
    double cpuTime = 0;
    size_t sites;
#ifdef NDEBUG
    sites = 7;
#else
    sites = 5;
#endif

    startCPUTime = std::clock();
    HubbardSystem s(sites, true);
    DoubleOccupancy<HubbardSystem> DO(s);
    s.H.construct();
    endCPUTime = std::clock();
    cpuTime = static_cast<double>(endCPUTime-startCPUTime)/CLOCKS_PER_SEC;
    std::cerr << "Time for construction of system with " << sites
              << " sites: " << cpuTime << "s" << std::endl;

    startCPUTime = std::clock();
    s.H.diagonalize();
    endCPUTime = std::clock();
    cpuTime = static_cast<double>(endCPUTime-startCPUTime)/CLOCKS_PER_SEC;
    std::cerr << "Time for diagonalize of system with " << sites
              << " sites: " << cpuTime << "s" << std::endl;

    startCPUTime = std::clock();
    s.measure(1, DO(0));
    endCPUTime = std::clock();
    cpuTime = static_cast<double>(endCPUTime-startCPUTime)/CLOCKS_PER_SEC;
    std::cerr << "Time for ensembleAverage of system with " << sites
              << " sites: " << cpuTime << "s" << std::endl;
}


#ifndef INCLUDED_chemical_stub_HH
#define INCLUDED_chemical_stub_HH

#include <Eigen/Geometry>

namespace scheme {
namespace chemical {

    template<class Xform,class InputPoint>
    Xform make_stub(InputPoint cen, InputPoint n, InputPoint ca, InputPoint c){
        typedef Eigen::Matrix< typename Xform::Scalar, 3, 1 > XPoint;
        XPoint _n, _ca, _c, _cen;
        for( int i = 0; i < 3; ++i){
            _n  [i] = n  [i];
            _ca [i] = ca [i];
            _c  [i] = c  [i];
            _cen[i] = cen[i];
        }
        Xform out;
        XPoint e1( _n - _ca );
        e1.normalize();
        XPoint e3( e1.cross(_c-_ca) );
        e3.normalize();
        XPoint e2( e3.cross(e1) );
        out.linear().col(0) = e1;
        out.linear().col(1) = e2;
        out.linear().col(2) = e3;
        out.translation() = _cen;
        return out;
    }

    template<class Xform,class InputPoint>
    Xform make_stub(InputPoint n, InputPoint ca, InputPoint c){
        return make_stub<Xform>( ca, n, ca, c );
    }

}
}

#endif


// Copyright (C) 2016 Daniele Panozzo <daniele.panozzo@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "tutorial_nrosy.h"
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/Eigenvalues>
#include <iostream>

#include "mesh_param.h"

using namespace std;
using namespace Eigen;

MatrixXd tutorial_nrosy
        (
        const MatrixXd& V,          // Vertices of the mesh
        const MatrixXi& F,          // Faces
        const MatrixXi& TT,         // Adjacency triangle-triangle
        const VectorXi& soft_id,    // Soft constraints face ids
        const MatrixXd& soft_value, // Soft constraints 3d vectors
        const int n                 // Degree of the n-rosy field
        )
{
  assert(soft_id.size() > 0); // One constraint is necessary to make the solution unique

  // This code works only for n==1, see tutorial_nrosy_complete for a generic implementation that works for n >= 1
  assert(n==1);

  Matrix<double,Eigen::Dynamic,3> T1(F.rows(),3), T2(F.rows(),3);

  // Compute the local reference systems for each face
  for (unsigned i=0;i<F.rows();++i)
  {
    Vector3d e1 =  V.row(F(i, 1)) - V.row(F(i, 0));
    Vector3d e2 =  V.row(F(i, 2)) - V.row(F(i, 0));
    T1.row(i) = e1.normalized();
    T2.row(i) = T1.row(i).cross(T1.row(i).cross(e2)).normalized();
  }

  // Build the sparse matrix, with an energy term for each edge
  std::vector< Triplet<std::complex<double> > > t;
  std::vector< Triplet<std::complex<double> > > tb;

  unsigned count = 0;
  for (unsigned f=0;f<F.rows();++f)
  {
    for (unsigned ei=0;ei<F.cols();++ei)
    {
      // Look up the opposite face
      int g = TT(f,ei);
      // If it is a boundary edge, it does not contribute to the energy
      if (g == -1) continue;
      // Avoid to count every edge twice
      if (f > g) continue;
      // Compute the complex representation of the common edge
      Vector3d e  = (V.row(F(f,(ei+1)%3)) - V.row(F(f,ei)));
      Vector2d vef = Vector2d(e.dot(T1.row(f)),e.dot(T2.row(f))).normalized();
      std::complex<double> ef(vef(0),vef(1));
      Vector2d veg = Vector2d(e.dot(T1.row(g)),e.dot(T2.row(g))).normalized();
      std::complex<double> eg(veg(0),veg(1));
      // Add the term conj(f)^n*ui - conj(g)^n*uj to the energy matrix
      t.push_back(Triplet<std::complex<double> >(count,f,    std::conj(ef)));
      t.push_back(Triplet<std::complex<double> >(count,g,-1.*std::conj(eg)));
      ++count;
    }
  }

  // Convert the constraints into the complex polynomial coefficients and add them as soft constraints
  double lambda = 10e6;
  for (unsigned r=0; r<soft_id.size(); ++r)
  {
    int f = soft_id(r);
    Vector3d v = soft_value.row(r);
    std::complex<double> c(v.dot(T1.row(f)),v.dot(T2.row(f)));
    t.push_back(Triplet<std::complex<double> >(count,f, sqrt(lambda)));
    tb.push_back(Triplet<std::complex<double> >(count,0, c * std::complex<double>(sqrt(lambda),0)));
    ++count;
  }

  // Solve the linear system
  typedef SparseMatrix<std::complex<double>> SparseMatrixXcd;
  SparseMatrixXcd A(count,F.rows());
  A.setFromTriplets(t.begin(), t.end());
  SparseMatrixXcd b(count,1);
  b.setFromTriplets(tb.begin(), tb.end());
  SimplicialLDLT< SparseMatrixXcd > solver;
  solver.compute(A.adjoint()*A);
  assert(solver.info()==Success);
  MatrixXcd u = solver.solve(A.adjoint()*MatrixXcd(b));
  assert(solver.info()==Success);

  // Debugging informations
  #ifdef DEBUG_1
  cout << "V: " << V.rows() << " * " << V.cols() << endl;
  cout << "F: " << F.rows() << " * " << F.cols() << endl;
  cout << "TT: " << TT.rows() << " * " << TT.cols() << endl;
  cout << "soft_id: " << soft_id.rows() << " * " << soft_id.cols() << endl;
  cout << "soft_value: " << soft_value.rows() << " * " << soft_value.cols() << endl;
  cout << "T1: " << T1.rows() << " * " << T1.cols() << endl;
  cout << "T2: " << T2.rows() << " * " << T2.cols() << endl;
  cout << "t: " << t.size() << endl;
  cout << "tb: " << tb.size() << endl;
  cout << "count = " << count << endl;
  cout << "A: " << A.rows() << " * " << A.cols() << endl;
  cout << "A*: " << A.adjoint().rows() << " * " << A.adjoint().cols() << endl;
  cout << "b: " << b.rows() << " * " << b.cols() << endl;
  cout << "u: " << u.rows() << " * " << u.cols() << endl;
  #endif

  // Convert the interpolated polyvector into Euclidean vectors
  MatrixXd R(F.rows(),3);
  for (int f=0; f<F.rows(); ++f)
    R.row(f) = T1.row(f) * u(f).real() + T2.row(f) * u(f).imag();

  #ifdef DEBUG_4
  cout << "final locations and values: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << u(soft_id(i)) << " ";
  }
  cout << endl;
  cout << "final locations and coordinates: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << R.row(soft_id(i)) << " ";
  }
  cout << endl;
  cout << "original locations and coordinates: ";
  for (int i = 0; i < soft_id.size(); i++) {
      cout << soft_id(i) << "," << soft_value.row(i) << " ";
  }
  cout << endl;
  #endif

  return R;
}


#include <multi-camera-motion/approx_relpose_generalized.h>
#include <multi-camera-motion/translation.h>

#include <Eigen/Geometry>

#include "problem.h"
#include "so3.h"

#include <iostream>

#include <cstdlib>
#include <ctime>

static Eigen::Vector2d project( const Eigen::Vector3d &x )
{
    return x.head(2)/x[2];
}

static Eigen::Matrix<double,6,1> pluecker( const Ray & ray )
{
    // make ray in Pluecker coordinates
    Eigen::Vector3d d = ray.x/ray.x.norm();
    Eigen::Vector3d m = ray.c.cross(d);
    Eigen::Matrix<double,6,1> p;
    p.head(3) = d;
    p.tail(3) = m;
    return p;
}

static Eigen::Matrix<double,6,6> make_w( const RayPair &ray_pair )
{
    Eigen::Matrix<double,6,1> u = pluecker(ray_pair.first);
    Eigen::Matrix<double,6,1> v = pluecker(ray_pair.second);
    return u*v.transpose();
}

int main( int argc, char **argv )
{
    srand(time(NULL));

    const double trans_mag = 1.;
    const double angle = 5.*M_PI/180.;
    const double noise = 0;

    Problem prob;
    generateProblem( trans_mag, angle, noise, prob );

    Eigen::MatrixXd x(2,6);
    Eigen::MatrixXd y(2,6);
    Eigen::MatrixXd cu(3,6);
    Eigen::MatrixXd cv(3,6);
    for ( int i = 0; i < 6; i++ )
    {
        x.col(i) = project(prob.ray_pairs[i].first.x);
        y.col(i) = project(prob.ray_pairs[i].second.x);
        cu.col(i) = prob.ray_pairs[i].first.c;
        cv.col(i) = prob.ray_pairs[i].second.c;
    }

    Eigen::Matrix<double,6,6> w1;
    Eigen::Matrix<double,6,6> w2;
    Eigen::Matrix<double,6,6> w3;
    Eigen::Matrix<double,6,6> w4;
    Eigen::Matrix<double,6,6> w5;
    Eigen::Matrix<double,6,6> w6;
    std::vector<Eigen::Vector3d> rsolns;

    w1 = make_w(prob.ray_pairs[0]);
    w2 = make_w(prob.ray_pairs[1]);
    w3 = make_w(prob.ray_pairs[2]);
    w4 = make_w(prob.ray_pairs[3]);
    w5 = make_w(prob.ray_pairs[4]);
    w6 = make_w(prob.ray_pairs[5]);
    approx_relpose_generalized(w1,w2,w3,w4,w5,w6,rsolns);

    for ( int i = 0; i < rsolns.size(); i++ )
    {
        Eigen::Matrix3d Rsoln = so3exp(rsolns[i]);
        Eigen::Vector3d tsoln = solve_translation(x,y,cu,cv,Rsoln);
        double rot_angle_err, trans_angle_err, trans_scale_err;
        compute_error( prob.R, prob.t,
                       Rsoln, tsoln,
                       rot_angle_err, trans_angle_err, trans_scale_err );
        std::cout << "solution " << i+1 << " error: ";
        std::cout << "\trotation error: " << rot_angle_err*180./M_PI << " deg\n";
        std::cout << "\ttranslation error: " << trans_angle_err*180./M_PI << " deg\n";
        std::cout << "\ttranslation scale error: " << trans_scale_err << "\n";
    }

}