/*
 * This file belongs to the Galois project, a C++ library for exploiting parallelism.
 * The code is being released under the terms of the 3-Clause BSD License (a
 * copy is located in LICENSE.txt at the top-level directory).
 *
 * Copyright (C) 2018, The University of Texas at Austin. All rights reserved.
 * UNIVERSITY EXPRESSLY DISCLAIMS ANY AND ALL WARRANTIES CONCERNING THIS
 * SOFTWARE AND DOCUMENTATION, INCLUDING ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR ANY PARTICULAR PURPOSE, NON-INFRINGEMENT AND WARRANTIES OF
 * PERFORMANCE, AND ANY WARRANTY THAT MIGHT OTHERWISE ARISE FROM COURSE OF
 * DEALING OR USAGE OF TRADE.  NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH
 * RESPECT TO THE USE OF THE SOFTWARE OR DOCUMENTATION. Under no circumstances
 * shall University be liable for incidental, special, indirect, direct or
 * consequential damages or loss of profits, interruption of business, or
 * related expenses which may arise from use of Software or Documentation,
 * including but not limited to those resulting from defects in Software and/or
 * Documentation, or loss or inaccuracy of data of any kind.
 */

#include <cmath>
#include <fstream>
#include <iostream>
#include <ostream>
#include "matrixCompletion.h"
#include "galois/runtime/TiledExecutor.h"
#include "galois/ParallelSTL.h"
#include "galois/graphs/Graph.h"
#include "Lonestar/BoilerPlate.h"

#ifdef HAS_EIGEN
#include <Eigen/Sparse>
#include <Eigen/Dense>
#endif

#ifdef _OPENMP
#include <omp.h>
#endif

static const char* const name = "Matrix Completion";
static const char* const desc =
    "Computes Matrix Decomposition using Stochastic "
    "Gradient Descent or Alternating Least Squares";
static const char* const url = 0;

enum Algo {
  syncALS,
  simpleALS,
  sgdByItems,
  sgdByEdges,
  sgdBlockEdge,
  sgdBlockJump,
};

enum Step { bold, bottou, intel, inverse, purdue };

/*
 * Commandline options for different Algorithms
 */
static cll::opt<Algo>
    algo("algo", cll::desc("Choose an algorithm:"),
         cll::values(
             clEnumValN(Algo::syncALS, "syncALS", "Alternating least squares"),
             clEnumValN(Algo::simpleALS, "simpleALS",
                        "Simple alternating least squares"),
             clEnumValN(Algo::sgdBlockEdge, "sgdBlockEdge",
                        "SGD Edge blocking (default)"),
             clEnumValN(Algo::sgdBlockJump, "sgdBlockJump",
                        "SGD using Block jumping "),
             clEnumValN(Algo::sgdByItems, "sgdByItems", "Simple SGD on Items"),
             clEnumValN(Algo::sgdByEdges, "sgdByEdges", "Simple SGD on edges"),
             clEnumValEnd),
         cll::init(Algo::sgdBlockEdge));
/*
 * Commandline options for different learning functions
 */
static cll::opt<Step> learningRateFunction(
    "learningRateFunction", cll::desc("Choose learning rate function:"),
    cll::values(clEnumValN(Step::intel, "intel", "Intel"),
                clEnumValN(Step::purdue, "purdue", "Purdue"),
                clEnumValN(Step::bottou, "bottou", "Bottou"),
                clEnumValN(Step::bold, "bold", "Bold (default)"),
                clEnumValN(Step::inverse, "inverse", "Inverse"), clEnumValEnd),
    cll::init(Step::bold));

static cll::opt<int> cutoff("cutoff");

static const unsigned ALS_CHUNK_SIZE = 4;

size_t NUM_ITEM_NODES = 0;

struct PurdueStepFunction : public StepFunction {
  virtual std::string name() const { return "Purdue"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate * 1.5 / (1.0 + decayRate * pow(round + 1, 1.5));
  }
};

struct IntelStepFunction : public StepFunction {
  virtual std::string name() const { return "Intel"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate * pow(decayRate, round);
  }
};

struct BottouStepFunction : public StepFunction {
  virtual std::string name() const { return "Bottou"; }
  virtual LatentValue stepSize(int round) const {
    return learningRate / (1.0 + learningRate * lambda * round);
  }
};

struct InverseStepFunction : public StepFunction {
  virtual std::string name() const { return "Inverse"; }
  virtual LatentValue stepSize(int round) const { return 1.0 / (round + 1); }
};

struct BoldStepFunction : public StepFunction {
  virtual std::string name() const { return "Bold"; }
  virtual bool isBold() const { return true; }
  virtual LatentValue stepSize(int round) const { return 0.0; }
};

template <typename Graph>
double sumSquaredError(Graph& g) {
  typedef typename Graph::GraphNode GNode;
  // computing Root Mean Square Error
  // Assuming only item nodes have edges
  galois::GAccumulator<double> error;

  galois::do_all(
      galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES), [&](GNode n) {
        for (auto ii = g.edge_begin(n), ei = g.edge_end(n); ii != ei; ++ii) {
          GNode dst = g.getEdgeDst(ii);
          LatentValue e =
              predictionError(g.getData(n).latentVector,
                              g.getData(dst).latentVector, g.getEdgeData(ii));
          error += (e * e);
        }
      });
  return error.reduce();
}

template <typename Graph>
size_t countEdges(Graph& g) {
  typedef typename Graph::GraphNode GNode;
  galois::GAccumulator<size_t> edges;
  galois::runtime::Fixed2DGraphTiledExecutor<Graph> executor(g);
  std::cout << "NUM_ITEM_NODES : " << NUM_ITEM_NODES << "\n";
  executor.execute(g.begin(), g.begin() + NUM_ITEM_NODES,
                   g.begin() + NUM_ITEM_NODES, g.end(), itemsPerBlock,
                   usersPerBlock,
                   [&](GNode src, GNode dst,
                       typename Graph::edge_iterator edge) { edges += 1; },
                   false); // false = no locks
  return edges.reduce();
}

template <typename Graph>
void verify(Graph& g, const std::string& prefix) {
  std::cout << countEdges(g) << " : " << g.sizeEdges() << "\n";
  if (countEdges(g) != g.sizeEdges()) {
    GALOIS_DIE("Error: edge list of input graph probably not sorted");
  }

  double error = sumSquaredError(g);
  double rmse  = std::sqrt(error / g.sizeEdges());

  std::cout << prefix << "RMSE: " << rmse << "\n";
}

template <typename T, unsigned Size>
struct ExplicitFiniteChecker {};

template <typename T>
struct ExplicitFiniteChecker<T, 4U> {
  static_assert(std::numeric_limits<T>::is_iec559, "Need IEEE floating point");
  bool isFinite(T v) {
    union {
      T value;
      uint32_t bits;
    } a = {v};
    if (a.bits == 0x7F800000) {
      return false; // +inf
    } else if (a.bits == 0xFF800000) {
      return false; // -inf
    } else if (a.bits >= 0x7F800001 && a.bits <= 0x7FBFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0xFF800001 && a.bits <= 0xFFBFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0x7FC00000 && a.bits <= 0x7FFFFFFF) {
      return false; // quiet NaN
    } else if (a.bits >= 0xFFC00000 && a.bits <= 0xFFFFFFFF) {
      return false; // quiet NaN
    }
    return true;
  }
};

template <typename T>
struct ExplicitFiniteChecker<T, 8U> {
  static_assert(std::numeric_limits<T>::is_iec559, "Need IEEE floating point");
  bool isFinite(T v) {
    union {
      T value;
      uint64_t bits;
    } a = {v};
    if (a.bits == 0x7FF0000000000000) {
      return false; // +inf
    } else if (a.bits == 0xFFF0000000000000) {
      return false; // -inf
    } else if (a.bits >= 0x7FF0000000000001 && a.bits <= 0x7FF7FFFFFFFFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0xFFF0000000000001 && a.bits <= 0xFFF7FFFFFFFFFFFF) {
      return false; // signaling NaN
    } else if (a.bits >= 0x7FF8000000000000 && a.bits <= 0x7FFFFFFFFFFFFFFF) {
      return false; // quiet NaN
    } else if (a.bits >= 0xFFF8000000000000 && a.bits <= 0xFFFFFFFFFFFFFFFF) {
      return false; // quiet NaN
    }
    return true;
  }
};

template <typename T>
bool isFinite(T v) {
#ifdef __FAST_MATH__
  return ExplicitFiniteChecker<T, sizeof(T)>().isFinite(v);
#else
  return std::isfinite(v);
#endif
}

double countFlops(size_t nnz, int rounds, int k) {
  double flop = 0;
  if (useExactError) {
    // dotProduct = 2K, square = 1, sum = 1
    flop += nnz * (2.0 * k + 1 + 1);
  } else {
    // Computed during gradient update: square = 1, sum = 1
    flop += nnz * (1 + 1);
  }
  // dotProduct = 2K, gradient = 10K,
  flop += rounds * (nnz * (12.0 * k));
  return flop;
}

/*
 * Common function to execute different algorithms
 * till convergence.
 *
 * @param StepFunction to be used
 * @param Graph
 * @param fn (algorithm)
 *
 */
template <typename Graph, typename Fn>
void executeUntilConverged(const StepFunction& sf, Graph& g, Fn fn) {
  galois::GAccumulator<double> errorAccum;
  std::vector<LatentValue> steps(updatesPerEdge);
  LatentValue last    = -1.0;
  unsigned deltaRound = updatesPerEdge;
  LatentValue rate    = learningRate;

  galois::StatTimer executeAlgoTimer("Algorithm Execution Time");
  galois::TimeAccumulator elapsed;
  elapsed.start();

  unsigned long lastTime = 0;

  for (unsigned int round = 0;; round += deltaRound) {
    if (fixedRounds > 0 && round >= fixedRounds)
      break;
    if (fixedRounds > 0)
      deltaRound = std::min(deltaRound, fixedRounds - round);

    for (unsigned i = 0; i < updatesPerEdge; ++i) {
      // Assume that loss decreases
      if (sf.isBold())
        steps[i] = i == 0 ? rate : steps[i - 1] * 1.05;
      else
        steps[i] = sf.stepSize(round + i);
    }

    executeAlgoTimer.start();
    fn(&steps[0], round + deltaRound, useExactError ? &errorAccum : NULL);
    executeAlgoTimer.stop();
    double error = useExactError ? errorAccum.reduce() : sumSquaredError(g);

    elapsed.stop();

    unsigned long curElapsed = elapsed.get();
    elapsed.start();
    unsigned long millis = curElapsed - lastTime;
    lastTime             = curElapsed;

    double gflops = countFlops(g.sizeEdges(), deltaRound, LATENT_VECTOR_SIZE) /
                    millis / 1e6;

    int curRound = round + deltaRound;
    galois::gPrint("R: ", curRound, " elapsed (ms): ", curElapsed,
                   " GFLOP/s: ", gflops);
    if (useExactError) {
      galois::gPrint(" RMSE (R ", curRound,
                     "): ", std::sqrt(error / g.sizeEdges()), "\n");
    } else {
      galois::gPrint(" Approx. RMSE (R ", (curRound - 1),
                     ".5): ", std::sqrt(std::abs(error / g.sizeEdges())), "\n");
    }

    galois::gPrint("Error Change : ", std::abs((last - error) / last), "\n");
    if (!isFinite(error))
      break;
    if (fixedRounds <= 0 &&
        (round >= maxUpdates || std::abs((last - error) / last) < tolerance))
      break;
    if (sf.isBold()) {
      // Assume that loss decreases first round
      if (last >= 0.0 && last < error)
        rate = steps[deltaRound - 1] * 0.5;
      else
        rate = steps[deltaRound - 1] * 1.05;
    }
    last = error;
  }
}

/*
 * Divides the Items and users into 2D blocks.
 * Locks each block to work on it.
 */
struct SGDBlockJumpAlgo {
  bool isSgd() const { return true; }
  typedef galois::substrate::PaddedLock<true> SpinLock;
  static const bool precomputeOffsets = true; // false;

  std::string name() const { return "sgdBlockJumpAlgo"; }

  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  typedef galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //    ::with_numa_alloc<true>::type
      ::with_no_lockable<true>::type Graph;
  typedef Graph::GraphNode GNode;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  size_t userIdToUserNode(size_t userId) { return userId + NUM_ITEM_NODES; }

  struct BlockInfo {
    size_t id;
    size_t x;
    size_t y;
    size_t userStart;
    size_t userEnd;
    size_t itemStart;
    size_t itemEnd;
    size_t numitems;
    size_t updates;
    double error;
    int* userOffsets;

    std::ostream& print(std::ostream& os) {
      os << "id: " << id << " x: " << x << " y: " << y
         << " userStart: " << userStart << " userEnd: " << userEnd
         << " itemStart: " << itemStart << " itemEnd: " << itemEnd
         << " updates: " << updates << "\n";
      return os;
    }
  };

  struct Process {
    Graph& g;
    SpinLock *xLocks, *yLocks;
    BlockInfo* blocks;
    size_t numXBlocks, numYBlocks;
    LatentValue* steps;
    size_t maxUpdates;
    galois::GAccumulator<double>* errorAccum;

    struct GetDst : public std::unary_function<Graph::edge_iterator, GNode> {
      Graph* g;
      GetDst() {}
      GetDst(Graph* _g) : g(_g) {}
      GNode operator()(Graph::edge_iterator ii) const {
        return g->getEdgeDst(ii);
      }
    };

    /**
     * Preconditions: row and column of slice are locked.
     *
     * Postconditions: increments update count, does sgd update on each item
     * and user in the slice
     */
    template <bool Enable = precomputeOffsets>
    size_t runBlock(BlockInfo& si,
                    typename std::enable_if<!Enable>::type* = 0) {
      typedef galois::NoDerefIterator<Graph::edge_iterator> no_deref_iterator;
      typedef boost::transform_iterator<GetDst, no_deref_iterator>
          edge_dst_iterator;

      LatentValue stepSize = steps[si.updates - maxUpdates + updatesPerEdge];
      size_t seen          = 0;
      double error         = 0.0;

      // Set up item iterators
      size_t itemId      = 0;
      Graph::iterator mm = g.begin(), em = g.begin();
      std::advance(mm, si.itemStart);
      std::advance(em, si.itemEnd);

      GetDst fn{&g};

      // For each item in the range
      for (; mm != em; ++mm, ++itemId) {
        GNode item      = *mm;
        Node& itemData  = g.getData(item);
        size_t lastUser = si.userEnd + NUM_ITEM_NODES;

        edge_dst_iterator start(no_deref_iterator(g.edge_begin(
                                    item, galois::MethodFlag::UNPROTECTED)),
                                fn);
        edge_dst_iterator end(no_deref_iterator(g.edge_end(
                                  item, galois::MethodFlag::UNPROTECTED)),
                              fn);

        // For each edge in the range
        for (auto ii =
                 std::lower_bound(start, end, si.userStart + NUM_ITEM_NODES);
             ii != end; ++ii) {
          GNode user = g.getEdgeDst(*ii.base());

          if (user >= lastUser)
            break;

          LatentValue e = doGradientUpdate(itemData.latentVector,
                                           g.getData(user).latentVector, lambda,
                                           g.getEdgeData(*ii.base()), stepSize);
          if (errorAccum)
            error += e * e;
          ++seen;
        }
      }

      si.updates += 1;
      if (errorAccum) {
        *errorAccum += (error - si.error);
        si.error = error;
      }

      return seen;
    }

    template <bool Enable = precomputeOffsets>
    size_t runBlock(BlockInfo& si, typename std::enable_if<Enable>::type* = 0) {
      LatentValue stepSize = steps[si.updates - maxUpdates + updatesPerEdge];
      size_t seen          = 0;
      double error         = 0.0;

      // Set up item iterators
      size_t itemId      = 0;
      Graph::iterator mm = g.begin(), em = g.begin();
      std::advance(mm, si.itemStart);
      std::advance(em, si.itemEnd);

      // For each item in the range
      for (; mm != em; ++mm, ++itemId) {
        if (si.userOffsets[itemId] < 0)
          continue;

        GNode item      = *mm;
        Node& itemData  = g.getData(item);
        size_t lastUser = si.userEnd + NUM_ITEM_NODES;

        // For each edge in the range
        for (auto ii = g.edge_begin(item) + si.userOffsets[itemId],
                  ei = g.edge_end(item);
             ii != ei; ++ii) {
          GNode user = g.getEdgeDst(ii);

          if (user >= lastUser)
            break;

          LatentValue e = doGradientUpdate(itemData.latentVector,
                                           g.getData(user).latentVector, lambda,
                                           g.getEdgeData(ii), stepSize);
          if (errorAccum)
            error += e * e;
          ++seen;
        }
      }

      si.updates += 1;
      if (errorAccum) {
        *errorAccum += (error - si.error);
        si.error = error;
      }

      return seen;
    }

    /**
     * Searches next slice to work on.
     *
     * @returns slice id to work on, x and y locks are held on the slice
     */
    size_t getNextBlock(BlockInfo* sp) {
      size_t numBlocks   = numXBlocks * numYBlocks;
      size_t nextBlockId = sp->id + 1;
      for (size_t i = 0; i < 2 * numBlocks; ++i, ++nextBlockId) {
        // Wrap around
        if (nextBlockId == numBlocks)
          nextBlockId = 0;

        BlockInfo& nextBlock = blocks[nextBlockId];

        if (nextBlock.updates < maxUpdates && xLocks[nextBlock.x].try_lock()) {
          if (yLocks[nextBlock.y].try_lock()) {
            // Return while holding locks
            return nextBlockId;
          } else {
            xLocks[nextBlock.x].unlock();
          }
        }
      }

      return numBlocks;
    }

    void operator()(unsigned tid, unsigned total) {
      galois::StatTimer timer("PerThreadTime");
      // TODO: Report Accumulators at the end
      galois::GAccumulator<size_t> edgesVisited;
      galois::GAccumulator<size_t> blocksVisited;
      size_t numBlocks = numXBlocks * numYBlocks;
      size_t xBlock    = (numXBlocks + total - 1) / total;
      size_t xStart    = std::min(xBlock * tid, numXBlocks - 1);
      size_t yBlock    = (numYBlocks + total - 1) / total;
      size_t yStart    = std::min(yBlock * tid, numYBlocks - 1);
      BlockInfo* sp    = &blocks[xStart + yStart + numXBlocks];

      timer.start();

      while (true) {
        sp = &blocks[getNextBlock(sp)];
        if (sp == &blocks[numBlocks])
          break;
        blocksVisited += 1;
        edgesVisited += runBlock(*sp);

        xLocks[sp->x].unlock();
        yLocks[sp->y].unlock();
      }

      timer.stop();
    }
  };

  void operator()(Graph& g, const StepFunction& sf) {
    galois::StatTimer preProcessTimer("PreProcessingTime");
    preProcessTimer.start();
    const size_t numUsers = g.size() - NUM_ITEM_NODES;
    const size_t numYBlocks =
        (NUM_ITEM_NODES + itemsPerBlock - 1) / itemsPerBlock;
    const size_t numXBlocks = (numUsers + usersPerBlock - 1) / usersPerBlock;
    const size_t numBlocks  = numXBlocks * numYBlocks;

    SpinLock* xLocks = new SpinLock[numXBlocks];
    SpinLock* yLocks = new SpinLock[numYBlocks];

    std::cout << "itemsPerBlock: " << itemsPerBlock
              << " usersPerBlock: " << usersPerBlock
              << " numBlocks: " << numBlocks << " numXBlocks: " << numXBlocks
              << " numYBlocks: " << numYBlocks << "\n";

    // Initialize
    BlockInfo* blocks = new BlockInfo[numBlocks];
    for (size_t i = 0; i < numBlocks; i++) {
      BlockInfo& si = blocks[i];
      si.id         = i;
      si.x          = i % numXBlocks;
      si.y          = i / numXBlocks;
      si.updates    = 0;
      si.error      = 0.0;
      si.userStart  = si.x * usersPerBlock;
      si.userEnd    = std::min((si.x + 1) * usersPerBlock, numUsers);
      si.itemStart  = si.y * itemsPerBlock;
      si.itemEnd    = std::min((si.y + 1) * itemsPerBlock, NUM_ITEM_NODES);
      si.numitems   = si.itemEnd - si.itemStart;
      if (precomputeOffsets) {
        si.userOffsets = new int[si.numitems];
      } else {
        si.userOffsets = nullptr;
      }
    }

    // Partition item edges in blocks to users according to range [userStart,
    // userEnd)
    if (precomputeOffsets) {
      galois::do_all(galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
                     [&](GNode item) {
                       size_t sliceY = item / itemsPerBlock;
                       BlockInfo* s  = &blocks[sliceY * numXBlocks];

                       size_t pos = item - s->itemStart;
                       auto ii = g.edge_begin(item), ei = g.edge_end(item);
                       size_t offset = 0;
                       for (size_t i = 0; i < numXBlocks; ++i, ++s) {
                         size_t start = userIdToUserNode(s->userStart);
                         size_t end   = userIdToUserNode(s->userEnd);

                         if (ii != ei && g.getEdgeDst(ii) >= start &&
                             g.getEdgeDst(ii) < end) {
                           s->userOffsets[pos] = offset;
                         } else {
                           s->userOffsets[pos] = -1;
                         }
                         for (; ii != ei && g.getEdgeDst(ii) < end;
                              ++ii, ++offset)
                           ;
                       }
                     });
    }
    preProcessTimer.stop();

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();
    executeUntilConverged(sf, g,
                          [&](LatentValue* steps, size_t maxUpdates,
                              galois::GAccumulator<double>* errorAccum) {
                            Process fn{g,      xLocks,     yLocks,
                                       blocks, numXBlocks, numYBlocks,
                                       steps,  maxUpdates, errorAccum};
                            galois::on_each(fn);
                          });
    executeTimer.stop();
  }
};

/*
 * Simple SGD going over all the destination(users) for a given
 * source(Item)
 */
class SGDItemsAlgo {
  static const bool makeSerializable = false;

  struct BasicNode {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdItemsAlgo"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;

    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {

      const LatentValue stepSize = steps[0];
      galois::for_each(
          galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
          [&](GNode src, auto& ctx) {
            for (auto ii : g.edges(src)) {

              GNode dst         = g.getEdgeDst(ii);
              LatentValue error = doGradientUpdate(
                  g.getData(src, galois::MethodFlag::UNPROTECTED).latentVector,
                  g.getData(dst).latentVector, lambda, g.getEdgeData(ii),
                  stepSize);

              edgesVisited += 1;
              if (useExactError)
                *errorAccum += error;
            }
          },
          galois::wl<galois::worklists::PerSocketChunkFIFO<64>>(),
          galois::no_pushes(), galois::loopname("sgdItemsAlgo"));
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdItemsAlgo");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdItemsAlgo", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/**
 * Simple by-edge grouped by items (only one edge per item on the WL at any
 * time)
 */
class SGDEdgeItem {
  static const bool makeSerializable = false;

  struct BasicNode {
    // latent vector to be learned.
    LatentValue latentVector[LATENT_VECTOR_SIZE];
    // if a item's update is interrupted, where to start when resuming.
    unsigned int edge_offset;
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdEdgeItem"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;
    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {
      const LatentValue stepSize = steps[0];
      galois::for_each(
          galois::iterate(g.begin(), g.begin() + NUM_ITEM_NODES),
          [&](GNode src, auto& ctx) {
            auto ii = g.edge_begin(src, galois::MethodFlag::UNPROTECTED);
            auto ee = g.edge_end(src, galois::MethodFlag::UNPROTECTED);

            if (ii == ee)
              return;

            // Do not need lock on the source node, since only one thread can
            // work on a given src(item).
            auto& srcData = g.getData(src, galois::MethodFlag::UNPROTECTED);
            // Advance to the edge that has not been worked yet.
            std::advance(ii, srcData.edge_offset);
            // Take lock on the destination as multiple source may update the
            // same destination.
            auto& dstData = g.getData(g.getEdgeDst(ii));
            LatentValue error =
                doGradientUpdate(srcData.latentVector, dstData.latentVector,
                                 lambda, g.getEdgeData(ii), stepSize);

            ++srcData.edge_offset;
            ++ii;

            edgesVisited += 1;
            if (useExactError)
              *errorAccum += error;

            if (ii == ee) {
              // Finished the last edge.
              // Start from the first edge.
              srcData.edge_offset = 0;
              return;
            } else {
              // More edges to work on, therefore push the current src
              // to the worklist.
              ctx.push(src);
            }
          },
          galois::wl<galois::worklists::PerSocketChunkLIFO<8>>(),
          galois::loopname("sgdEdgeItem"));
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdEdgeItem");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdEdgeItem", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/*
 * Simple edge-wise operator
 * Use Fixed2DGraphTiledExecutor to divide Items and Users in to blocks.
 * Locks blocks (blocks may share Items or Users) to work on them.
 *
 */
class SGDBlockEdgeAlgo {
  static const bool makeSerializable = false;

  struct BasicNode {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  using Node = BasicNode;

public:
  bool isSgd() const { return true; }

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>
      //::template with_numa_alloc<true>::type
      ::template with_out_of_line_lockable<true>::type ::
          template with_no_lockable<!makeSerializable>::type Graph;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  std::string name() const { return "sgdBlockEdge"; }

  size_t numItems() const { return NUM_ITEM_NODES; }

private:
  using GNode         = typename Graph::GraphNode;
  using edge_iterator = typename Graph::edge_iterator;

  struct Execute {
    Graph& g;
    galois::GAccumulator<unsigned>& edgesVisited;

    void operator()(LatentValue* steps, int maxUpdates,
                    galois::GAccumulator<double>* errorAccum) {
      galois::runtime::Fixed2DGraphTiledExecutor<Graph> executor(g);
      executor.execute(
          g.begin(), g.begin() + NUM_ITEM_NODES, g.begin() + NUM_ITEM_NODES,
          g.end(), itemsPerBlock, usersPerBlock,
          [&](GNode src, GNode dst, edge_iterator edge) {
            const LatentValue stepSize = steps[0];
            LatentValue error          = doGradientUpdate(
                g.getData(src).latentVector, g.getData(dst).latentVector,
                lambda, g.getEdgeData(edge), stepSize);
            edgesVisited += 1;
            if (useExactError)
              *errorAccum += error;
          },
          true // use locks
      );
    }
  };

public:
  void operator()(Graph& g, const StepFunction& sf) {
    verify(g, "sgdBlockEdgeAlgo");
    galois::GAccumulator<unsigned> edgesVisited;

    // galois::StatTimer executeTimer("Total Execution Time");
    galois::StatTimer executeTimer("Time");
    executeTimer.start();

    Execute fn{g, edgesVisited};
    executeUntilConverged(sf, g, fn);

    executeTimer.stop();

    galois::runtime::reportStat_Single("sgdBlockEdgeAlgo", "EdgesVisited",
                                       edgesVisited.reduce());
  }
};

/**
 * ALS algorithms
 */

#ifdef HAS_EIGEN

struct SimpleALSalgo {
  bool isSgd() const { return false; }
  std::string name() const { return "AlternatingLeastSquares"; }
  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType>::with_no_lockable<
      true>::type Graph;
  typedef Graph::GraphNode GNode;
  // Column-major access
  typedef Eigen::SparseMatrix<LatentValue> Sp;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> MT;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, 1> V;
  typedef Eigen::Map<V> MapV;

  Sp A;
  Sp AT;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  void copyToGraph(Graph& g, MT& WT, MT& HT) {
    // Copy out
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        mapV = WT.col(n);
      } else {
        mapV = HT.col(n - NUM_ITEM_NODES);
      }
    }
  }

  void copyFromGraph(Graph& g, MT& WT, MT& HT) {
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        WT.col(n) = mapV;
      } else {
        HT.col(n - NUM_ITEM_NODES) = mapV;
      }
    }
  }

  void initializeA(Graph& g) {
    typedef Eigen::Triplet<int> Triplet;
    std::vector<Triplet> triplets{g.sizeEdges()};
    auto it = triplets.begin();
    for (auto n : g) {
      for (auto edge : g.out_edges(n)) {
        *it++ = Triplet(n, g.getEdgeDst(edge) - NUM_ITEM_NODES,
                        g.getEdgeData(edge));
      }
    }
    A.resize(NUM_ITEM_NODES, g.size() - NUM_ITEM_NODES);
    A.setFromTriplets(triplets.begin(), triplets.end());
    AT = A.transpose();
  }

  void operator()(Graph& g, const StepFunction&) {
    galois::TimeAccumulator elapsed;
    elapsed.start();

    // Find W, H that minimize ||W H^T - A||_2^2 by solving alternating least
    // squares problems:
    //   (W^T W + lambda I) H^T = W^T A (solving for H^T)
    //   (H^T H + lambda I) W^T = H^T A^T (solving for W^T)
    MT WT{LATENT_VECTOR_SIZE, NUM_ITEM_NODES};
    MT HT{LATENT_VECTOR_SIZE, g.size() - NUM_ITEM_NODES};
    typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, LATENT_VECTOR_SIZE>
        XTX;
    typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> XTSp;
    typedef galois::substrate::PerThreadStorage<XTX> PerThrdXTX;

    galois::gPrint("ALS::Start initializeA\n");
    initializeA(g);
    galois::gPrint("ALS::End initializeA\n");
    galois::gPrint("ALS::Start copyFromGraph\n");
    copyFromGraph(g, WT, HT);
    galois::gPrint("ALS::End copyFromGraph\n");

    double last = -1.0;
    galois::StatTimer mmTime("MMTime");
    galois::StatTimer update1Time("UpdateTime1");
    galois::StatTimer update2Time("UpdateTime2");
    galois::StatTimer copyTime("CopyTime");
    galois::StatTimer totalExecTime("totalExecTime");
    galois::StatTimer totalAlgoTime("Time");
    PerThrdXTX xtxs;

    totalAlgoTime.start();
    for (unsigned round = 1;; ++round) {
      totalExecTime.start();
      mmTime.start();
      // TODO parallelize this using tiled executor
      XTSp WTA = WT * A;
      mmTime.stop();

      update1Time.start();
      // TODO: Change to Do_all, pass ints to iterator
      galois::for_each(
          galois::iterate(boost::counting_iterator<int>(0),
                          boost::counting_iterator<int>(A.outerSize())),
          [&](int col, galois::UserContext<int>&) {
            // Compute WTW = W^T * W for sparse A
            XTX& WTW = *xtxs.getLocal();
            WTW.setConstant(0);
            for (Sp::InnerIterator it(A, col); it; ++it)
              WTW.triangularView<Eigen::Upper>() +=
                  WT.col(it.row()) * WT.col(it.row()).transpose();
            for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
              WTW(i, i) += lambda;
            HT.col(col) =
                WTW.selfadjointView<Eigen::Upper>().llt().solve(WTA.col(col));
          });
      update1Time.stop();

      mmTime.start();
      XTSp HTAT = HT * AT;
      mmTime.stop();

      update2Time.start();
      galois::for_each(
          galois::iterate(boost::counting_iterator<int>(0),
                          boost::counting_iterator<int>(AT.outerSize())),
          [&](int col, galois::UserContext<int>&) {
            // Compute HTH = H^T * H for sparse A
            XTX& HTH = *xtxs.getLocal();
            HTH.setConstant(0);
            for (Sp::InnerIterator it(AT, col); it; ++it)
              HTH.triangularView<Eigen::Upper>() +=
                  HT.col(it.row()) * HT.col(it.row()).transpose();
            for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
              HTH(i, i) += lambda;
            WT.col(col) =
                HTH.selfadjointView<Eigen::Upper>().llt().solve(HTAT.col(col));
          });
      update2Time.stop();

      copyTime.start();
      copyToGraph(g, WT, HT);
      copyTime.stop();
      totalExecTime.stop();

      double error = sumSquaredError(g);
      elapsed.stop();
      std::cout << "R: " << round << " elapsed (ms): " << elapsed.get()
                << " RMSE (R " << round
                << "): " << std::sqrt(error / g.sizeEdges()) << "\n";
      elapsed.start();

      if (fixedRounds <= 0 && round > 1 &&
          std::abs((last - error) / last) < tolerance)
        break;
      if (fixedRounds > 0 && round >= fixedRounds)
        break;

      last = error;
    }
    totalAlgoTime.stop();
  }
};

struct SyncALSalgo {

  bool isSgd() const { return false; }

  std::string name() const { return "SynchronousAlternatingLeastSquares"; }

  struct Node {
    LatentValue latentVector[LATENT_VECTOR_SIZE];
  };

  static const bool NEEDS_LOCKS = false;
  typedef typename galois::graphs::LC_CSR_Graph<Node, EdgeType> BaseGraph;
  typedef typename std::conditional<
      NEEDS_LOCKS,
      typename BaseGraph::template with_out_of_line_lockable<true>::type,
      typename BaseGraph::template with_no_lockable<true>::type>::type Graph;
  typedef typename Graph::GraphNode GNode;
  // Column-major access
  typedef Eigen::SparseMatrix<LatentValue> Sp;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> MT;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, 1> V;
  typedef Eigen::Map<V> MapV;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, LATENT_VECTOR_SIZE>
      XTX;
  typedef Eigen::Matrix<LatentValue, LATENT_VECTOR_SIZE, Eigen::Dynamic> XTSp;

  typedef galois::substrate::PerThreadStorage<XTX> PerThrdXTX;
  typedef galois::substrate::PerThreadStorage<V> PerThrdV;

  Sp A;
  Sp AT;

  void readGraph(Graph& g) { galois::graphs::readGraph(g, inputFilename); }

  void copyToGraph(Graph& g, MT& WT, MT& HT) {
    // Copy out
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        mapV = WT.col(n);
      } else {
        mapV = HT.col(n - NUM_ITEM_NODES);
      }
    }
  }

  void copyFromGraph(Graph& g, MT& WT, MT& HT) {
    for (GNode n : g) {
      LatentValue* ptr = &g.getData(n).latentVector[0];
      MapV mapV{ptr};
      if (n < NUM_ITEM_NODES) {
        WT.col(n) = mapV;
      } else {
        HT.col(n - NUM_ITEM_NODES) = mapV;
      }
    }
  }

  void initializeA(Graph& g) {
    typedef Eigen::Triplet<int> Triplet;
    std::vector<Triplet> triplets{g.sizeEdges()};
    auto it = triplets.begin();
    for (auto n : g) {
      for (auto edge : g.out_edges(n)) {
        *it++ = Triplet(n, g.getEdgeDst(edge) - NUM_ITEM_NODES,
                        g.getEdgeData(edge));
      }
    }
    A.resize(NUM_ITEM_NODES, g.size() - NUM_ITEM_NODES);
    A.setFromTriplets(triplets.begin(), triplets.end());
    AT = A.transpose();
  }

  void update(Graph& g, size_t col, MT& WT, MT& HT, PerThrdXTX& xtxs,
              PerThrdV& rhs) {
    // Compute WTW = W^T * W for sparse A
    V& r = *rhs.getLocal();
    if (col < NUM_ITEM_NODES) {
      r.setConstant(0);
      // HTAT = HT * AT; r = HTAT.col(col)
      for (Sp::InnerIterator it(AT, col); it; ++it)
        r += it.value() * HT.col(it.row());
      XTX& HTH = *xtxs.getLocal();
      HTH.setConstant(0);
      for (Sp::InnerIterator it(AT, col); it; ++it)
        HTH.triangularView<Eigen::Upper>() +=
            HT.col(it.row()) * HT.col(it.row()).transpose();
      for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
        HTH(i, i) += lambda;
      WT.col(col) = HTH.selfadjointView<Eigen::Upper>().llt().solve(r);
    } else {
      col = col - NUM_ITEM_NODES;
      r.setConstant(0);
      // WTA = WT * A; x = WTA.col(col)
      for (Sp::InnerIterator it(A, col); it; ++it)
        r += it.value() * WT.col(it.row());
      XTX& WTW = *xtxs.getLocal();
      WTW.setConstant(0);
      for (Sp::InnerIterator it(A, col); it; ++it)
        WTW.triangularView<Eigen::Upper>() +=
            WT.col(it.row()) * WT.col(it.row()).transpose();
      for (int i = 0; i < LATENT_VECTOR_SIZE; ++i)
        WTW(i, i) += lambda;
      HT.col(col) = WTW.selfadjointView<Eigen::Upper>().llt().solve(r);
    }
  }

  struct NonDetTraits {
    typedef std::tuple<> base_function_traits;
  };

  struct Process {
    struct LocalState {
      LocalState(Process&, galois::PerIterAllocTy&) {}
    };

    struct DeterministicId {
      uintptr_t operator()(size_t x) const { return x; }
    };

    typedef std::tuple<galois::per_iter_alloc, galois::intent_to_read,
                       galois::local_state<LocalState>,
                       galois::det_id<DeterministicId>>
        ikdg_function_traits;
    typedef std::tuple<galois::per_iter_alloc, galois::fixed_neighborhood,
                       galois::local_state<LocalState>,
                       galois::det_id<DeterministicId>>
        add_remove_function_traits;
    typedef std::tuple<> nondet_function_traits;

    SyncALSalgo& self;
    Graph& g;
    MT& WT;
    MT& HT;
    PerThrdXTX& xtxs;
    PerThrdV& rhs;

    Process(SyncALSalgo& self, Graph& g, MT& WT, MT& HT, PerThrdXTX& xtxs,
            PerThrdV& rhs)
        : self(self), g(g), WT(WT), HT(HT), xtxs(xtxs), rhs(rhs) {}

    void operator()(size_t col, galois::UserContext<size_t>& ctx) {
      self.update(g, col, WT, HT, xtxs, rhs);
    }
  };

  void operator()(Graph& g, const StepFunction&) {
    if (!useSameLatentVector) {
      galois::gWarn("Results are not deterministic with different numbers of "
                    "threads unless -useSameLatentVector is true");
    }
    galois::TimeAccumulator elapsed;
    elapsed.start();

    // Find W, H that minimize ||W H^T - A||_2^2 by solving alternating least
    // squares problems:
    //   (W^T W + lambda I) H^T = W^T A (solving for H^T)
    //   (H^T H + lambda I) W^T = H^T A^T (solving for W^T)
    MT WT{LATENT_VECTOR_SIZE, NUM_ITEM_NODES};
    MT HT{LATENT_VECTOR_SIZE, g.size() - NUM_ITEM_NODES};

    initializeA(g);
    copyFromGraph(g, WT, HT);

    double last = -1.0;
    galois::StatTimer updateTime("UpdateTime");
    galois::StatTimer copyTime("CopyTime");
    galois::StatTimer totalExecTime("totalExecTime");
    galois::StatTimer totalAlgoTime("Time");
    PerThrdXTX xtxs;
    PerThrdV rhs;

    totalAlgoTime.start();
    for (unsigned round = 1;; ++round) {

      totalExecTime.start();
      updateTime.start();

      typedef galois::worklists::PerThreadChunkLIFO<ALS_CHUNK_SIZE> WL_ty;
      galois::for_each(
          galois::iterate(boost::counting_iterator<size_t>(0),
                          boost::counting_iterator<size_t>(NUM_ITEM_NODES)),
          Process(*this, g, WT, HT, xtxs, rhs), galois::wl<WL_ty>(),
          galois::loopname("syncALS-users"));
      galois::for_each(
          galois::iterate(boost::counting_iterator<size_t>(NUM_ITEM_NODES),
                          boost::counting_iterator<size_t>(g.size())),
          Process(*this, g, WT, HT, xtxs, rhs), galois::wl<WL_ty>(),
          galois::loopname("syncALS-items"));

      updateTime.stop();

      copyTime.start();
      copyToGraph(g, WT, HT);
      copyTime.stop();
      totalExecTime.stop();

      double error = sumSquaredError(g);
      elapsed.stop();
      std::cout << "R: " << round << " elapsed (ms): " << elapsed.get()
                << " RMSE (R " << round
                << "): " << std::sqrt(error / g.sizeEdges()) << "\n";
      elapsed.start();

      if (fixedRounds <= 0 && round > 1 &&
          std::abs((last - error) / last) < tolerance)
        break;
      if (fixedRounds > 0 && round >= fixedRounds)
        break;

      last = error;
    } // end for
    totalAlgoTime.stop();
  }
};

#endif // HAS_EIGEN

/**
 * Initializes latent vector with random values and returns basic graph
 * parameters.
 *
 * @tparam Graph type of g
 * @param g Graph to initialize
 * @returns number of item nodes, i.e. nodes with outgoing edges. They should
 * be the first nodes of the graph in memory
 */

template <typename Graph>
size_t initializeGraphData(Graph& g) {
  galois::gPrint("initializeGraphData\n");
  galois::StatTimer initTimer("InitializeGraph");
  initTimer.start();
  double top = 1.0 / std::sqrt(LATENT_VECTOR_SIZE);
  galois::substrate::PerThreadStorage<std::mt19937> gen;

#if __cplusplus >= 201103L || defined(HAVE_CXX11_UNIFORM_INT_DISTRIBUTION)
  std::uniform_real_distribution<LatentValue> dist(0, top);
#else
  std::uniform_real<LatentValue> dist(0, top);
#endif

  if (useDetInit) {
    galois::do_all(galois::iterate(g), [&](typename Graph::GraphNode n) {
      auto& data = g.getData(n);
      auto val   = genVal(n);
      for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
        data.latentVector[i] = val;
      }
    });
  } else {
    galois::do_all(galois::iterate(g), [&](typename Graph::GraphNode n) {
      auto& data = g.getData(n);

      // all threads initialize their assignment with same generator or
      // a thread local one
      if (useSameLatentVector) {
        std::mt19937 sameGen;
        for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
          data.latentVector[i] = dist(sameGen);
        }
      } else {
        for (int i = 0; i < LATENT_VECTOR_SIZE; i++) {
          data.latentVector[i] = dist(*gen.getLocal());
        }
      }
    });
  }

   auto activeThreads = galois::getActiveThreads();
   std::vector<uint32_t> largestNodeID_perThread(activeThreads);

    galois::on_each([&](unsigned tid, unsigned nthreads) {
      unsigned int block_size = g.size() / nthreads;
      if ((g.size() % nthreads) > 0)
        ++block_size;

      uint32_t start = tid * block_size;
      uint32_t end   = (tid + 1) * block_size;
      if (end > g.size())
        end = g.size();

      largestNodeID_perThread[tid] = 0;
      for (uint32_t i = start; i < end; ++i) {
        if(std::distance(g.edge_begin(i), g.edge_end(i))) {
          if(largestNodeID_perThread[tid] < i)
            largestNodeID_perThread[tid] = i;
          }
      }
    });

    uint32_t largestNodeID = 0;
    for(uint32_t t = 0; t < activeThreads; ++t){
      if(largestNodeID < largestNodeID_perThread[t])
        largestNodeID = largestNodeID_perThread[t];
    }
    size_t numItemNodes = largestNodeID + 1;

  initTimer.stop();
  return numItemNodes;
}

StepFunction* newStepFunction() {
  switch (learningRateFunction) {
  case Step::intel:
    return new IntelStepFunction;
  case Step::purdue:
    return new PurdueStepFunction;
  case Step::bottou:
    return new BottouStepFunction;
  case Step::inverse:
    return new InverseStepFunction;
  case Step::bold:
    return new BoldStepFunction;
  default:
    GALOIS_DIE("unknown step function");
  }
}

template <typename Graph>
void writeBinaryLatentVectors(Graph& g, const std::string& filename) {
  std::ofstream file(filename);
  for (auto ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    auto& v = g.getData(*ii).latentVector;
    for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {
      file.write(reinterpret_cast<char*>(&v[i]), sizeof(v[i]));
    }
  }
  file.close();
}

template <typename Graph>
void writeAsciiLatentVectors(Graph& g, const std::string& filename) {
  std::ofstream file(filename);
  for (auto ii = g.begin(), ei = g.end(); ii != ei; ++ii) {
    auto& v = g.getData(*ii).latentVector;
    for (int i = 0; i < LATENT_VECTOR_SIZE; ++i) {
      file << v[i] << " ";
    }
    file << "\n";
  }
  file.close();
}

/**
 * Run the provided algorithm (provided through the template argument).
 *
 * @param Algo algorithm to run
 */
template <typename Algo>
void run() {
  typename Algo::Graph g;
  Algo algo;

  galois::runtime::reportNumaAlloc("NumaAlloc0");

  // Bipartite graph in general graph data structure should be following:
  // * items are the first m nodes
  // * users are the next n nodes
  // * only items have outedges
  algo.readGraph(g);

  galois::runtime::reportNumaAlloc("NumaAlloc1");

  // initialize latent vectors and get number of item nodes
  NUM_ITEM_NODES = initializeGraphData(g);

  galois::runtime::reportNumaAlloc("NumaAlloc2");

  std::cout << "num users: " << g.size() - NUM_ITEM_NODES
            << " num items: " << NUM_ITEM_NODES
            << " num ratings: " << g.sizeEdges() << "\n";

  std::unique_ptr<StepFunction> sf{newStepFunction()};
  std::cout << "latent vector size: " << LATENT_VECTOR_SIZE
            << " algo: " << algo.name() << " lambda: " << lambda;

  if (algo.isSgd()) {
    std::cout << " learning rate: " << learningRate
              << " decay rate: " << decayRate
              << " step function: " << sf->name();
  }

  std::cout << "\n";

  if (!skipVerify) {
    verify(g, "Initial");
  }

  // algorithm call
  galois::StatTimer totalTimer("Total Time");
  totalTimer.start();
  algo(g, *sf);
  totalTimer.stop();

  if (!skipVerify) {
    verify(g, "Final");
  }

  if (outputFilename != "") {
    std::cout << "Writing latent vectors to " << outputFilename << "\n";
    switch (outputType) {
    case OutputType::binary:
      writeBinaryLatentVectors(g, outputFilename);
      break;
    case OutputType::ascii:
      writeAsciiLatentVectors(g, outputFilename);
      break;
    default:
      GALOIS_DIE("Invalid output type for latent vector output");
    }
  }

  galois::runtime::reportNumaAlloc("NumaAlloc");
}

int main(int argc, char** argv) {
  galois::SharedMemSys G;
  LonestarStart(argc, argv, name, desc, url);

  switch (algo) {
#ifdef HAS_EIGEN
  case Algo::syncALS:
    run<SyncALSalgo>();
    break;
  case Algo::simpleALS:
    run<SimpleALSalgo>();
    break;
#endif
  case Algo::sgdByItems:
    run<SGDItemsAlgo>();
    break;
  case Algo::sgdByEdges:
    run<SGDEdgeItem>();
    break;
  case Algo::sgdBlockEdge:
    run<SGDBlockEdgeAlgo>();
    break;
  case Algo::sgdBlockJump:
    run<SGDBlockJumpAlgo>();
    break;
  default:
    GALOIS_DIE("unknown algorithm");
    break;
  }

  return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-hyperbolic
    Function object implementing sinhc capabilities

    Returns hyperbolic cardinal sine: \f$\frac{\sinh(x)}{x}\f$.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = sinhc(x);
    @endcode

    is similar to:

    @code
    T r = sinh(x)/x;
    @endcode

    @see sinh

  **/
  const boost::dispatch::functor<tag::sinhc_> sinhc = {};
} }
#endif

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

#endif


#include <string.h>
#include <math.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>

extern "C" {
void dsyevr_ (
	char	*jobz,
	char	*range,
	char	*uplo,
	int	*n,
	double	*a,
	int	*lda,
	double	*vl,
	double	*vu,
	int	*il,
	int	*iu,
	double	*abstol,
	int	*m,
	double	*w,
	double	*z,
	int	*ldz,
	int	*isuppz,
	double	*work,
	int	*lwork,
	int	*iwork,
	int	*liwork,
	int	*info
);
}

using namespace std;

template <class F> class Point;

template <class F> class PointBase {
public:
	vector<F>	x;

	template <class G> PointBase<F> & operator =(Point<G> const &);
//	template <class G> PointBase<double> & operator =(PointBase<G> const &);
	template <class G> PointBase<F> & operator +=(PointBase<G> const &);

	PointBase<F>(void) {};

	PointBase<F>(int s) {
		x.resize(s);
		for (int i=0; i<s; i++) x[i] = 0.;
	}

	template <class G> PointBase<F>(PointBase<G> const & g) {
		for (int i=0; i<g.x.size(); i++) x.push_back(g.x[i]);
	}

	PointBase<F> & operator *= (double d) {
		for (int i=0; i<x.size(); i++) x[i] *= d;
	}

	F dotp(vector<F> const &v) const {
		F	ret = 0;

		for (int i=0; i<x.size(); i++) ret += v[i]*x[i];

		return	ret;
	}
};

template <class F> template <class G> PointBase<F> & PointBase<F>::operator=(Point<G> const &f)
{
	x = f.x;

	return *this;
}

template <class F> template <class G> PointBase<F> & PointBase<F>::operator +=(PointBase<G>  const & r)
{
	for (int i=0; i<x.size(); i++) x[i] += r.x[i];
	return *this;
}

template <class F> class Point: public PointBase<F> {
public:
	F	chi2;
	vector<string>	types;

	bool	load(istream &);
};

template <class F> bool Point<F>::load(istream & in)
{
	char	line[BUFSIZ];
	this->x.clear();
	string	s;

#define CLUE	"    Function number"

	while (! in.getline(line,sizeof line).eof() && strlen(line) > 0 && strncmp(line,CLUE,sizeof(CLUE)-1)) ;

	stringstream	parse(line);

//    Function number   284    F =  1.0973091125D+00
	parse >> s >> s >> s >> s >> s >> chi2;

	while (! in.getline(line,sizeof line).eof() && strlen(line) > 0) {

		F	xx[5] = { FP_NAN, FP_NAN, FP_NAN, FP_NAN, FP_NAN };

		for (int j=0; line[j]; j++) if (line[j] == 'D') line[j] = 'e';
		int n = sscanf(line,"%f %f %f %f %f",xx,xx+1,xx+2,xx+3,xx+4);
		for (int j=0; j<n; j++) this->x.push_back(xx[j]);
	}

	return !in.eof();
}

class Select {
public:
	float	chi2;

	Select() {
		chi2	= 1e38;
	}

	bool match(Point<float> const & p) const {
		if (p.chi2  > chi2) return false;

		return true;
	}
};

#include <boost/program_options.hpp>
namespace po = boost::program_options;

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

	po::options_description desc("Options");
	desc.add_options()
		("help", "help message")
		("chi2", po::value<float>()->default_value(100000.0), "select points for dimension reduction - smaller chi2 only")
		("dim", po::value<int>()->default_value(2), "reduce to this number of dimensions")
		("all", "map all points, not only those selected by chi2")
;

	po::variables_map opt;
	try { po::store(po::parse_command_line(argc, argv, desc), opt); }
	catch (exception &e) {
		cerr << argv[0] << ": " << e.what() << endl;
		return 1;
	}
	po::notify(opt);

	if (opt.count("help")) { cerr << desc; return 1; }

	Select	sel;
	sel.chi2 = opt["chi2"].as<float>();

	int	dimreduce = opt["dim"].as<int>();
	bool allpoints = opt.count("all");

	vector<Point<float> >	pt;
	Point<float>		p;

	while (p.load(cin)) {
		pt.push_back(p);
		if (pt.size() % 100 == 0) cerr << pt.size() << " points read      \r" << flush;
	}
	cerr << pt.size() << " points read      " << endl;

	PointBase<double> sum(pt[0].x.size());
	vector<PointBase<float> > matched;
	long nmatch = 0;

	for (int i=0; i<pt.size(); i++)
		if (sel.match(pt[i])) {
			PointBase<float>	pb = pt[i];
			matched.push_back(pb);
			sum += pt[i];
			nmatch++;
		}

	cerr << nmatch << " points match criteria" << endl;

	PointBase<float> minusavg(sum);
	double invnmatch = 1. / nmatch;
        minusavg *= - invnmatch;

	for (int i=0; i<matched.size(); i++) matched[i] += minusavg;

	int	npar = pt[0].x.size(), npar2 = npar*npar;
	double *cov = new double[npar2]; // lower triangular
	vector<double>	par(npar);

	for (int k=0; k<npar2; k++) cov[k] = 0.;

	for (int n=0; n < nmatch; n++) {
		for (int i=0; i<npar; i++) {
			par[i] = matched[n].x[i];
		}

		for (int i=0; i<npar; i++) {
			int	icol = i*npar;
			for (int j=i; j<npar; j++) cov[icol + j] += par[i] * par[j];
		}
	}

	for (int k=0; k<npar2; k++) cov[k] *= invnmatch;

	double zero = 0., abstol = 0. ;
	int	izero = 0,lwork = npar * 100;
	int	m, *isuppz = new int[2*npar], info, liwork = 10*npar, *iwork = new int[liwork];
	double	*w = new double[npar], *z = new double[npar2], *work = new double[lwork];

	dsyevr_("V", // JOBZ
		"A", // RANGE
		"L", // UPLO
		&npar, // N
		cov, // A
		&npar, // LDA
		&zero, // VL
		&zero, // VU
		&izero, // IL
		&izero, // IU
		&abstol,
		&m,
		w,
		z,
		&npar, // LDZ
		isuppz,
		work,
		&lwork,
		iwork,
		&liwork,
		&info);

	cerr << "dsyevr() = " << info << endl;
	if (info) return 1;

	vector<float>	evnorm(npar);
	float	evsum = 0, evcum = 0;

	cerr << "raw eigenvalues: " << endl;
	for (int i=0; i<npar; i++) { cerr << w[i] << " "; evsum += w[i]; }
	cerr << endl;

	cerr << "normalized reverse cummulative: " << endl;
	for (int i=0; i<npar; i++) {
		evnorm[i] = w[npar-i-1] / evsum;
		evcum += evnorm[i];
		cerr << evcum << " ";
	}
	cerr << endl;

	vector<vector<float> > evec(dimreduce);

	for (int i=0; i<dimreduce; i++) {
		evec[i].resize(npar);
		int	icol = (npar-i-1) * npar;
		float	dotp = 0, norm = 0.;
		for (int j=0; j<npar; j++) {
			float	e = evec[i][j] = z[icol+j];
			norm += e*e;
		}
		norm = 1./sqrt(norm)/sqrt(1.*npar);;

		cerr << "eigenvector[" << i << "]:" ;
		for (int j=0; j<npar; j++) {
			cerr << evec[i][j] * norm << " ";
			dotp += evec[i][j] * norm;
		}
		cerr << endl << "dotprod with diagonal: " << dotp << endl;
	}

	for (int i=0; i<pt.size(); i++)
		if (allpoints || sel.match(pt[i])) {
			for (int j=0; j<dimreduce; j++) {
				cout << pt[i].dotp(evec[j]) << ", ";
			}
			cout << pt[i].chi2;
			cout << endl;
		}

	delete [] isuppz;
	delete [] iwork;
	delete [] w;
	delete [] z;
	delete [] work;

	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_COST_BASE_HPP_
#define CROCODDYL_CORE_COST_BASE_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/core/data-collector-base.hpp"
#include "crocoddyl/core/activation-base.hpp"
#include "crocoddyl/core/activations/quadratic.hpp"

namespace crocoddyl {

/**
 * @brief Abstract class for cost models
 *
 * In Crocoddyl, a cost model is defined by the scalar activation function \f$a(\cdot)\f$ and by the residual function
 * \f$\mathbf{r}(\cdot)\f$ as follows: \f[ cost = a(\mathbf{r}(\mathbf{x}, \mathbf{u})), \f] where
 * the residual function depends on the state point \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state manifold
 * described with a `nq`-tuple, its velocity \f$\dot{\mathbf{x}}\in T_{\mathbf{x}}\mathcal{X}\f$ that belongs to
 * the tangent space with `nv` dimension, and the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$. The residual vector
 * is defined by \f$\mathbf{r}\in\mathbb{R}^{nr}\f$ where `nr` describes its dimension in the Euclidean space. On the
 * other hand, the activation function builds a cost value based on the definition of the residual vector. The residual
 * vector has to be specialized in a derived classes.
 *
 * The main computations are carring out in `calc` and `calcDiff` routines. `calc` computes the cost (and its residual)
 * and `calcDiff` computes the derivatives of the cost function (and its residual). Concretely speaking, `calcDiff`
 * builds a linear-quadratic approximation of the cost function with the form: \f$\mathbf{l_x}\in\mathbb{R}^{ndx}\f$,
 * \f$\mathbf{l_u}\in\mathbb{R}^{nu}\f$, \f$\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$,
 * \f$\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\f$, \f$\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the
 * Jacobians and Hessians, respectively.
 * Additionally, it is important remark that `calcDiff()` computes the derivates using the latest stored values by
 * `calc()`. Thus, we need to run first `calc()`.
 *
 * \sa `StateAbstractTpl`, `ActivationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
 */
template <typename _Scalar>
class CostModelAbstractTpl {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
  typedef StateAbstractTpl<Scalar> StateAbstract;
  typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
  typedef ActivationModelQuadTpl<Scalar> ActivationModelQuad;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  /**
   * @brief Initialize the cost model
   *
   * @param[in] state       State of the multibody system
   * @param[in] activation  Activation model
   * @param[in] nu          Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation,
                       const std::size_t nu);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state       State of the multibody system
   * @param[in] activation  Activation model
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, boost::shared_ptr<ActivationModelAbstract> activation);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * We use `ActivationModelQuadTpl` as a default activation model (i.e. \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$)
   *
   * @param[in] state  State of the multibody system
   * @param[in] nr     Dimension of residual vector
   * @param[in] nu     Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr, const std::size_t nu);

  /**
   * @copybrief CostModelAbstractTpl()
   *
   * We use `ActivationModelQuadTpl` as a default activation model (i.e. \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$).
   * Furthermore, the default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
   *
   * @param[in] state  State of the multibody system
   * @param[in] nr     Dimension of residual vector
   * @param[in] nu     Dimension of control vector
   */
  CostModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nr);
  virtual ~CostModelAbstractTpl();

  /**
   * @brief Compute the cost value and its residual vector
   *
   * @param[in] data  Cost data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                    const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Compute the Jacobian and Hessian of cost and its residual vector
   *
   * It computes the Jacobian and Hessian of the cost function. It assumes that `calc()` has been run first.
   *
   * @param[in] data  Cost data
   * @param[in] x     State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
   * @param[in] u     Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
   */
  virtual void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
                        const Eigen::Ref<const VectorXs>& u) = 0;

  /**
   * @brief Create the cost data
   *
   * The default data contains objects to store the values of the cost, residual vector and their derivatives (first
   * and second order derivatives). However, it is possible to specialized this function is we need to create
   * additional data, for instance, to avoid dynamic memory allocation.
   *
   * @param data  Data collector
   * @return the cost data
   */
  virtual boost::shared_ptr<CostDataAbstract> createData(DataCollectorAbstract* const data);

  /**
   * @copybrief calc()
   *
   * @param[in] data  Cost data
   * @param[in] x     State point
   */
  void calc(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @copybrief calcDiff()
   *
   * @param[in] data  Cost data
   * @param[in] x     State point
   */
  void calcDiff(const boost::shared_ptr<CostDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);

  /**
   * @brief Return the state
   */
  const boost::shared_ptr<StateAbstract>& get_state() const;

  /**
   * @brief Return the activation model
   */
  const boost::shared_ptr<ActivationModelAbstract>& get_activation() const;

  /**
   * @brief Return the dimension of the control input
   */
  std::size_t get_nu() const;

  /**
   * @brief Modify the cost reference
   */
  template <class ReferenceType>
  void set_reference(ReferenceType ref);

  /**
   * @brief Return the cost reference
   */
  template <class ReferenceType>
  ReferenceType get_reference() const;

 protected:
  /**
   * @copybrief set_reference()
   */
  virtual void set_referenceImpl(const std::type_info&, const void*);

  /**
   * @copybrief get_reference()
   */
  virtual void get_referenceImpl(const std::type_info&, void*) const;

  boost::shared_ptr<StateAbstract> state_;                 //!< State description
  boost::shared_ptr<ActivationModelAbstract> activation_;  //!< Activation model
  std::size_t nu_;                                         //!< Control dimension
  VectorXs unone_;                                         //!< No control vector
};

template <typename _Scalar>
struct CostDataAbstractTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  typedef _Scalar Scalar;
  typedef MathBaseTpl<Scalar> MathBase;
  typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
  typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
  typedef typename MathBase::VectorXs VectorXs;
  typedef typename MathBase::MatrixXs MatrixXs;

  template <template <typename Scalar> class Model>
  CostDataAbstractTpl(Model<Scalar>* const model, DataCollectorAbstract* const data)
      : shared(data),
        activation(model->get_activation()->createData()),
        cost(Scalar(0.)),
        Lx(model->get_state()->get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
        Lxu(model->get_state()->get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()),
        r(model->get_activation()->get_nr()),
        Rx(model->get_activation()->get_nr(), model->get_state()->get_ndx()),
        Ru(model->get_activation()->get_nr(), model->get_nu()) {
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
    r.setZero();
    Rx.setZero();
    Ru.setZero();
  }
  virtual ~CostDataAbstractTpl() {}

  DataCollectorAbstract* shared;
  boost::shared_ptr<ActivationDataAbstract> activation;
  Scalar cost;
  VectorXs Lx;
  VectorXs Lu;
  MatrixXs Lxx;
  MatrixXs Lxu;
  MatrixXs Luu;
  VectorXs r;
  MatrixXs Rx;
  MatrixXs Ru;
};

}  // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/cost-base.hxx"

#endif  // CROCODDYL_CORE_COST_BASE_HPP_


#include "Tunneling1D.h"
#include <cmath>
#include <iostream>
// #include <gsl/gsl_errno.h>
// #include <gsl/gsl_min.h>
// #include <gsl/gsl_roots.h>
#include "GSL_Wraper.h"
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/bessel.hpp>

using namespace std;
using namespace boost::math;

// typedef double (*root_func)(double,void*);
// double find_root_gsl_wraper(root_func func, void *params, double x_max, double x_min)
// {
//     int status;
//     int iter = 0, max_iter = 100;
//     const gsl_root_fsolver_type *T = gsl_root_fsolver_brent;
//     gsl_root_fsolver *s = gsl_root_fsolver_alloc(T);
//     double r;
//     double r_min = x_min;
//     double r_max = x_max;

//     gsl_function F;
//     // struct param_initialConditions params = {this, phi0, dV0, d2V0, phi_absMin, delta_phi_cutoff};

//     F.function = func;
//     F.params = params;

//     gsl_root_fsolver_set(s, &F, r_min, r_max);

//     do
//     {
//         iter++;
//         status = gsl_root_fsolver_iterate(s);
//         r      = gsl_root_fsolver_root(s);
//         r_min  = gsl_root_fsolver_x_lower(s);
//         r_max  = gsl_root_fsolver_x_upper(s);
//         status = gsl_root_test_interval(r_min,r_max,1e-8,1e-10);
//     } while (status == GSL_CONTINUE && iter < max_iter);

//     gsl_root_fsolver_free(s);
//     return r;
// }

// * function used by RungeKutta
VD func_for_rkqc(double r, VD y, void *param)
{
    Tunneling1D *mod = (Tunneling1D*) param;
    return mod->equationOfMotion(r,y);
}
Tunneling1D::Tunneling1D(double absMin, double metaMin, ScalarFunction V_, dScalarFunction dV_, HM d2V_, double dim, double phi_eps_rel_)
{
    V = V_;
    dV = dV_;
    d2V = d2V_;

    _rk_calculator.SetDOF(2);
    _rk_calculator.SetODE(func_for_rkqc);
    _rk_calculator.SetParams(this);

    phi_absMin = absMin;
    phi_metaMin = metaMin;
    phi_bar = findBarrierLocation();
    // cout<<"Barrier at "<<phi_bar<<endl;

    rscale = findRScale();
    // cout<<"r-scale: "<<rscale<<endl;

    Spatial_Dim = dim;
    alpha = dim-1;

    phi_eps_rel = phi_eps_rel_;
    phi_eps_abs = phi_eps_rel*abs(phi_absMin-phi_metaMin);
}
void Tunneling1D::SetMinima(double absMin, double metaMin)
{
    phi_absMin = absMin;
    phi_metaMin = metaMin;
}
void Tunneling1D::SetPotential(ScalarFunction potential)
{
    V = potential;
}
void Tunneling1D::SetdPotential(dScalarFunction dpotential)
{
    dV = dpotential;
}
void Tunneling1D::SetHM(HM d2potential)
{
    d2V = d2potential;
}
void Tunneling1D::SetPhiAtBarrier(double phibar)
{
    phi_bar = phibar;
}
void Tunneling1D::SetSpatialDim(double dim)
{
    Spatial_Dim = dim;
    alpha = Spatial_Dim - 1;
}
void Tunneling1D::SetPrecision(double eps_rel)
{
    phi_eps_rel = eps_rel;
    // if (isnan(phi_absMin) || isnan(phi_metaMin))
    // {
    //     phi_eps_abs = NAN;
    //     cout<<"Please Set the Location of the Minimum First"<<endl;
    // }
    // else
    // {
    //     phi_eps_abs = phi_eps_rel * abs(phi_absMin - phi_metaMin);
    // }
}
double Tunneling1D::dV_from_absMin(double delta_phi)
{
    double phi = phi_absMin + delta_phi;
    double T = 0;
    double dV_f = dV({phi},&T)[0];

    double dV_d = d2V({phi},&T)[0][0] * delta_phi;

    double blend_factor = exp(-pow(delta_phi/phi_eps_abs,2));

    return dV_f*(1-blend_factor) + dV_d*blend_factor;
}
double Tunneling1D::findBarrierLocation()
{
    double phi_tol = abs(phi_absMin-phi_metaMin)*1e-12;
    double T = 0;
    double V_meta = V({phi_metaMin},&T);
    double phiH = phi_metaMin;
    double phiL = phi_absMin;
    double phiM = (phiH + phiL)/2;

    double V0;
    while (abs(phiH-phiL) > phi_tol)
    {
        V0 = V({phiM},&T);
        if (V0 > V_meta)
        {
            phiH = phiM;
        }
        else
        {
            phiL = phiM;
        }
        phiM = (phiH + phiL)/2;
    }
    return phiM;
}
double func_for_findRScale(double x, void *params)
{
    Tunneling1D * mod = (Tunneling1D*)params;
    return -mod->VvalatX(x);
}
double Tunneling1D::findRScale()
{
    if (std::isnan(phi_bar)) findBarrierLocation();
    double phi_tol = abs(phi_bar - phi_metaMin)*1e-6;
    double x1 = min(phi_bar,phi_metaMin);
    double x2 = max(phi_bar,phi_metaMin);

    double phi_bar_top = find_min_arg_gsl_wraper(func_for_findRScale,this,x2,x1,phi_tol);

    if (phi_bar_top + phi_tol > x2 || phi_bar_top - phi_tol < x1)
    {
        cout<<"In findRScale: No Barrier for the potential: can't find the top position."<<endl;
    }

    double Vtop = VvalatX(phi_bar_top) - VvalatX(phi_metaMin);
    double xtop = phi_bar_top - phi_metaMin;

    if (Vtop <= 0)
    {
        cout<<"In findRScale: No Barrier for the potential: non-positive barrier height."<<endl;
    }
    // This is the `time'-scale for a harmonic oscillation (Approximating the potential by a quadratic potential).
    return abs(xtop)/sqrt(abs(2*Vtop));
}
tuple<double, double> Tunneling1D::exactSolution(double r, double phi0, double dV_, double d2V_)
{
    // ! Find phi(r) (and dphi(r)) given phi(0) assuming a quadractic potential
    double beta = sqrt(abs(d2V_));
    double beta_r = beta*r;
    double nu = (alpha - 1.0)/2.0;

    double phi = 0;
    double dphi = 0;
    double tmp;
    if (beta_r < 1e-2)
    {
        // Using the expansion to approximate the Bessel function
        double s = d2V_>0?1:-1;
        for (int k = 1; k < 4; k++)
        {
            tmp = pow(beta_r/2,2*k-2)*pow(s,k)/(tgamma((k+1)*1.0)*tgamma(k+1+nu));
            phi += tmp;
            dphi += tmp*(2*k);
        }
        phi *= tgamma(nu+1)*r*r*dV_*s/4;
        dphi *= tgamma(nu+1)*r*dV_*s/4;
        phi += phi0;
    }
    else if (d2V_ > 0)
    {
        // cout<<"beta_r: "<<beta_r<<endl;
        try
        {
            phi = (tgamma(nu+1)/pow(beta_r/2,nu)*cyl_bessel_i(nu,beta_r)-1)*dV_/d2V_;
            dphi = -nu/r/pow(beta_r/2,nu)*cyl_bessel_i(nu,beta_r);
            dphi += beta/2/pow(beta_r/2,nu)*(cyl_bessel_i(nu-1,beta_r)+cyl_bessel_i(nu+1,beta_r));
            dphi *= tgamma(nu+1)*dV_/d2V_;
            phi += phi0;
        }
        catch(const std::overflow_error& e)
        {
            // std::cerr << e.what() << '\n';
            // just ignore the overflow
            phi = INFINITY;
            dphi = INFINITY;
        }
    }
    else
    {
        phi = (tgamma(nu+1)/pow(beta_r/2,nu)*cyl_bessel_j(nu,beta_r)-1)*dV_/d2V_;
        dphi = -nu/r/pow(beta_r/2,nu)*cyl_bessel_j(nu,beta_r);
        dphi += beta/2/pow(beta_r/2,nu)*(cyl_bessel_j(nu-1,beta_r)-cyl_bessel_j(nu+1,beta_r));
        dphi *= tgamma(nu+1)*dV_/d2V_;
        phi += phi0;
    }
    return make_tuple(phi,dphi);
}
struct param_initialConditions
{
    Tunneling1D *tun;
    double phi0;
    double dV0;
    double d2V0;

    double phi_absMin;
    double delta_phi_cutoff;
};
double func_for_initialConditions(double r, void *params)
{
    param_initialConditions *mod = (param_initialConditions*)params;
    double phi0 = mod->phi0;
    double dV0 = mod->dV0;
    double d2V0 = mod->d2V0;
    double phi_absMin = mod->phi_absMin;
    double delta_phi_cutoff = mod->delta_phi_cutoff;
    double phir,dphir;
    std::tie(phir,dphir) = (mod->tun)->exactSolution(r,phi0,dV0,d2V0);
    return abs(phir - phi_absMin) - abs(delta_phi_cutoff);
}
tuple<double, double, double> Tunneling1D::initialConditions(double delta_phi0, double rmin, double delta_phi_cutoff)
{
    /*
    * Find the initial conditions for the ODE integration.
    *
    * The instanton equations of motion are singular at `r=0`,
    * so we need to start the integration at some larger radius.
    * This function finds the value `r0` such that `phi(r0) = phi_cutoff`.
    * If there is no such value, it returns the intial conditions at `rmin`.
    */

    double T = 0;
    double phi0 = phi_absMin + delta_phi0;
    double dV0 = dV_from_absMin(delta_phi0);
    double d2V0 = d2V({phi0},&T)[0][0];

    double phi_rmin, dphi_rmin;
    std::tie(phi_rmin, dphi_rmin) = exactSolution(rmin, phi0, dV0, d2V0);
    if (abs(phi_rmin - phi_absMin) > abs(delta_phi_cutoff))
    {
        return make_tuple(rmin, phi_rmin, dphi_rmin);
    }
    if (sign(dphi_rmin) != sign(delta_phi0))
    {
        return make_tuple(rmin, phi_rmin, dphi_rmin);
    }

    double r_cur = rmin;
    double r_last = rmin;

    double phi, dphi;
    r_last = r_cur;
    r_cur *= 10;
    while (std::isfinite(r_cur))
    {
        std::tie(phi, dphi) = exactSolution(r_cur, phi0, dV0, d2V0);
        if (!std::isfinite(phi))
        {
            r_cur = (r_last + r_cur)/2.0;
            continue;
        }
        if (abs(phi - phi_absMin) > abs(delta_phi_cutoff))
        {
            break;
        }
        r_last = r_cur;
        r_cur *= 10;
    }
    struct param_initialConditions params = {this, phi0, dV0, d2V0, phi_absMin, delta_phi_cutoff};
    // cout<<"Before root finding"<<endl;
    // cout<<"r_cur="<<r_cur<<" r_last="<<r_last<<endl;
    double r = find_root_gsl_wraper(&func_for_initialConditions,&params,r_cur,r_last);
    // cout<<"After root finding"<<endl;

    std::tie(phi,dphi) = exactSolution(r,phi0,dV0,d2V0);
    return make_tuple(r,phi,dphi);
}
VD Tunneling1D::equationOfMotion(double r, VD y)
{
    VD res(2);
    double T = 0;
    res[0] = y[1];
    res[1] = dV({y[0]},&T)[0]-alpha*y[1]/r;
    return res;
}
struct cubic_param
{
    double y0;
    double dy0;
    double y1;
    double dy1;
    double diff;
};

double cubicInterpolation(double x, void *param)
{
    cubic_param* mod = (cubic_param*)param;
    double mt = 1-x;
    double c3 = mod->y1;
    double c2 = mod->y1 - mod->dy1/3.0;
    double c1 = mod->y0 + mod->dy0/3.0;
    double c0 = mod->y0;
    return c0*pow(mt,3) + 3*c1*mt*mt*x + 3*c2*mt*x*x + c3*pow(x,3) - mod->diff;
}
tuple<double, VD, CONVERGENCETYPE> Tunneling1D::integrateProfile(double r0, VD y0, double dr0, double epsfrac, double epsabs, double drmin, double rmax)
{
    VD y_final_value = {phi_metaMin,0};
    VD y_diff;
    double dr_guess = dr0;
    double dr_did,dr_next;
    double r = r0;
    VD y = y0;
    VD dydr = equationOfMotion(r,y);
    double r_cache;
    VD y_cache;
    VD dydr_cache;
    VD y_scale;
    VD y_inter(2);
    int ysign = sign(y0[0]-phi_metaMin);
    rmax += r0;

    CONVERGENCETYPE convergQ = NONE;
    cubic_param inter_param;
    double x;
    while (true)
    {
        y_scale = abs(y)+abs(dydr*dr_guess);
        r_cache = r;
        y_cache = y;
        dydr_cache = dydr;
        // cout<<"\t\t----"<<endl;
        // cout<<"\t\t"<<r_cache<<"  "<<y_cache[0]<<"  "<<y_cache[1]<<"  "<<dydr_cache[0]<<"  "<<dydr_cache[1]<<"  "<<dr_guess<<"  "<<epsabs<<endl;
        _rk_calculator._RKQC_SingleStep(r_cache,y_cache,dydr_cache,dr_guess,epsabs,y_scale,dr_did,dr_next);
        dydr_cache = equationOfMotion(r_cache,y_cache);

        y_diff = abs(y_cache-y_final_value);
        // cout<<"\t\t"<<r_cache<<"  "<<y_cache[0]<<"  "<<y_cache[1]<<"  "<<ysign<<"  "<<dr_did<<"  "<<dr_next<<endl;
        // cout<<"\t\t\t"<<y_diff[0]<<"/"<<epsabs<<"  "<<y_diff[1]<<"/"<<epsabs<<endl;
        if ( y_diff[0] < epsabs && y_diff[1] < epsabs)
        {
            r = r_cache;
            y = y_cache;
            convergQ = CONVERGED;
            break;
        }

        if (y_cache[1]*ysign > 0)
        {
            // This means the `ball` is heading back, so it will never reach the desired point.
            convergQ = UNDERSHOOT;
            inter_param = {y[1],dydr[1]*dr_did,y_cache[1],dydr_cache[1]*dr_did,0};
            x = find_root_gsl_wraper(&cubicInterpolation,&inter_param,1,0);
            r += dr_did*x;
            y_inter[1] = cubicInterpolation(x,&inter_param);
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,0};
            y_inter[0] = cubicInterpolation(x,&inter_param);
            y = y_inter;
            break;
        }

        if ((y_cache[0]-phi_metaMin)*ysign<0)
        {
            // Already passing the desired ending point
            convergQ = OVERSHOOT;
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,phi_metaMin};
            x = find_root_gsl_wraper(&cubicInterpolation,&inter_param,1,0);
            r += dr_did*x;
            inter_param = {y[1],dydr[1]*dr_did,y_cache[1],dydr_cache[1]*dr_did,0};
            y_inter[1] = cubicInterpolation(x,&inter_param);
            inter_param = {y[0],dydr[0]*dr_did,y_cache[0],dydr_cache[0]*dr_did,0};
            y_inter[0] = cubicInterpolation(x,&inter_param);
            y = y_inter;
            break;
        }

        r = r_cache;
        y = y_cache;
        dydr = dydr_cache;
        dr_guess = dr_next;
    }
    y_diff = abs(y-y_final_value);
    if ( y_diff[0] < epsabs && y_diff[1] < epsabs)
    {
        convergQ = CONVERGED;
    }
    return make_tuple(r,y,convergQ);
}

tuple<VD, VD, VD, double> Tunneling1D::integrateAndSaveProfile(VD R, VD y0, double dr, double epsfrac, double epsabs, double drmin)
{
    int N = R.size();
    double r0 = R[0];
    VVD Yout(y0.size(),VD(N,0));
    Yout[0][0] = y0[0];
    Yout[1][0] = y0[1];
    VD dydr0 = equationOfMotion(r0,y0);
    double Rerr = NAN;

    int i = 1;
    double r = r0;
    VD y = y0;
    VD dydr = dydr0;
    double r_cache;
    VD y_cache;
    VD dydr_cache;
    double dr_guess = dr;
    double dr_did,dr_next;
    VD y_scale(2);
    cubic_param inter_param;
    while (i<N)
    {
        y_scale = abs(y)+abs(dydr*dr_guess);
        r_cache = r;
        y_cache = y;
        dydr_cache = dydr;
        _rk_calculator._RKQC_SingleStep(r_cache,y_cache,dydr_cache,dr_guess,epsabs,y_scale,dr_did,dr_next);
        if (dr_did < drmin)
        {
            y_cache = y + (y_cache-y)*drmin/dr_did;
            dr_did = drmin;
            dr_next = drmin;
            r_cache = r + dr_did;
            if (!(std::isnan(Rerr)))
            {
                Rerr = r_cache;
            }
        }
        dydr_cache = equationOfMotion(r_cache,y_cache);
        if (r < R[i] && R[i] <= r_cache)
        {
            while (i < N && r < R[i] && R[i] <= r_cache)
            {
                double x = (R[i]-r)/dr_did;
                inter_param = {y[0], dr_did*dydr[0], y_cache[0], dr_did*dydr_cache[0], 0};
                Yout[0][i] = cubicInterpolation(x, &inter_param);
                inter_param = {y[1], dr_did*dydr[1], y_cache[1], dr_did*dydr_cache[1], 0};
                Yout[1][i] = cubicInterpolation(x, &inter_param);
                i += 1;
            }
        }

        r = r_cache;
        y = y_cache;
        dydr = dydr_cache;
        dr_guess = dr_next;
    }

    return make_tuple(R,Yout[0],Yout[1],Rerr);
}
tuple<VD,VD,VD,double> Tunneling1D::findProfile(double xguess,double xtol,double phitol,double thinCutoff,int npoints,double rmin, double rmax, int max_interior_pts)
{
    double xmin = xtol*10;
    double xmax = INFINITY;
    double x;
    if (!std::isnan(xguess))
    {
        x = xguess;
    }
    else
    {
        x = - log(abs((phi_bar-phi_absMin)/(phi_metaMin-phi_absMin)));
    }
    // cout<<"Starting point: x = "<<x<<endl;
    double xincrease = 5.0;

    rmin *= rscale;
    double dr0 = rmin;
    double drmin = rmin*1e-2;
    rmax *= rscale;

    double delta_phi = phi_metaMin - phi_absMin;
    double epsabs = abs(delta_phi*phitol);
    double epsfrac = phitol;
    double delta_phi_cutoff = thinCutoff*delta_phi;

    double rf = NAN;
    double delta_phi0;
    double r0_,phi0,dphi0;
    double r0;
    VD y0;
    VD yf;
    CONVERGENCETYPE ctype;
    // cout<<"Starting of the shooting: "<<endl;
    while (true)
    {
        // cout<<"--------"<<endl;
        delta_phi0 = exp(-x)*delta_phi;
        // cout<<"\tdelta_phi0="<<delta_phi0<<endl;
        std::tie(r0_,phi0,dphi0) = initialConditions(delta_phi0,rmin,delta_phi_cutoff);
        // cout<<"\tInitial condition: r0="<<r0_<<"  phi0="<<phi0<<" dphi0="<<dphi0<<endl;
        if ( !std::isfinite(r0_) || !std::isfinite(x))
        {
            if (std::isnan(rf))
            {
                cerr<<"Failed to retrieve initial conditions on the first try"<<endl;
            }
            break;
        }
        r0 = r0_;
        y0 = {phi0,dphi0};
        std::tie(rf,yf,ctype) = integrateProfile(r0,y0,dr0,epsfrac,epsabs,drmin,rmax);
        if (ctype == CONVERGED)
        {
            break;
        }
        else if (ctype == UNDERSHOOT)
        {
            xmin = x;
            x = std::isfinite(xmax)?(xmin+xmax)/2:x*xincrease;
        }
        else if (ctype == OVERSHOOT)
        {
            xmax = x;
            x = (xmin+xmax)/2;
        }

        if (xmax-xmin < xtol)
        {
            break;
        }
    }

    VD R(npoints);
    for (size_t i = 0; i < npoints; i++)
    {
        R[i] = r0 + i*(rf-r0)/(npoints-1);
    }
    VD Phi_ex;
    VD dPhi_ex;
    double Rerr;
    std::tie(R,Phi_ex,dPhi_ex,Rerr)=integrateAndSaveProfile(R,y0,dr0,epsfrac,epsabs,drmin);

    VD R_int;
    if (max_interior_pts < 0)
    {
        max_interior_pts = R.size()/2;
    }
    if (max_interior_pts > 0)
    {
        double dx0 = R[1]-R[0];
        if (R[0]/dx0 <= max_interior_pts)
        {
            int n = ceil(R[0]/dx0);
            for (size_t i = 0; i < n; i++)
            {
                R_int.push_back(0+i*(R[0])/(n));
            }
        }
        else
        {
            int n = max_interior_pts;
            double a = (R[0]/dx0 - n)*2/(n*(n+1));
            for (size_t i = 0; i < n; i++)
            {
                int k = n-i;
                R_int.push_back(R[0]-dx0*(k + a*k*(k+1)/2));
            }
            R_int[0] = 0.0;
        }
    }
    VD Phi_int(R_int.size(),0);
    VD dPhi_int(R_int.size(),0);
    Phi_int[0] = phi_absMin + delta_phi0;
    dPhi_int[0] = 0.0;
    double dV_ = dV_from_absMin(delta_phi0);
    double d2V_ = d2V({Phi_int[0]},0)[0][0];
    for (size_t i = 1; i < R_int.size(); i++)
    {
        std::tie(Phi_int[i],dPhi_int[i]) = exactSolution(R_int[i],Phi_int[0],dV_,d2V_);
    }
    VD R_final(R_int);
    VD Phi_final(Phi_int);
    VD dPhi_final(dPhi_int);

    R_final.insert(R_final.end(),R.begin(),R.end());
    Phi_final.insert(Phi_final.end(),Phi_ex.begin(),Phi_ex.end());
    dPhi_final.insert(dPhi_final.end(),dPhi_ex.begin(),dPhi_ex.end());

    return make_tuple(R_final,Phi_final,dPhi_final,Rerr);

}
double Tunneling1D::findAction(VD R, VD Phi, VD dPhi)
{
    int N = R.size();
    double Sphere_area = pow(M_PI,Spatial_Dim/2.0)/tgamma(Spatial_Dim/2.0);
    VD area = pow(R,alpha)*Sphere_area;
    VD integrand(N);
    double T = 0;
    for (size_t i = 0; i < N; i++)
    {
        integrand[i] = (pow(dPhi[i],2)/2 + V({Phi[i]},&T) - V({phi_metaMin},&T))*area[i];
    }
    double S = Simpson(R,integrand);

    // For the bulk inside the bubble interior
    double volume = pow(R[0],Spatial_Dim)*pow(M_PI,Spatial_Dim/2.0)/tgamma(Spatial_Dim/2.0+1.0);
    S += volume*(V({Phi[0]},&T)-V({phi_metaMin},&T));
    return S;
}
std::tuple<VD, VD> Tunneling1D::evenlySpacedPhi(VD phi, VD dphi, int npoint, int k, bool fixAbs)
{
    if (fixAbs)
    {
        phi.insert(phi.begin(),phi_absMin);
        phi.insert(phi.end(),phi_metaMin);
        dphi.insert(dphi.begin(),0.0);
        dphi.insert(dphi.end(),0.0);
    }
    else
    {
        phi.insert(phi.end(),phi_metaMin);
        dphi.insert(dphi.end(),0.0);
    }

    // Sort phi in increasing order
    VVD fullPhi = transpose({phi,dphi});
    // cout<<"fullPhi dim: ("<<fullPhi.size()<<","<<fullPhi[0].size()<<")"<<endl;
    sort(fullPhi.begin(),fullPhi.end(),[](VD x1, VD x2){return x1[0]<x2[0];});
    VVD::iterator iter = unique(fullPhi.begin(),fullPhi.end(),[](VD x1, VD x2){return x1[0]==x2[0];});
    fullPhi.resize(distance(fullPhi.begin(),iter));
    fullPhi=transpose(fullPhi);
    // cout<<fullPhi[0]<<endl;

    GSL_Spline_Inter inter;
    inter.SetData(&fullPhi[1],&fullPhi[0]);

    VD p;
    if (fixAbs)
    {
        p = linspace(phi_absMin,phi_metaMin,npoint);
    }
    else
    {
        p = linspace(phi[0],phi_metaMin,npoint);
    }

    return make_tuple(p,inter.valAt(p));
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_CoherentScatteringDistribution.cpp
//! \author Luke Kersting
//! \brief  The coherent photon scattering distribution definition.
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "MonteCarlo_CoherentScatteringDistribution.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_GaussKronrodIntegrator.hpp"
#include "Utility_ContractException.hpp"

namespace MonteCarlo{

// Constructor
CoherentScatteringDistribution::CoherentScatteringDistribution(
		    const Teuchos::RCP<const Utility::TabularOneDDistribution>&
		    form_factor_function_squared )
  : PhotonScatteringDistribution(),
    d_form_factor_function_squared( form_factor_function_squared )
{
  // Make sure the array is valid
  testPrecondition( !form_factor_function_squared.is_null() );
}

// Evaluate the distribution
/*! The cross section (b) differential in the scattering angle cosine is
 * returned from this function.
 */
double CoherentScatteringDistribution::evaluate(
			           const double incoming_energy,
			           const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  const double mult = Utility::PhysicalConstants::pi*
    Utility::PhysicalConstants::classical_electron_radius*
    Utility::PhysicalConstants::classical_electron_radius;

  const double form_factor_squared =
    this->evaluateFormFactorSquared( incoming_energy, scattering_angle_cosine);

  return mult*1e24*(1.0 + scattering_angle_cosine*scattering_angle_cosine)*
    form_factor_squared;
}

// Evaluate the PDF
double CoherentScatteringDistribution::evaluatePDF(
				   const double incoming_energy,
				   const double scattering_angle_cosine ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );
  // Make sure the scattering angle cosine is valid
  testPrecondition( scattering_angle_cosine >= -1.0 );
  testPrecondition( scattering_angle_cosine <= 1.0 );

  return this->evaluate( incoming_energy, scattering_angle_cosine )/
    this->evaluateIntegratedCrossSection( incoming_energy, 1e-3 );
}

// Evaluate the integrated cross section (b)
double CoherentScatteringDistribution::evaluateIntegratedCrossSection(
					         const double incoming_energy,
					         const double precision ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Evaluate the integrated cross section
  boost::function<double (double x)> diff_cs_wrapper =
    boost::bind<double>( &CoherentScatteringDistribution::evaluate,
			 boost::cref( *this ),
			 incoming_energy,
			 _1 );

  double abs_error, integrated_cs;

  Utility::GaussKronrodIntegrator quadrature_gkq_set( precision );

  quadrature_gkq_set.integrateAdaptively<15>( diff_cs_wrapper,
					     -1.0,
					     1.0,
					     integrated_cs,
					     abs_error );

  // Make sure the integrated cross section is valid
  testPostcondition( integrated_cs > 0.0 );

  return integrated_cs;
}

// Sample an outgoing energy and direction from the distribution
void CoherentScatteringDistribution::sample(
				     const double incoming_energy,
				     double& outgoing_energy,
				     double& scattering_angle_cosine ) const
{
  // The outgoing energy is always equal to the incoming energy
  outgoing_energy = incoming_energy;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( incoming_energy,
				   scattering_angle_cosine,
				   trial_dummy );
}

// Sample an outgoing energy and direction and record the number of trials
void CoherentScatteringDistribution::sampleAndRecordTrials(
					    const double incoming_energy,
					    double& outgoing_energy,
					    double& scattering_angle_cosine,
					    unsigned& trials ) const
{
  // The outgoing energy is always equal to the incoming energy
  outgoing_energy = incoming_energy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( incoming_energy,
				   scattering_angle_cosine,
				   trials );
}

// Randomly scatter the photon
void CoherentScatteringDistribution::scatterPhoton(
				     PhotonState& photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  double scattering_angle_cosine;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( photon.getEnergy(),
				   scattering_angle_cosine,
				   trial_dummy );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  // Set the new direction
  photon.rotateDirection( scattering_angle_cosine,
			  this->sampleAzimuthalAngle() );
}

// Randomly scatter the adjoint photon
void CoherentScatteringDistribution::scatterAdjointPhoton(
				     AdjointPhotonState& adjoint_photon,
				     ParticleBank& bank,
				     SubshellType& shell_of_interaction ) const
{
  double scattering_angle_cosine;

  unsigned trial_dummy;

  // Sample an outgoing direction
  this->sampleAndRecordTrialsImpl( adjoint_photon.getEnergy(),
				   scattering_angle_cosine,
				   trial_dummy );

  shell_of_interaction = UNKNOWN_SUBSHELL;

  // Set the new direction
  adjoint_photon.rotateDirection( scattering_angle_cosine,
				  this->sampleAzimuthalAngle() );
}

// Evaluate the form factor squared
double CoherentScatteringDistribution::evaluateFormFactorSquared(
				  const double incoming_energy,
				  const double scattering_angle_cosine ) const
{
  // The inverse wavelength of the photon (1/cm)
  const double inverse_wavelength = incoming_energy/
    (Utility::PhysicalConstants::planck_constant*
     Utility::PhysicalConstants::speed_of_light);

  // The squared form factor argument
  const double form_factor_arg_squared = ((1.0 - scattering_angle_cosine)/2.0)*
    inverse_wavelength*inverse_wavelength;

  // Make sure the squared form factor argument is valid
  testPostcondition( form_factor_arg_squared >= 0.0 );

  return d_form_factor_function_squared->evaluate( form_factor_arg_squared );
}

// Basic sampling implementation
void CoherentScatteringDistribution::sampleAndRecordTrialsBasicImpl(
					    const double incoming_energy,
					    double& scattering_angle_cosine,
					    unsigned& trials ) const
{
  // Make sure the incoming energy is valid
  testPrecondition( incoming_energy > 0.0 );

  // Increment the number of trials
  ++trials;

  // Use the probability mixing technique to sample an outgoing angle
  double random_number_1 =
    Utility::RandomNumberGenerator::getRandomNumber<double>();
  double random_number_2 =
    Utility::RandomNumberGenerator::getRandomNumber<double>();

  if( random_number_1 <= 0.75 )
    scattering_angle_cosine = 2*random_number_2 - 1.0;
  else
  {
    scattering_angle_cosine = pow( fabs(2*random_number_2 - 1.0), 1.0/3.0 );

    if( random_number_2 < 0.5 )
      scattering_angle_cosine *= -1.0;
  }

  // Check for roundoff error
  if( fabs( scattering_angle_cosine ) > 1.0 )
    scattering_angle_cosine = copysign( 1.0, scattering_angle_cosine );

  // Make sure the scattering angle cosine is valid
  testPostcondition( scattering_angle_cosine >= -1.0 );
  testPostcondition( scattering_angle_cosine <= 1.0 );
}

// Return the form factor function squared distribution
const Teuchos::RCP<const Utility::TabularOneDDistribution>&
CoherentScatteringDistribution::getFormFactorSquaredDistribution() const
{
  return d_form_factor_function_squared;
}

} // end MonteCarlo namespace

//---------------------------------------------------------------------------//
// end MonteCarlo_IncoherentScatteringDistribution.cpp
//---------------------------------------------------------------------------//


#include <boost/python/numpy.hpp>
#include <boost/python/module.hpp>
#include <boost/python/def.hpp>
#include <fstream>
#include <iomanip>
#include <string>
#include <numeric>
#include <algorithm>
#include <unistd.h>
#include <cmath>
#include <chrono>
#include <random>
#include <iostream>
#include <assert.h>
#include <vector>

namespace p = boost::python;
namespace np = boost::python::numpy;

int n_nodes;
int n_edges;
int **edges;
float *edge_weights;
float **X;

void gen_rand_network(int n, int m)
{
    int row;
    n_nodes = n;
    n_edges = m;
    edges = (int **)malloc(n_edges*sizeof(int *));
    for(int i = 0; i < n_edges; i++)
        edges[i] = (int *)malloc(2*sizeof(int));

    edge_weights = (float *)malloc(n_edges*sizeof(float));
    assert(edges != NULL);
    row = 0;
    while(row < n_edges)
    {
        edges[row][0] = rand()%n_nodes;
        edges[row][1] = rand()%n_nodes;
        edge_weights[row] = 1.0;
        row++;
    }
}

void init_embedding(int d)
{
    X = (float **)malloc(n_nodes*sizeof(float *));
    for(int i = 0; i < n_nodes; i++)
        X[i] = (float *)malloc(d*sizeof(float));
    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
    std::default_random_engine generator (seed);
    std::normal_distribution<float> distribution (0.0,1.0);
    for (int i = 0; i < n_nodes; i++)
        for(int j = 0; j < d; j++)
            X[i][j] = 0.01*distribution(generator);
}

void load_network(std::string f_name, bool is_weighted)
{
    std::ifstream f;
    int v_i, v_j, row;
    float w;

    f.open(f_name);
    f >> n_nodes;
    f >> n_edges;
    edges = (int **)malloc(n_edges*sizeof(int *));
    for(int i = 0; i < n_edges; i++)
        edges[i] = (int *)malloc(2*sizeof(int));

    edge_weights = (float *)malloc(n_edges*sizeof(float));
    assert(edges != NULL);

    row = 0;
    if(is_weighted)
    {
        while(f >> v_i  >> v_j >>  w)
        {
            edges[row][0] = v_i;
            edges[row][1] = v_j;
            edge_weights[row] = w;
            row++;
        }
    }
    else
    {
        while(f >> v_i  >> v_j)
        {
            edges[row][0] = v_i;
            edges[row][1] = v_j;
            edge_weights[row] = 1.0;
            row++;
        }
    }
    f.close();
}

void _print_f_value(int d)
{
    float f1 = 0.0, f2 = 0.0;
    float i_j_dot, w_ij;
    int v_i, v_j;
    for(int edge_id = 0; edge_id < n_edges; edge_id++)
    {
        v_i = edges[edge_id][0];
        v_j = edges[edge_id][1];
        w_ij = edge_weights[edge_id];
        i_j_dot = 0.0;
        for(int d_id = 0; d_id < d; d_id++)
            i_j_dot += X[v_i][d_id]*X[v_j][d_id];
        f1 += (w_ij - i_j_dot)*(w_ij - i_j_dot);
    }
    for(int node_id = 0; node_id < n_nodes; node_id++)
        for(int d_id = 0; d_id < d; d_id++)
            f2 += X[node_id][d_id]*X[node_id][d_id];
    std::cout << "\t\tObjective: " << f1+f2 << ", f1: " << f1 << ", f2:" << f2 << std::endl;
}

void saveEmbToTxt(std::string of_name, int d)
{
    std::ofstream f;
    f.open(of_name);
    f << n_nodes << " " << d << std::endl;
    for(int node_id = 0; node_id < n_nodes; node_id++)
    {
        f << node_id;
        for(int d_id = 0; d_id < d; d_id++)
            f << " " << X[node_id][d_id];
        f << std::endl;
    }
    f.close();

}
void learn_embedding(std::string if_name, std::string of_name, bool verbose, bool is_weighted, int d, float eta, float regu, int max_iter)
{
    clock_t t;
    srand(time(NULL));
    t = clock();
    load_network(if_name, is_weighted);
    // gen_rand_network(1e3, 3e4);
    init_embedding(d);
    int v_i, v_j;
    float i_j_dot, w_ij;
    for(int iter_id = 0; iter_id < max_iter; iter_id++)
    {
        if(verbose)
        {
            if(iter_id%100 == 0)
            {
                std::cout << "\tIter id: " << iter_id << std::endl;
                _print_f_value(d);
            }
        }
        for(int edge_id = 0; edge_id < n_edges; edge_id++)
        {
            v_i = edges[edge_id][0];
            v_j = edges[edge_id][1];
            w_ij = edge_weights[edge_id];
            if(v_j <= v_i)
                continue;
            i_j_dot = 0;
            for(int d_id = 0; d_id < d; d_id++)
                i_j_dot += X[v_i][d_id]*X[v_j][d_id];
            for(int d_id = 0; d_id < d; d_id++)
                X[v_i][d_id] -= eta*(regu*X[v_i][d_id] - (w_ij - i_j_dot)*X[v_j][d_id]);
        }
    }
    t = clock() - t;
    t = clock();
    saveEmbToTxt(of_name, d);
}

BOOST_PYTHON_MODULE(graphFac_ext)
{
    // Py_Initialize();
    // Py_Initialize();
    // np::initialize();
    // srand(time(NULL));
    p::def("learn_embedding", learn_embedding);
}