/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    DoglegOptimizer.h
 * @brief   Unit tests for DoglegOptimizer
 * @author  Richard Roberts
 */

#include <tests/smallExample.h>
#include <gtsam/nonlinear/DoglegOptimizerImpl.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>

#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#endif
#include <boost/bind.hpp>
#ifdef __GNUC__
#pragma GCC diagnostic pop
#endif
#include <boost/assign/list_of.hpp> // for 'list_of()'
#include <functional>
#include <boost/iterator/counting_iterator.hpp>

using namespace std;
using namespace gtsam;

// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
double computeError(const GaussianBayesNet& gbn, const LieVector& values) {

  // Convert Vector to VectorValues
  VectorValues vv = *allocateVectorValues(gbn);
  internal::writeVectorValuesSlices(values, vv,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));

  // Convert to factor graph
  GaussianFactorGraph gfg(gbn);
  return gfg.error(vv);
}

/* ************************************************************************* */
double computeErrorBt(const BayesTree<GaussianConditional>& gbt, const LieVector& values) {

  // Convert Vector to VectorValues
  VectorValues vv = *allocateVectorValues(gbt);
  internal::writeVectorValuesSlices(values, vv,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(vv.size()));

  // Convert to factor graph
  GaussianFactorGraph gfg(gbt);
  return gfg.error(vv);
}

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {

  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));

  // Compute the Hessian numerically
  Matrix hessian = numericalHessian(
      boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
      LieVector(VectorValues::Zero(*allocateVectorValues(gbn)).asVector()));

  // Compute the gradient numerically
  VectorValues gradientValues = *allocateVectorValues(gbn);
  Vector gradient = numericalGradient(
      boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)),
      LieVector(VectorValues::Zero(gradientValues).asVector()));
  internal::writeVectorValuesSlices(gradient, gradientValues,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));

  // Compute the gradient using dense matrices
  Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
  LONGS_EQUAL(11, augmentedHessian.cols());
  VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
  EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));

  // Compute the steepest descent point
  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
  VectorValues expected = gradientValues;  scal(step, expected);

  // Compute the steepest descent point with the dogleg function
  VectorValues actual = optimizeGradientSearch(gbn);

  // Check that points agree
  EXPECT(assert_equal(expected, actual, 1e-5));

  // Check that point causes a decrease in error
  double origError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
  double newError = GaussianFactorGraph(gbn).error(actual);
  EXPECT(newError < origError);
}

/* ************************************************************************* */
TEST(DoglegOptimizer, BT_BN_equivalency) {

  // Create an arbitrary Bayes Tree
  BayesTree<GaussianConditional> bt;
  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
      GaussianConditional::shared_ptr(new GaussianConditional(
          boost::assign::pair_list_of
          (2, Matrix_(6,2,
              31.0,32.0,
              0.0,34.0,
              0.0,0.0,
              0.0,0.0,
              0.0,0.0,
              0.0,0.0))
          (3, Matrix_(6,2,
              35.0,36.0,
              37.0,38.0,
              41.0,42.0,
              0.0,44.0,
              0.0,0.0,
              0.0,0.0))
          (4, Matrix_(6,2,
              0.0,0.0,
              0.0,0.0,
              45.0,46.0,
              47.0,48.0,
              51.0,52.0,
              0.0,54.0)),
          3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
      GaussianConditional::shared_ptr(new GaussianConditional(
          boost::assign::pair_list_of
          (0, Matrix_(4,2,
              3.0,4.0,
              0.0,6.0,
              0.0,0.0,
              0.0,0.0))
          (1, Matrix_(4,2,
              0.0,0.0,
              0.0,0.0,
              17.0,18.0,
              0.0,20.0))
          (2, Matrix_(4,2,
              0.0,0.0,
              0.0,0.0,
              21.0,22.0,
              23.0,24.0))
          (3, Matrix_(4,2,
              7.0,8.0,
              9.0,10.0,
              0.0,0.0,
              0.0,0.0))
          (4, Matrix_(4,2,
              11.0,12.0,
              13.0,14.0,
              25.0,26.0,
              27.0,28.0)),
          2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4))))));

  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));

  GaussianFactorGraph expected(gbn);
  GaussianFactorGraph actual(bt);

  EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian()));
}

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {

  // Create an arbitrary Bayes Tree
  BayesTree<GaussianConditional> bt;
  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
      GaussianConditional::shared_ptr(new GaussianConditional(
          boost::assign::pair_list_of
          (2, Matrix_(6,2,
              31.0,32.0,
              0.0,34.0,
              0.0,0.0,
              0.0,0.0,
              0.0,0.0,
              0.0,0.0))
          (3, Matrix_(6,2,
              35.0,36.0,
              37.0,38.0,
              41.0,42.0,
              0.0,44.0,
              0.0,0.0,
              0.0,0.0))
          (4, Matrix_(6,2,
              0.0,0.0,
              0.0,0.0,
              45.0,46.0,
              47.0,48.0,
              51.0,52.0,
              0.0,54.0)),
          3, Vector_(6, 29.0,30.0,39.0,40.0,49.0,50.0), ones(6))))));
  bt.insert(BayesTree<GaussianConditional>::sharedClique(new BayesTree<GaussianConditional>::Clique(
      GaussianConditional::shared_ptr(new GaussianConditional(
          boost::assign::pair_list_of
          (0, Matrix_(4,2,
              3.0,4.0,
              0.0,6.0,
              0.0,0.0,
              0.0,0.0))
          (1, Matrix_(4,2,
              0.0,0.0,
              0.0,0.0,
              17.0,18.0,
              0.0,20.0))
          (2, Matrix_(4,2,
              0.0,0.0,
              0.0,0.0,
              21.0,22.0,
              23.0,24.0))
          (3, Matrix_(4,2,
              7.0,8.0,
              9.0,10.0,
              0.0,0.0,
              0.0,0.0))
          (4, Matrix_(4,2,
              11.0,12.0,
              13.0,14.0,
              25.0,26.0,
              27.0,28.0)),
          2, Vector_(4, 1.0,2.0,15.0,16.0), ones(4))))));

  // Compute the Hessian numerically
  Matrix hessian = numericalHessian(
      boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
      LieVector(VectorValues::Zero(*allocateVectorValues(bt)).asVector()));

  // Compute the gradient numerically
  VectorValues gradientValues = *allocateVectorValues(bt);
  Vector gradient = numericalGradient(
      boost::function<double(const LieVector&)>(boost::bind(&computeErrorBt, bt, _1)),
      LieVector(VectorValues::Zero(gradientValues).asVector()));
  internal::writeVectorValuesSlices(gradient, gradientValues,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));

  // Compute the gradient using dense matrices
  Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
  LONGS_EQUAL(11, augmentedHessian.cols());
  VectorValues denseMatrixGradient = *allocateVectorValues(bt);
  internal::writeVectorValuesSlices(-augmentedHessian.col(10).segment(0,10), denseMatrixGradient,
    boost::make_counting_iterator(size_t(0)), boost::make_counting_iterator(gradientValues.size()));
  EXPECT(assert_equal(gradientValues, denseMatrixGradient, 1e-5));

  // Compute the steepest descent point
  double step = -gradient.squaredNorm() / (gradient.transpose() * hessian * gradient)(0);
  VectorValues expected = gradientValues;  scal(step, expected);

  // Known steepest descent point from Bayes' net version
  VectorValues expectedFromBN(5,2);
  expectedFromBN[0] = Vector_(2, 0.000129034, 0.000688183);
  expectedFromBN[1] = Vector_(2, 0.0109679, 0.0253767);
  expectedFromBN[2] = Vector_(2, 0.0680441, 0.114496);
  expectedFromBN[3] = Vector_(2, 0.16125, 0.241294);
  expectedFromBN[4] = Vector_(2, 0.300134, 0.423233);

  // Compute the steepest descent point with the dogleg function
  VectorValues actual = optimizeGradientSearch(bt);

  // Check that points agree
  EXPECT(assert_equal(expected, actual, 1e-5));
  EXPECT(assert_equal(expectedFromBN, actual, 1e-5));

  // Check that point causes a decrease in error
  double origError = GaussianFactorGraph(bt).error(VectorValues::Zero(*allocateVectorValues(bt)));
  double newError = GaussianFactorGraph(bt).error(actual);
  EXPECT(newError < origError);
}

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeBlend) {
  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));

  // Compute steepest descent point
  VectorValues xu = optimizeGradientSearch(gbn);

  // Compute Newton's method point
  VectorValues xn = optimize(gbn);

  // The Newton's method point should be more "adventurous", i.e. larger, than the steepest descent point
  EXPECT(xu.asVector().norm() < xn.asVector().norm());

  // Compute blend
  double Delta = 1.5;
  VectorValues xb = DoglegOptimizerImpl::ComputeBlend(Delta, xu, xn);
  DOUBLES_EQUAL(Delta, xb.asVector().norm(), 1e-10);
}

/* ************************************************************************* */
TEST(DoglegOptimizer, ComputeDoglegPoint) {
  // Create an arbitrary Bayes Net
  GaussianBayesNet gbn;
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      0, Vector_(2, 1.0,2.0), Matrix_(2,2, 3.0,4.0,0.0,6.0),
      3, Matrix_(2,2, 7.0,8.0,9.0,10.0),
      4, Matrix_(2,2, 11.0,12.0,13.0,14.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      1, Vector_(2, 15.0,16.0), Matrix_(2,2, 17.0,18.0,0.0,20.0),
      2, Matrix_(2,2, 21.0,22.0,23.0,24.0),
      4, Matrix_(2,2, 25.0,26.0,27.0,28.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      2, Vector_(2, 29.0,30.0), Matrix_(2,2, 31.0,32.0,0.0,34.0),
      3, Matrix_(2,2, 35.0,36.0,37.0,38.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      3, Vector_(2, 39.0,40.0), Matrix_(2,2, 41.0,42.0,0.0,44.0),
      4, Matrix_(2,2, 45.0,46.0,47.0,48.0), ones(2)));
  gbn += GaussianConditional::shared_ptr(new GaussianConditional(
      4, Vector_(2, 49.0,50.0), Matrix_(2,2, 51.0,52.0,0.0,54.0), ones(2)));

  // Compute dogleg point for different deltas

  double Delta1 = 0.5;  // Less than steepest descent
  VectorValues actual1 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta1, optimizeGradientSearch(gbn), optimize(gbn));
  DOUBLES_EQUAL(Delta1, actual1.asVector().norm(), 1e-5);

  double Delta2 = 1.5;  // Between steepest descent and Newton's method
  VectorValues expected2 = DoglegOptimizerImpl::ComputeBlend(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
  VectorValues actual2 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta2, optimizeGradientSearch(gbn), optimize(gbn));
  DOUBLES_EQUAL(Delta2, actual2.asVector().norm(), 1e-5);
  EXPECT(assert_equal(expected2, actual2));

  double Delta3 = 5.0;  // Larger than Newton's method point
  VectorValues expected3 = optimize(gbn);
  VectorValues actual3 = DoglegOptimizerImpl::ComputeDoglegPoint(Delta3, optimizeGradientSearch(gbn), optimize(gbn));
  EXPECT(assert_equal(expected3, actual3));
}

/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
  // really non-linear factor graph
  boost::shared_ptr<example::Graph> fg(new example::Graph(
      example::createReallyNonlinearFactorGraph()));

  // config far from minimum
  Point2 x0(3,0);
  boost::shared_ptr<Values> config(new Values);
  config->insert(X(1), x0);

  // ordering
  boost::shared_ptr<Ordering> ord(new Ordering());
  ord->push_back(X(1));

  double Delta = 1.0;
  for(size_t it=0; it<10; ++it) {
    GaussianSequentialSolver solver(*fg->linearize(*config, *ord));
    GaussianBayesNet gbn = *solver.eliminate();
    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
    double nonlinearError = fg->error(*config);
    double linearError = GaussianFactorGraph(gbn).error(VectorValues::Zero(*allocateVectorValues(gbn)));
    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config));
    Delta = result.Delta;
    EXPECT(result.f_error < fg->error(*config)); // Check that error decreases
    Values newConfig(config->retract(result.dx_d, *ord));
    (*config) = newConfig;
    DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in
  }
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


#include "gfunc.h"
#include <algorithm>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/variant.hpp>

namespace bg = boost::geometry;

typedef bg::model::d2::point_xy<double> DPoint;
typedef bg::model::segment<DPoint> DSegment;

std::vector<double>
intersection_seg_arc(double xc, double yc, double rr, double x0, double y0, double x1, double y1) {
    double t1, t2;
    std::vector<double> res;
    double h = xc;
    double k = yc;
    double r = rr;
    double a = (x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0);
    double b = 2 * (x1 - x0) * ((x0 - h)) + 2 * (y1 - y0) * (y0 - k);
    double c = (x0 - h) * (x0 - h) + (y0 - k) * (y0 - k) - r * r;
    if (b * b - 4 * a * c < 0) {
        return res;
    } else {
        t1 = (-b + sqrt(b * b - 4 * a * c)) / (2 * a);
        t2 = (-b - sqrt(b * b - 4 * a * c)) / (2 * a);
    }
    if (t1 >= 0 && t1 <= 1) {

        res.push_back((x1 - x0) * t1 + x0);
        res.push_back((y1 - y0) * t1 + y0);
        // res[0] = (x1 - x0) * t1 + x0;
        // res[1] = (y1 - y0) * t1 + y0;
    } else if (t2 >= 0 && t2 <= 1) {
        res.push_back((x1 - x0) * t2 + x0);
        res.push_back((y1 - y0) * t2 + y0);
        // res[0] = ((x1 - x0) * t2 + x0);
        // res[1] = ((y1 - y0) * t2 + y0);
    } else {
        // 无交点
        return res;
    }
    return res;
}

//std::vector<double> intersection_seg_seg(double *&st0, double *&ed0, double *&st1, double *&ed1) {
//    std::vector<double> res;
//    DPoint pt00(st0[0],st0[1]);
//    DPoint pt01(ed0[0],ed0[1]);
//    DPoint pt10(st1[0],st1[1]);
//    DPoint pt11(ed1[0],ed1[1]);
//    DSegment sg0(pt00,pt01);
//    DSegment sg1(pt10,pt11);
//
//    std::list<DPoint> lstPoints;
//
//    if (bg::intersects(sg0, sg1)){
//        bg::intersection(sg0, sg1, lstPoints);
//        res.push_back(lstPoints.begin()->x());
//        res.push_back(lstPoints.begin()->y());
//    }
//    return res;
//}

std::vector<double> intersection_seg_seg(py::list st0, py::list ed0, py::list st1, py::list ed1) {
    std::vector<double> res;
    DPoint pt00(st0[0].cast<double>(), st0[1].cast<double>());
    DPoint pt01(ed0[0].cast<double>(), ed0[1].cast<double>());
    DPoint pt10(st1[0].cast<double>(), st1[1].cast<double>());
    DPoint pt11(ed1[0].cast<double>(), ed1[1].cast<double>());
    DSegment sg0(pt00, pt01);
    DSegment sg1(pt10, pt11);

    std::list<DPoint> lstPoints;

    if (bg::intersects(sg0, sg1)) {
        bg::intersection(sg0, sg1, lstPoints);
        res.push_back(lstPoints.begin()->x());
        res.push_back(lstPoints.begin()->y());
    }
    return res;
}


#ifndef MLT_MODELS_TRANSFORMERS_PRINCIPAL_COMPONENTS_ANALYSIS_IMPL_HPP
#define MLT_MODELS_TRANSFORMERS_PRINCIPAL_COMPONENTS_ANALYSIS_IMPL_HPP

#include <Eigen/Core>
#include <Eigen/SVD>

#include "transformer.hpp"

namespace mlt {
namespace models {
namespace transformers {
	template<class ConcreteType>
	class PrincipalComponentsAnalysisImpl : public Transformer<ConcreteType> {
	public:
		inline auto components_size() const { assert(_fitted); return _components_size; }

		inline const auto components() const { assert(_fitted); return _components; }

		inline const auto explained_variance_ratio() const { assert(_fitted); return _explained_variance_ratio; }

		inline const auto noise_variance() const { assert(_fitted); return _noise_variance; }

		Self& fit(Features input, bool = true) {
			assert(_components_size == -1 || _components_size <= input.cols());

			_mean = input.rowwise().mean();
			MatrixXd final = input.colwise() - _mean;

			auto svd = ((final * final.transpose()) / input.cols()).jacobiSvd(ComputeThinU);

			_explained_variance = svd.singularValues();
			_explained_variance_ratio = _explained_variance / _explained_variance.sum();

			if (_components_size < 1 && _variance_to_retain < 0) {
				_components_size = input.rows();
			} else if (_components_size < 1 && _variance_to_retain > 0) {
				double acum = 0;
				size_t i = 0;
				while (i < _explained_variance_ratio.rows() && acum < _variance_to_retain) {
					acum += _explained_variance_ratio(i);
					i++;
				}
				_components_size = i + 1;
			}

			if (_components_size > svd.matrixU().cols()) {
				_components_size = _components.cols();
			}

			if (_components_size < std::min(input.rows(), input.cols())) {
				_noise_variance = _explained_variance.tail(_explained_variance.size() - _components_size).mean();
			} else {
				_noise_variance = 0;
			}

			_components = svd.matrixU().leftCols(_components_size);
			_explained_variance = _explained_variance.head(_components_size);
			_explained_variance_ratio = _explained_variance_ratio.head(_components_size);

			_fitted = true;

			return _self();
		}

		Result transform(Features input) const {
			assert(_fitted);

			if (_whiten) {
				auto transformed = (_components.transpose() * (input.colwise() - _mean));
				return transformed.array().colwise() * _explained_variance.cwiseSqrt().cwiseInverse().array();
			}

			return _components.transpose() * (input.colwise() - _mean);
		}

		Features inverse_transform(Result input) const {
			assert(_fitted);

			if (_whiten) {
				return (_components * (input.array().colwise() *_explained_variance.cwiseSqrt().array()).matrix()).colwise() + _mean;
			}

			return (_components * input).colwise() + _mean;
		}

	protected:
		explicit PrincipalComponentsAnalysisImpl(int components_size, bool whiten = false) : _components_size(components_size), _whiten(whiten) {}

		explicit PrincipalComponentsAnalysisImpl(double variance_to_retain, bool whiten = false) : _variance_to_retain(variance_to_retain), _whiten(whiten) {
			assert(variance_to_retain > 0 && variance_to_retain <= 1);
		}

		explicit PrincipalComponentsAnalysisImpl(bool whiten = false) : _whiten(whiten) {}

		int _components_size = -1;
		double _variance_to_retain = -1;
		VectorXd _mean;
		MatrixXd _components;
		VectorXd _explained_variance;
		VectorXd _explained_variance_ratio;
		double _noise_variance = 0;
		bool _whiten;
	};
}
}
}
#endif


/*
Copyright 2020 Standard Cyborg

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include "standard_cyborg/algorithms/PrincipalAxes.hpp"
#include "standard_cyborg/algorithms/Centroid.hpp"

#include "standard_cyborg/sc3d/Geometry.hpp"
#include "standard_cyborg/math/Mat3x4.hpp"

#include "standard_cyborg/util/DataUtils.hpp"

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdocumentation"
#include <Eigen/Eigenvalues>
#pragma clang diagnostic pop

using standard_cyborg::sc3d::Geometry;
using standard_cyborg::math::Vec3;
using standard_cyborg::math::Mat3x4;

using standard_cyborg::sc3d::Face3;

namespace standard_cyborg {

namespace algorithms {

Mat3x4 computeNormalwisePrincipalAxes(const Geometry& geometry)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid{0.0f, 0.0f, 0.0f};
    float areaSum = 0.0f;

    int numFaces = geometry.faceCount();
    const std::vector<Face3>& faces = geometry.getFaces();
    const std::vector<Vec3>& positions = geometry.getPositions();

    for (int i = 0; i < numFaces; i++) {
        const Face3& face = faces[i];
        const Vec3& pA = positions[face[0]];
        const Vec3& pB = positions[face[1]];
        const Vec3& pC = positions[face[2]];

        Vec3 faceCentroid = (pA + pB + pC) * (1.0f / 3.0f);

        // Vectors along two of the edges
        Vec3 vBA = pB - pA;
        Vec3 vCA = pC - pA;

        // Face area vector (strictly speaking, twice the area, but since we use it as
        // a weight and also divide by twice the area, it's strictly equivalent.
        Vec3 dArea = Vec3::cross(vBA, vCA);

        // Integrate the differential surface element area vectors into a summed area
        // vectors. For a perfectly closed volume, this will sum to zero and won't work
        // very well, but if part of the model is open, it will provide stable orientation
        // where eigenvalues+eigenvectors don't.
        areaVector += dArea;

        // Add this face's contributions to accumulators
        float dAreaMagnitude = dArea.norm();
        centroid += faceCentroid * dAreaMagnitude;
        areaSum += dAreaMagnitude;

        Moment(0, 0) += dArea.x * dArea.x;
        Moment(1, 0) += dArea.x * dArea.y;
        Moment(2, 0) += dArea.x * dArea.z;

        //Moment(0, 1) += dArea.y * dArea.x;
        Moment(1, 1) += dArea.y * dArea.y;
        Moment(2, 1) += dArea.y * dArea.z;

        //Moment(0, 2) += dArea.z * dArea.x;
        //Moment(1, 2) += dArea.z * dArea.y;
        Moment(2, 2) += dArea.z * dArea.z;
    }

    centroid /= areaSum;

    // The eigen docs state these entries are not used at all. We could copy them, but
    // it has no effect.
    // Moment(0, 1) = Moment(1, 0);
    // Moment(0, 2) = Moment(2, 0);
    // Moment(1, 2) = Moment(2, 1);

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    Vec3 axis0 = toVec3(eigenvectors.col(0));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

Mat3x4 computePointwisePrincipalAxes(const Geometry& geometry)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid(computeCentroid(geometry));

    int numFaces = geometry.faceCount();
    const std::vector<Face3>& faces = geometry.getFaces();
    const std::vector<Vec3>& positions = geometry.getPositions();

    for (int i = 0; i < numFaces; i++) {
        const Face3 face = faces[i];
        const Vec3 pA = positions[face[0]];
        const Vec3 pB = positions[face[1]];
        const Vec3 pC = positions[face[2]];

        Vec3 offset = (pA + pB + pC) * (1.0f / 3.0f) - centroid;

        // Face area vector (strictly speaking, twice the area, but since we use it as
        // a weight and also divide by twice the area, it's strictly equivalent.
        Vec3 dArea = 0.5 * Vec3::cross(pB - pA, pC - pA);

        // Integrate the differential surface element area vectors into a summed area
        // vectors. For a perfectly closed volume, this will sum to zero and won't work
        // very well, but if part of the model is open, it will provide stable orientation
        // where eigenvalues+eigenvectors don't.
        areaVector += dArea;

        // Weight each face by its area
        float weight = dArea.norm();

        Moment(0, 0) += offset.x * offset.x * weight;
        Moment(1, 0) += offset.x * offset.y * weight;
        Moment(2, 0) += offset.x * offset.z * weight;

        //Moment(0, 1) += offset.y * offset.x * weight;
        Moment(1, 1) += offset.y * offset.y * weight;
        Moment(2, 1) += offset.y * offset.z * weight;

        //Moment(0, 2) += offset.z * offset.x * weight;
        //Moment(1, 2) += offset.z * offset.y * weight;
        Moment(2, 2) += offset.z * offset.z * weight;
    }

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    // Eigenvalues are sorted in *increasing* order, so the strongest axis is the last
    Vec3 axis0 = toVec3(eigenvectors.col(2));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

Mat3x4 computePointwisePrincipalAxes(const std::vector<math::Vec3>& positions)
{
    using namespace math;

    Eigen::Matrix3f Moment;
    Moment.setZero();

    Vec3 areaVector{0.0f, 0.0f, 0.0f};
    Vec3 centroid(computeCentroid(positions));

    for (int i = 0; i < positions.size(); i++) {
        const Vec3 pA = positions[i];

        Vec3 offset = pA - centroid;
        areaVector += offset;

        Moment(0, 0) += offset.x * offset.x;
        Moment(1, 0) += offset.x * offset.y;
        Moment(2, 0) += offset.x * offset.z;

        //Moment(0, 1) += offset.y * offset.x * weight;
        Moment(1, 1) += offset.y * offset.y;
        Moment(2, 1) += offset.y * offset.z;

        //Moment(0, 2) += offset.z * offset.x * weight;
        //Moment(1, 2) += offset.z * offset.y * weight;
        Moment(2, 2) += offset.z * offset.z;
    }

    // Recall that if a self-adjoint matrix is real, then it's also symmetric, and the
    // eigenvalues of a real, symmetric matrix are real and also orthogonal.
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(Moment);

    auto eigenvectors = eigensolver.eigenvectors();
    auto eigenvalues = eigensolver.eigenvalues();

    // Eigenvalues are sorted in *increasing* order, so the strongest axis is the last
    Vec3 axis0 = toVec3(eigenvectors.col(2));
    Vec3 axis1 = toVec3(eigenvectors.col(1));

    // Eigenvectors are unique up to a sign, so we use the integrated area vector to
    // select an orientation stable with respect to small peturtbations. The first two
    // eigenvectors correspond to the strongest eigenvalues, so we flip their parity.
    // The third eigenvalue encodes the least amount of information, so we overwrite it
    // to enforce the correct parity.
    if (Vec3::dot(areaVector, axis0) < 0.0f) axis0 = -axis0;
    if (Vec3::dot(areaVector, axis1) < 0.0f) axis1 = -axis1;
    Vec3 axis2{Vec3::cross(axis0, axis1)};

    // Return the transform which moves a model at the origin *into* alignment with this
    // model. To align this model upright, you must invert this transform.
    return Mat3x4{
        axis0.x, axis1.x, axis2.x, centroid.x,
        axis0.y, axis1.y, axis2.y, centroid.y,
        axis0.z, axis1.z, axis2.z, centroid.z
    };
}

}

} // namespace StandardCyborg {


#define CATCH_CONFIG_MAIN
#include "catch.hpp"

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

#include <unsupported/Eigen/KroneckerProduct>

#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/SymEigsSolver.h>

#include <iostream>
#include <cassert>
#include <random>
#include <algorithm>

#include "edlib/Basis/Basis1DZ2.hpp"
#include "edlib/Basis/ToOriginalBasis.hpp"
#include "edlib/Hamiltonians/TIXXZ.hpp"
#include "edlib/Op/NodeMV.hpp"

#include "edlib/EDP/LocalHamiltonian.hpp"
#include "edlib/EDP/ConstructSparseMat.hpp"

using namespace edlib;

Eigen::SparseMatrix<double> getSX()
{
	Eigen::SparseMatrix<double> res(2,2);
	res.insert(0,1) = 1.0;
	res.insert(1,0) = 1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<std::complex<double> > getSY()
{
	Eigen::SparseMatrix<std::complex<double> > res(2,2);
	constexpr std::complex<double> I(0., 1.);
	res.insert(0,1) = -I;
	res.insert(1,0) = I;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSZ()
{
	Eigen::SparseMatrix<double> res(2,2);
	res.insert(0,0) = 1.0;
	res.insert(1,1) = -1.0;
	res.makeCompressed();
	return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
twoQubitOp(int N, int pos1, int pos2,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v1,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v2)
{
	using namespace Eigen;
	const uint32_t dim = (1u << N);

	assert(pos1 < pos2);

	Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> res(1,1);
	res(0,0) = 1.0;

	for(int i = 0; i < pos1; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}

	res = Eigen::kroneckerProduct(v1, res).eval();
	for(int i = pos1+1; i < pos2; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}
	res = Eigen::kroneckerProduct(v2, res).eval();
	for(int i = pos2+1; i < N; i++)
	{
		res = Eigen::kroneckerProduct(MatrixXd::Identity(2,2), res).eval();
	}
	return res;
}

template<typename T>
Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
singleQubitOp(int N, int pos,
		const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& v)
{
	using namespace Eigen;
	const uint32_t dim = (1u << N);

	using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ;
	MatrixT res(1,1);
	res(0,0) = 1.0;

	for(int i = 0; i < pos; i++)
	{
		res = Eigen::kroneckerProduct(MatrixT::Identity(2,2), res).eval();
	}

	res = Eigen::kroneckerProduct(v, res).eval();
	for(int i = pos+1; i < N; i++)
	{
		res = Eigen::kroneckerProduct(MatrixT::Identity(2,2), res).eval();
	}
	return res;
}

Eigen::SparseMatrix<double> getSXXYY()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(1,2) = 2.0;
	res.insert(2,1) = 2.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSXX()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,3) = 1.0;
	res.insert(1,2) = 1.0;
	res.insert(2,1) = 1.0;
	res.insert(3,0) = 1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSYY()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,3) = -1.0;
	res.insert(1,2) = 1.0;
	res.insert(2,1) = 1.0;
	res.insert(3,0) = -1.0;
	res.makeCompressed();
	return res;
}

Eigen::SparseMatrix<double> getSZZ()
{
	Eigen::SparseMatrix<double> res(4,4);
	res.insert(0,0) = 1.0;
	res.insert(1,1) = -1.0;
	res.insert(2,2) = -1.0;
	res.insert(3,3) = 1.0;
	res.makeCompressed();
	return res;
}

template<int N>
class CompareXXZ
{
private:
	double delta_;
	Basis1DZ2<uint32_t> basis_;

	Eigen::MatrixXd hamFull_;
	double gsEnergy_;
	Eigen::VectorXd gsVec_;
public:

	constexpr static int k = (N/2)*((N/2)%2);
	constexpr static int parity = 1-2*((N/2)%2);

	CompareXXZ(double delta)
		: delta_{delta}, basis_{N, k, parity, true}
	{
		using namespace Eigen;
		static_assert(N % 2 == 0, "N must be even");

		edp::LocalHamiltonian<double> lh(N,2);
		for(int i = 0; i < N; i++)
		{
			lh.addTwoSiteTerm({i, (i+1) % N}, getSXXYY() + delta_*getSZZ());
		}
		hamFull_ = MatrixXd(edp::constructSparseMat<double>(1<<N, lh));
		SelfAdjointEigenSolver<MatrixXd> es;
		es.compute(hamFull_);

		gsEnergy_ = es.eigenvalues()[0];
		gsVec_ = es.eigenvectors().col(0);

	}

	void Test()
	{
		using namespace Eigen;
		TIXXZ<uint32_t> ham(basis_, 1.0, delta_);
		const int dim = basis_.getDim();

		NodeMV mv(dim, 0, dim, ham);

		Spectra::SymEigsSolver<double, Spectra::SMALLEST_ALGE, NodeMV> eigs(&mv, 2, 6);
		eigs.init();
		eigs.compute(10000, 1e-12, Spectra::SMALLEST_ALGE);
		if(eigs.info() != Spectra::SUCCESSFUL)
			REQUIRE(false);
		double gsEnergy1 = eigs.eigenvalues()[0];

		REQUIRE(std::abs(gsEnergy_ - gsEnergy1) < 1e-4);
		VectorXd subspaceGs = eigs.eigenvectors().col(0);
		VectorXd gsVec1;
		{
			auto v = toOriginalVector(basis_, subspaceGs.data());
			gsVec1 = Map<VectorXd>(v.data(), 1<<N);
		}

		double gsEnergy2 = double(gsVec1.transpose()*hamFull_*gsVec1)/double(gsVec1.transpose()*gsVec1);

		REQUIRE(std::abs(gsEnergy1 - gsEnergy2) < 1e-6);
		REQUIRE(std::abs(std::abs(gsVec_.transpose()*gsVec1) - 1.) < 1e-6);
	}
};

TEST_CASE("Compare GS of XXZ using LocalHamiltonian and TIBasis", "[XXZGS]")
{
	SECTION("TIBasis Z2 XXZ N=8") {
		CompareXXZ<8> test(1.0);
		test.Test();
	}
	SECTION("TIBasis Z2 XXZ N=10") {
		CompareXXZ<10> test(1.0);
		test.Test();
	}
	SECTION("TIBasis Z2 XXZ N=12") {
		CompareXXZ<12> test(1.0);
		test.Test();
	}
}


// gnuplot-c++ includes
#include <gnuplot-iostream.h>

// MLearn includes
#include <MLearn/Core>
#include <MLearn/StochasticProcess/GaussianProcess/GP.h>

// STL includes
#include <vector>
#include <array>
#include <string>

// Eigen includes
#include <Eigen/Core>
#include <Eigen/SVD>

// Boost includes
#include <boost/program_options.hpp>

#define N_STEPS 1000 // n_points along the time
#define N_SAMPLES 1 // n of curves to sample

int main(int argc, char** argv){
	std::srand((unsigned int) time(0));

	Gnuplot gp;

	using namespace MLearn;
	using namespace SP::GP;
	namespace po = boost::program_options;

	// Create command line options
	po::options_description
	desc("This is a demo showing different samples from a gaussian process.");

	desc.add_options()
		("help", "Show the help")
		("kernel", po::value<int>(), "Value in [0, 10]."
		"This value indicates which kernel will be used. Available kernels:\n"
		"LINEAR (0)\n"
		"POLYNOMIAL (1)\n"
		"RBF (2)\n"
		"LAPLACIAN (3)\n"
		"ABEL (4)\n"
		"CONSTANT (5)\n"
		"MIN (6)\n"
		"MATERN_32 (7)\n"
		"MATERN_52 (8)\n"
		"RATIONAL_QUADRATIC (9)\n"
		"PERIODIC (10)");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		return 1;
	}

	int K_id = 0;
	if (vm.count("kernel")){
		K_id = vm["kernel"].as<int>();
	}

	typedef double float_type;
	typedef MLMatrix<float_type> Matrix;
	typedef MLVector<float_type> Vector;

	// mean and query points
	Vector mean = Vector::Zero(N_STEPS);
	Matrix pts = Matrix::Zero(1, N_STEPS);
	pts.row(0) = Vector::LinSpaced(N_STEPS, -10, 10);

	Matrix samples(N_STEPS, N_SAMPLES);

	// Quite ugly, but classes were not designed for
	// dynamic polymorphism
	switch(K_id){
		case 0:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::LINEAR>());
			break;
		case 1:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::POLYNOMIAL, float_type>());
			break;
		case 2:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::RBF, float_type>());
			break;
		case 3:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::LAPLACIAN, float_type>());
			break;
		case 4:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::ABEL, float_type>());
			break;
		case 5:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::CONSTANT, float_type>());
			break;
		case 6:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::MIN>());
			break;
		case 7:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts,
					Kernel<KernelType::MATERN32, float_type>());
			break;
		case 8:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts,
					Kernel<KernelType::MATERN52, float_type>());
			break;
		case 9:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts,
					Kernel<KernelType::RATIONAL_QUADRATIC, float_type>());
			break;
		case 10:
			samples =
				GaussianProcess<float_type>::sample(
					mean, pts, Kernel<KernelType::PERIODIC, float_type>());
			break;
	}

	gp << "set xrange [-10:10]\nset yrange [-3:3]\n";
	std::vector<std::pair<float_type, float_type>> gnu_pts;
	for(uint i = 0; i < N_STEPS; ++i) {
		gnu_pts.push_back(std::make_pair(pts(0, i), samples(i, 0)));
	}
	gp << "plot '-' with lines\n";
	gp.send1d(gnu_pts);
	return 0;
}


#ifndef DENSEMATRIX_HPP
#define DENSEMATRIX_HPP

#include <vector>
#include <fstream>
#include <exception>

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/binary_object.hpp>
#include <boost/serialization/vector.hpp>

#include <Eigen/Core>

class dimensionMismatch: public std::exception
{
  virtual const char* what() const throw()
  {
    return "Dimension Mismatch. Operation not possible.";
  }
} dimensionMismatch;

template<class scalar>
class denseMatrix
{
public:

  denseMatrix();

  denseMatrix(int rows, int cols);

  denseMatrix(std::vector<scalar> &data, int rows, int cols);

  denseMatrix(std::vector<scalar> &data, int rows, int cols, bool trnsps);

  void resize(int rows, int cols);

  void setRandom(int seed);

  void save(std::string fname);
  void load(std::string fname);

  int rows() { return rows_; };
  int rows() const { return rows_; };
  int get_rows();

  int cols() { return cols_; };
  int cols() const { return cols_; }
  int get_cols();

  denseMatrix get_row(int i);
  denseMatrix get_col(int i);
  denseMatrix get_diagonal(int i);
  denseMatrix get_block(int i, int j, int k, int l);

  void set_row(int i, const denseMatrix& r);
  void set_col(int i, const denseMatrix& c);
  void set_diagonal(int i, const denseMatrix& d);
  void set_block(int i, int j, int k, int l, const denseMatrix& b);

  bool is_transpose() { return transpose_mat; }
  bool is_transpose() const { return transpose_mat; }

  std::vector<scalar>& container() { return data_; }
  std::vector<scalar>  container() const { return data_; }

  Eigen::Map< Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic> > getEigenMap();
  Eigen::Map< const Eigen::Matrix<scalar, Eigen::Dynamic, Eigen::Dynamic> > getEigenMap() const;

  scalar* data() { return data_.data(); }
  const scalar* data() const { return data_.data(); }

  scalar &operator[](int i) { return data_[i]; }
  scalar &operator[](int i) const { return data_[i]; }
  scalar &operator()(int row, int col) { return data_[row + col*rows_]; }

  denseMatrix& operator=(const  denseMatrix& other);
  denseMatrix& operator+=(const denseMatrix& other);
  denseMatrix& operator-=(const denseMatrix& other);
  denseMatrix& operator*=(const double a);
  denseMatrix  operator*(const double a);
  denseMatrix  operator+(const denseMatrix& other);
  denseMatrix  operator-(const denseMatrix& other);
  denseMatrix  operator*(const denseMatrix& other);

  denseMatrix transpose();

  /// extra member functions to fascilitation python wrapping
  void assign(const denseMatrix& other);
  void setElem(scalar elem, int row, int col);
  scalar getElem(int row, int col) { return data_[row + col*rows_]; }

  scalar norm();

  scalar trace();

  size_t size();

  void print();

private:

  friend class boost::serialization::access;

  template <typename Archive>
  void serialize(Archive &ar, const unsigned int version)
  {
    ar & cols_;
    ar & rows_;
    ar & data_;
    ar & transpose_mat;
  }

  /// size of matrix
  int cols_;
  int rows_;

  bool transpose_mat;
  void set_transpose() { transpose_mat = true; }

  /// container class to hold data for eigen map
  std::vector<scalar> data_;

};

#include "denseMatrix_impl.hpp"

#endif /*DENSEMATRIX_HPP*/


#include "ecpy_native.h"
#include <boost/format.hpp>
#include <stdexcept>
using namespace std;

static int ac_count = 0;
static int wa_count = 0;

#define ES_ASSERT_EQ_FM(x,y,m) ES_ASSERTION(x == y, m)
#define ES_ASSERT_NEQ_FM(x,y,m) ES_ASSERTION(x != y, m)
#define ES_ASSERT_EQ_M(x,y,m) ES_ASSERT_EQ_FM(x, y, m " == " #y)
#define ES_ASSERT_NEQ_M(x,y,m) ES_ASSERT_NEQ_FM(x, y, m " != " #y)
#define ES_ASSERT_EQ(x,y) ES_ASSERT_EQ_M(x, y, #x)
#define ES_ASSERT_NEQ(x,y) ES_ASSERT_NEQ_M(x, y, #x)

#define ES_ASSERTION(cond, msg) do {\
  cout << boost::format("[+] %-16s...%-8s") % msg % "";\
  try { \
    if(!(cond)) { \
      cout << "\033[31m[ FAILED ]\033[0m" << endl; \
      wa_count++; \
    } else { \
      cout << "\033[33m[   OK   ]\033[0m" << endl; \
      ac_count++;\
    }\
  } catch (const runtime_error& e) { \
      cout << "\033[31mFAILED(EXCEPTION)\033[0m" << endl; \
      cerr << "[-] \033[31mAssertion Failed: <" \
          << __FILE__ << "> " << __FUNCTION__ << ":" << __LINE__ \
          << "(Exception occurerd!) -> !(" << #cond << ")\033[0m" \
          << "\n\t-> \033[01;04;31m" << e.what() << "\033[0m" << endl;\
      wa_count++; \
  } \
} while(0)

#define TEST(name) void _ ## name ## _test(); void name ## _test() { \
  clock_t start, end; \
  double time;\
  cout << boost::format("Start Test: %s\n") % #name; \
  start = clock();\
  _ ## name ## _test(); \
  end = clock();\
  time = ((double)(end - start) / CLOCKS_PER_SEC); \
  cout << boost::format("Test Finished. Time: %s sec (%s usec)\n") % time % (time * 1e+6); \
} \
void _ ## name ## _test()

TEST(ec_ff) {
  auto F = FF(7);
  auto E = EC<FF>(F, 0, 1);
  auto P = EC_elem<FF_elem>(0, 1, 1);
  auto Q = EC_elem<FF_elem>(3, 0, 1);
  EC_elem<FF_elem> T, U, Z;

  ES_ASSERT_NEQ_FM(E.equ(P, Q), true, "P != Q");
  ES_ASSERT_EQ_FM(E.equ(P, P), true, "P == P");
  ES_ASSERT_EQ_FM(E.equ(Q, Q), true, "Q == Q");

  E.add(T, P, Q);
  E.add(U, Q, P);

  ES_ASSERT_EQ_FM(E.equ(T, U), true, "P+Q == Q+P");

  Z = {6, 3, 6};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+Q=(6:3:6)");

  E.add(T, P, P);
  Z = {0, 6, 1};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+P=(0:6:1)");

  E.sub(T, P, P);
  Z = {0, 1, 0};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P-P=(0:1:0)");

  E.mul(T, P, 3);
  Z = {0, 1, 0};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "3P=(0:1:0)");

  ES_ASSERT_EQ(E.is_on_curve(P), true);
  ES_ASSERT_EQ(E.is_on_curve(Q), true);
  ES_ASSERT_EQ(E.is_on_curve(T), true);

  auto r = E.line_coeff(P, Q);
  ES_ASSERT_EQ_FM(F.equ(r, FF_elem(2)), true, "line_coeff(P, Q)");
}

TEST(ec_miller) {
  auto F = FF(631);
  auto E = EC<FF>(F, 30, 34);
  auto m = 5;

  auto P = EC_elem<FF_elem>{36, 60, 1};
  auto Q = EC_elem<FF_elem>{121, 387, 1};
  auto S = EC_elem<FF_elem>{0, 36, 1};
  FF_elem t {0}, z {0};
  EC_elem<FF_elem> Z;

  E.add(Z, Q, S);
  miller(t, E, P, Z, m);
  z = {103};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "miller(P, Q+S) == 103");

  miller(t, E, P, S, m);
  z = {219};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "miller(P, S) == 219");

  E.sub(Z, P, S);
  miller(t, E, Q, Z, m);
  z = {284};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "miller(Q, P-S) == 284");

  E.sub(Z, EC_elem<FF_elem>{0, 1, 0}, S);
  miller(t, E, Q, Z, m);
  z = {204};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "miller(Q, -S) == 204");

  weil_pairing(t, E, P, Q, S, m);
  z = {242};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "weil_pairing(P, Q) == 242");

  tate_pairing(t, E, P, Q, m, 1);
  z = {279};
  ES_ASSERT_EQ_FM(F.equ(t, z), true, "tate_pairing(P, Q) == 279");
}

TEST(ec_ef_1) {
  auto F = EF(7, IrreduciblePolynomialType::X2_1);
  auto E = EC<EF>(F, 0, 1);
  auto P = EC_elem<EF_elem>(EF_elem(4, 2), EF_elem(2, 1), EF_elem(1, 0));
  auto Q = EC_elem<EF_elem>(EF_elem(0, 3), EF_elem(3, 6), EF_elem(1, 0));
  EC_elem<EF_elem> T, U, Z;

  ES_ASSERT_NEQ_FM(E.equ(P, Q), true, "P != Q");
  ES_ASSERT_EQ_FM(E.equ(P, P), true, "P == P");
  ES_ASSERT_EQ_FM(E.equ(Q, Q), true, "Q == Q");

  E.add(T, P, Q);
  E.add(U, Q, P);
  ES_ASSERT_EQ_FM(E.equ(T, U), true, "P+Q == Q+P");

  Z = {EF_elem(0, 2), EF_elem(4, 6), EF_elem(1)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+Q=(2i:4+6i:1)");

  E.add(T, P, P);
  Z = {EF_elem(4, 1), EF_elem(6, 5), EF_elem(2, 4)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+P=(4+i:6+5i:2+4i)");

  E.sub(T, P, P);
  Z = {0, 1, 0};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P-P=(0:1:0)");

  E.mul(T, P, 9);
  Z = {EF_elem(1, 1), EF_elem(0, 1), EF_elem(5, 3)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "9P=(1+i:i:5+3i)");

  auto r = E.line_coeff(P, Q);
  ES_ASSERT_EQ_FM(F.equ(r, EF_elem(5)), true, "line_coeff(P, Q)");

  ES_ASSERT_EQ(E.is_on_curve(P), true);
  ES_ASSERT_EQ(E.is_on_curve(Q), true);
  ES_ASSERT_EQ(E.is_on_curve(T), true);
}

TEST(ec_ef_2) {
  auto F = EF(41, IrreduciblePolynomialType::X2_X_1);
  auto E = EC<EF>(F, 0, 1);
  auto P = EC_elem<EF_elem>(EF_elem(39, 39), EF_elem(3, 0), EF_elem(1, 0));
  auto Q = EC_elem<EF_elem>(EF_elem(5, 5), EF_elem(9, 0), EF_elem(1, 0));
  EC_elem<EF_elem> T, U, Z;

  ES_ASSERT_NEQ_FM(E.equ(P, Q), true, "P != Q");
  ES_ASSERT_EQ_FM(E.equ(P, P), true, "P == P");
  ES_ASSERT_EQ_FM(E.equ(Q, Q), true, "Q == Q");

  E.add(T, P, Q);
  E.add(U, Q, P);
  ES_ASSERT_EQ_FM(E.equ(T, U), true, "P+Q == Q+P");

  Z = {EF_elem(10, 10), EF_elem(27), EF_elem(26)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+Q=(10+10w:27:26)");

  E.add(T, P, P);
  Z = {EF_elem(0), EF_elem(11), EF_elem(11)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P+P=(0:11:11)");

  E.sub(T, P, P);
  Z = {0, 1, 0};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "P-P=(0:1:0)");

  E.add(T, P, Q);
  E.mul(T, T, 27);
  Z = {EF_elem(24, 24), EF_elem(29, 0), EF_elem(2, 0)};
  ES_ASSERT_EQ_FM(E.equ(T, Z), true, "27(P+Q)=(24+24w:29:2)");

  auto r = E.line_coeff(P, Q);
  ES_ASSERT_EQ_FM(F.equ(r, EF_elem(0, 5)), true, "line_coeff(P, Q)");

  ES_ASSERT_EQ(E.is_on_curve(P), true);
  ES_ASSERT_EQ(E.is_on_curve(Q), true);
  ES_ASSERT_EQ(E.is_on_curve(T), true);
}

TEST(ef_1) {
  auto F = EF(7, IrreduciblePolynomialType::X2_1);
  auto x = EF_elem(3, 0);
  auto y = EF_elem(0, 5);
  EF_elem t;

  F.add(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 3 && t.v.v == 5), true, "x+y=3+5i");
  F.sub(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 3 && t.v.v == 2), true, "x-y=3+2i");
  F.mul(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 0 && t.v.v == 1), true, "x*y=i");
  F.div(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 0 && t.v.v == 5), true, "x/y=5i");
  F.pow(t, y, 31);
  ES_ASSERT_EQ_FM((t.u.v == 0 && t.v.v == 2), true, "y^31=2i");
}

TEST(ef_2) {
  auto F = EF(41, IrreduciblePolynomialType::X2_X_1);
  auto x = EF_elem(15, 25);
  auto y = EF_elem(39, 10);
  EF_elem t;

  F.add(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 13 && t.v.v == 35), true, "x+y=13+35w");
  F.sub(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 17 && t.v.v == 15), true, "x-y=17+15w");
  F.mul(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 7 && t.v.v == 14), true, "x*y=7+14w");
  F.div(t, x, y);
  ES_ASSERT_EQ_FM((t.u.v == 29 && t.v.v == 5), true, "x/y=29+5w");
  F.pow(t, x, 40);
  ES_ASSERT_EQ_FM((t.u.v == 14 && t.v.v == 17), true, "x^40=14+17w");
}

TEST(ff) {
  auto ff = FF(7);
  FF_elem x(3), y(6);
  FF_elem t;
  ES_ASSERT_EQ(x.v, 3);
  ES_ASSERT_EQ(y.v, 6);

  ff.add(t, x, y);
  ES_ASSERT_EQ_FM(t.v, 2, "x+y");

  ff.sub(t, x, y);
  ES_ASSERT_EQ_FM(t.v, 4, "x-y");

  ff.mul(t, x, y);
  ES_ASSERT_EQ_FM(t.v, 4, "x*y");

  ff.div(t, x, y);
  ES_ASSERT_EQ_FM(t.v, 4, "x/y");

  ff.pow(t, x, 30);
  ES_ASSERT_EQ_FM(t.v, 1, "x^30");
}

void exec_test() {
  ff_test();
  ef_1_test();
  ef_2_test();
  ec_ff_test();
  ec_ef_1_test();
  ec_ef_2_test();
  ec_miller_test();
  cout << boost::format("[+] %d Test(s) finished. %d Test(s) success, %d Test(s) fail.")
    % (ac_count + wa_count)
    % ac_count
    % wa_count
    << endl;
}

int main(int ac, char **av) {
  exec_test();
  return wa_count;
}


#include "lorenz_aliases.h"
#include "lorenz.h"
#include "lorenz_equations.h"
#include "push_back_local_maxima.h"

#include <fstream>
#include <vector>

#include <boost/array.hpp>
#include <boost/numeric/odeint.hpp>

void outputLorenzAttractorData(const std::string& output_directory)
{
	constexpr double sigma = 10.0;
	constexpr double r = 28.0;
	constexpr double b = 8.0 / 3.0;

	std::ofstream file_stream;

	file_stream.open(output_directory);

	state_type x = { 10.0 , 1.0 , 1.0 }; // initial conditions

	auto lambda_writer = [&file_stream](const state_type& x, const double t)
	{
		file_stream << t << ',' << x[0] << ',' << x[1] << ',' << x[2] << std::endl;
	};

	boost::numeric::odeint::integrate(lorenz(sigma, r, b), x, 0.0, 100.0, 0.01, lambda_writer);

	file_stream.close();
}

void outputLorenzBifurcationDiagramData(const std::string& output_file)
{

	// range for parameter change
	constexpr double r_min = 0.0;
	constexpr double r_max = 400.0;

	constexpr int r_num_divisions = 400;
	constexpr double r_division = (r_max - r_min) / (double)r_num_divisions;

	std::ofstream file_stream;

	file_stream.open(output_file);

	double r{ r_min };

	for (int i{ 0 }; i < r_num_divisions; i++)
	{
		r += r_division;
		constexpr double sigma{ 10.0 };
		constexpr double b{ 8.0 / 3.0 };

		state_type state = { 15.0, 20.0, 30.0 };

		constexpr double transient_time = 100.0;

		boost::numeric::odeint::integrate(lorenz(sigma, r, b), state, 0.0, transient_time, 0.1);

		std::vector<state_type> states;
		std::vector<double> times;

		boost::numeric::odeint::integrate(lorenz(sigma, r, b), state, 0.0, 100.0, 0.1, push_back_local_maxima(states, times));

		for (size_t i{ 0 }; i < states.size(); i++)
			file_stream << times[i] << ',' << r << ',' << states[i][0] << ',' << states[i][1] << ',' << states[i][2] << std::endl;
	}

	file_stream.close();

	return;
}


#ifndef _EIGEN_RIDGE_HPP_

#define _EIGEN_RIDGE_HPP_

/*
 * L2-regularized (ridge) linear regression without intercept in c++11 as Eigen3 template function
 * Uses singular value decomposition, works with both tall and wide design matrices
 * Written by Carlos Guerreiro carlos@perceptiveconstructs.com
 * This is free and unencumbered software released into the public domain.
 */

#include <Eigen/Dense>

template<typename M, typename V, typename P>
M ridge(const M& A, const V& y,  P alpha) {
  const auto& svd = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
  const auto& s = svd.singularValues();
  const auto r = s.rows();
  const auto& D = s.cwiseQuotient((s.array().square() + alpha).matrix()).asDiagonal();
  return svd.matrixV().leftCols(r) * D * svd.matrixU().transpose().topRows(r) * y;
}

#endif


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

int main()
{
  Matrix3d m = Matrix3d::Random();
  m = (m + Matrix3d::Constant(1.2)) * 50;
  cout << "m =" << endl << m << endl;
  Vector3d v(1,2,3);

  cout << "m * v =" << endl << m * v << endl;
}


// The template and inlines for the -*- C++ -*- 3d vector classes.
// Initially implemented by Wai-Shing Luk <luk036@gmail.com>
//

/** @file include/vector3d.hpp
 *  This is a C++ Library header.
 */

#ifndef FUN_VECTOR3D_HPP
#define FUN_VECTOR3D_HPP 1

#include <boost/operators.hpp>   // for boost::addable etc

namespace fun
{

/**
 *  3-dimensional vector.
 *
 *  @param  Tp  Type of vector elements
 */
template <typename _K>
class vector3d :
  boost::equality_comparable < vector3d<_K>,
  boost::addable < vector3d<_K>,
  boost::subtractable < vector3d<_K>,
  boost::multipliable2 < vector3d<_K>, _K,
  boost::dividable2 < vector3d<_K>, _K
  > > > > >
{
  /// Value typedef.
  typedef _K value_type;

public:
  /// static memeber zero
  static const vector3d<_K> zero;

  /// Default constructor.
  constexpr vector3d<_K>(
    const _K& e1,
    const _K& e2,
    const _K& e3) noexcept
    : _e1{e1}, _e2{e2}, _e3{e3} { }

  /// Return first element of vector.
  constexpr _K e1() const noexcept { return _e1; }
  /// Return second element of vector.
  constexpr _K e2() const noexcept { return _e2; }
  /// Return third element of vector.
  constexpr _K e3() const noexcept { return _e3; }

  // Lets the compiler synthesize the assignment operator
  // vector3d<_K>& operator= (const vector3d<_K>&);
  /// Assign this vector to vector @a w.
  /// Add @a w to this vector.
  vector3d<_K>& operator+=(const vector3d<_K>& w)
  { _e1 += w.e1(); _e2 += w.e2(); _e3 += w.e3(); return *this; }

  /// Subtract @a w from this vector.
  vector3d<_K>& operator-=(const vector3d<_K>& w)
  { _e1 -= w.e1(); _e2 -= w.e2(); _e3 -= w.e3(); return *this; }

  /// Multiply this vector by @a a.
  vector3d<_K>& operator*=(const _K& a)
  { _e1 *= a; _e2 *= a; _e3 *= a; return *this; }

  /// Divide this vector by @a a.
  vector3d<_K>& operator/=(const _K& a)
  { _e1 /= a; _e2 /= a; _e3 /= a; return *this; }

  constexpr bool operator== (const vector3d<_K>& w) const
  { return e1() == w.e1() && e2() == w.e2() && e3() == w.e3(); }

private:
  _K _e1;
  _K _e2;
  _K _e3;
};

template <typename _K>
const vector3d<_K> vector3d<_K>::zero (_K(0), _K(0), _K(0));

/// Return @a v.
template<typename _K>
inline constexpr vector3d<_K>
operator+(const vector3d<_K>& v) noexcept
{ return v; }

/// Return negation of @a v/
template<typename _K>
inline constexpr vector3d<_K>
operator-(const vector3d<_K>& v) noexcept
{ return vector3d<_K>(-v.e1(), -v.e2(), -v.e3()); }

///  Return dot product of  @a v and @a w
template<typename _K>
inline constexpr _K
dot(const vector3d<_K>& v, const vector3d<_K>& w) noexcept
{ return v.e1()*w.e1() + v.e2()*w.e2() + v.e3()* w.e3(); }

///  Return new vector @a v x @a w (cross product).
template<typename _K>
inline constexpr vector3d<_K>
cross(const vector3d<_K>& v, const vector3d<_K>& w) noexcept
{
  return vector3d<_K>(
     v.e2()*w.e3() - v.e3()*w.e2(),
    -v.e1()*w.e3() + v.e3()*w.e1(),
     v.e1()*w.e2() - v.e2()*w.e1()  );
}

///  Return determinant of @a p,  @a q and @a r.
template<typename _K>
inline constexpr _K
det(const vector3d<_K>& p, const vector3d<_K>& q,
    const vector3d<_K>& r) noexcept
{
  return dot(p, cross(q, r)); // Pl\{"}ucker's formula
}

///  Insertion operator for vector values.
template<typename _K, class _Stream>
_Stream& operator<<(_Stream& os, const vector3d<_K>& v)
{
  os << '[' << v.e1() << ',' << v.e2() << ',' << v.e3() << ']';
  return os;
}

///  Return the quadrance of  @a v
template<typename _K>
inline constexpr _K
quadrance(const vector3d<_K>& v) noexcept
{ return dot(v,v); }

} // namespace fun

#endif


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef UTIL_MEDIAN
#define UTIL_MEDIAN

#include <boost/range/algorithm.hpp>
#include <vector>       // std::vector

namespace sferes {
  namespace util {
  	class Median
		{
		public:
  		/**
  		 * Calculate the median of the doubles in the given list.
  		 */
  		static double calculate_median(std::vector<double>& list)
  		{
  			size_t size = list.size();	// Size of the list

  			assert(size > 0);

  			std::sort(list.begin(), list.end());

  			// If there are an odd number of doubles
  			if (size % 2 == 1)
  			{
  				// Take the middle number
  				size_t index_middle = (size - 1)/2;
  				return list[index_middle];
  			}
  			// If there are an even number of doubles
  			else
  			{
  				size_t index_above = size / 2;
  				size_t index_below = index_above - 1;

  				// Average of two middle numbers
  				return (list[index_above] + list[index_below]) / 2;
  			}
  		}
		};
  }
}
#endif


/**
 * @file semimprk_main.cc
 * @brief NPDE homework SemImpRK code
 * @author Unknown, Oliver Rietmann
 * @date 04.04.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>

#include "semimprk.h"

int main() {
  auto f = [](Eigen::Vector3d y) -> Eigen::Vector3d {
    return Eigen::Vector3d(y(0) * y(1), y(1) * y(2), y(2) - y(0));
  };
  auto df = [](Eigen::Vector3d y) {
    Eigen::Matrix3d J;
    J << y(1), y(0), 0.0, 0.0, y(2), y(1), -1.0, 0.0, 1.0;
    return J;
  };
  Eigen::Vector3d y0(1.0, 2.0, 3.0);
  unsigned int M = 10;
  double T = 2.0;

  std::cout << "Test of SolveRosenbrock():"
            << SemImpRK::SolveRosenbrock(f, df, y0, M, T).back().transpose()
            << std::endl;

  double cvgRate = SemImpRK::CvgRosenbrock();
  std::cout << "Convergence rate: " << cvgRate << std::endl;

  return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <math.h>
#include "scf.h"
#include <Eigen/Eigenvalues>
#include <Eigen/Core>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;
using std::sqrt;
int INDEX(int i,int j) {
    if (i>j) return i*(i+1)/2 + j;
    return j*(j+1)/2 + i;
}
#define MAX_ITER 75
#define EDIFF_CONV 0.000000000002
#define RMS_CONV 0.000000000014
double rmsDensity(MatrixXd A, MatrixXd B, int mat_size){
    double result = 0;
    for (int i =0 ; i < mat_size; i++){
        for(int j =0; j < mat_size; j++){
            result += pow(A(i,j) - B(i,j),2);
        }
    }
    return pow(result,0.5);
}

void run_scf() {
//creating variables/objects
// Reading input
    string SCF_options[1] = {"num_elec"};
    ifstream input ("INPUT");
    string newline,temp_str;
    int occ = 0;
    while(getline(input,newline)){
        istringstream ss(newline);
        ss >> temp_str;
        if (temp_str == "num_elec") {
            ss >> occ;
            occ /= 2;
        }
    }

    ifstream numffile ("../mol/num_nao.dat");
    int num_basisf ;
    numffile >> num_basisf;
    int mat_length = num_basisf * (num_basisf + 1) / 2 ;
    int total_mat_length = num_basisf * num_basisf;
    double enuc, rms, Ediff;
    double e0, eX, eC, etotal =0;
    MatrixXd t_array(num_basisf,num_basisf);
    MatrixXd v_array(num_basisf,num_basisf);
    MatrixXd s_array(num_basisf,num_basisf);
    MatrixXd hcore(num_basisf,num_basisf);
    MatrixXd F0(num_basisf,num_basisf);
    MatrixXd FC(num_basisf,num_basisf);
    MatrixXd FX(num_basisf,num_basisf);
    MatrixXd AO_eigenvectors(num_basisf,num_basisf);
    MatrixXd D0(num_basisf,num_basisf);
    MatrixXd DC(num_basisf,num_basisf);
    MatrixXd DX(num_basisf,num_basisf);
    MatrixXd FX_org(num_basisf,num_basisf);
    MatrixXd FC_org(num_basisf,num_basisf);
    VectorXd eri_array(num_basisf*num_basisf*num_basisf*num_basisf);

 // Reading variables
 // Nuclear repulsion energy: enuc.dat --> enuc
 // Kinetic energy inegrals: t.dat --> t_array
 // Nuclear attraction inegrals: v.dat --> v_array
 // AO Overlap: s.dat --> s_array
 // Core Hamiltonian: hcore

 // Reading enuc
    ifstream enucfile ("../mol/enuc.dat");
    enucfile >> enuc;
//  Reading One-Electron Inegrals

    ifstream tfile ("../mol/t.dat");
    ifstream vfile ("../mol/v.dat");
    ifstream sfile ("../mol/s.dat");
    while(getline(tfile, newline)){
        int i,j;
        double temp_t;
        istringstream ss(newline);
        ss >> i >> j >> temp_t;
        t_array(i-1,j-1) = temp_t;
        if(i != j ) t_array(j-1,i-1) = temp_t;
    }

    while(getline(vfile, newline)){
        int i,j;
        double temp_v;
        istringstream ss(newline);
        ss >> i >> j >> temp_v;
        v_array(i-1,j-1) = temp_v;
        if(i != j ) v_array(j-1,i-1) = temp_v;
    }
    while(getline(sfile, newline)){
        int i,j;
        double temp_s;
        istringstream ss(newline);
        ss >> i >> j >> temp_s;
        s_array(i-1,j-1) = temp_s;
        if(i != j ) s_array(j-1,i-1) = temp_s;
    }
//  Core Hamiltonian hcore;
//  Hcore = T + V
        hcore = t_array + v_array;

//  Read Two Electron repulsion data
//  eri.dat > eri_array
    ifstream erifile ("../mol/eri.dat");
    while(getline(erifile, newline)){
        int i, j, ij, k, l, kl, ijkl;
        double temp_eri;
        istringstream ss(newline);
        ss >> i >> j >> k >> l >> temp_eri;
        ij = INDEX(i,j);
        kl = INDEX(k,l);
        ijkl = INDEX(ij,kl);
        eri_array(ijkl) = temp_eri;
    }

//  Orthogonlization matrix
// V * D^-1/2 * V^-1
    SelfAdjointEigenSolver<MatrixXd> s_eigen(s_array);

//  Initial Guess of Wavefunction
    F0 = s_eigen.operatorSqrt().inverse().transpose() * hcore * s_eigen.operatorSqrt().inverse();
    SelfAdjointEigenSolver<MatrixXd> F_eigen(F0);
    AO_eigenvectors = s_eigen.operatorSqrt().inverse() * F_eigen.eigenvectors();
    for ( int i = 0 ; i < num_basisf ; i++){
        for ( int j = 0 ; j < num_basisf; j++){
            for (int k = 0 ; k < occ ; k++){
               D0(i,j) +=  AO_eigenvectors.col(k).row(i) * AO_eigenvectors.col(k).row(j);
            }
        }
    }
//  Compute Inital Hartree Fock Energy
    for (int i =0 ; i < num_basisf; i++){
        for (int j =0 ; j < num_basisf; j++){
            e0 += D0(i,j) * (hcore(i,j) + hcore(i,j));
        }
    }
    etotal = e0 + enuc;
    DC = DX = D0;
    eC = eX = e0;
    cout << setw(10) << "Iter";
    cout << setw(25)<< setprecision(15) << "E(elec)";
    cout << setw(25)<< setprecision(15) << "E(Total)";
    cout <<setw(25) << setprecision(15) << "D(E)" ;
    cout << setw(25) << setprecision(15) << "RMS(D)" << endl;
    cout << setw(10) << "0";
    cout << setw(25)<< setprecision(15) << eC;
    cout << setw(25)<< setprecision(15) << eC + enuc;
    cout <<setw(25) << setprecision(15) << "0.00";
    cout << setw(25) << setprecision(15) << "0.00" << endl;
// enter into SCF loop
for (int iter = 1 ; iter <= MAX_ITER; iter++){
// Build new Fock Matrix
    for ( int i =1; i <= num_basisf; i++){
        for (int j =1 ; j <= num_basisf; j++){
            FC(i-1,j-1) = hcore(i-1,j-1);
            for(int k =1; k <= num_basisf; k++){
                for( int l=1; l <= num_basisf; l++){
                    int ij = INDEX(i,j);
                    int kl = INDEX(k,l);
                    int ijkl = INDEX(ij,kl);
                    int ik = INDEX(i,k);
                    int jl = INDEX(j,l);
                    int ikjl = INDEX(ik,jl);
                    FC(i-1,j-1) += DC(k-1,l-1) * ( 2.0 * eri_array(ijkl) - eri_array(ikjl));
                }
            }
        }
    }

//  Build Density Matrix
    FC_org = s_eigen.operatorSqrt().inverse().transpose() * FC * s_eigen.operatorSqrt().inverse();
    SelfAdjointEigenSolver<MatrixXd> FC_eigen(FC_org);
    MatrixXd AOC_eigenvectors(num_basisf,num_basisf);
    AOC_eigenvectors = s_eigen.operatorSqrt().inverse() * FC_eigen.eigenvectors();
    for ( int i = 0 ; i < num_basisf ; i++){
        for ( int j = 0 ; j < num_basisf; j++){
            DC(i,j)= 0;
            for (int k = 0 ; k < occ ; k++){
               DC(i,j) +=  AOC_eigenvectors.col(k).row(i) * AOC_eigenvectors.col(k).row(j);
            }
        }
    }

//  Compute new Hartree Fock Energy
    eC = 0;
    for (int i =0 ; i < num_basisf; i++){
        for (int j =0 ; j < num_basisf; j++){
            eC += DC(i,j) * ( hcore(i,j) + FC(i,j));
        }
    }
    Ediff = eC - eX;
    rms = rmsDensity(DC,DX,num_basisf);
    cout << setw(10) << iter;
    cout << setw(25)<< setprecision(15) << eC;
    cout << setw(25)<< setprecision(15) << eC + enuc;
    cout <<setw(25) << setprecision(15) << Ediff;
    cout << setw(25) << setprecision(15) << rms << endl;

    if (Ediff < EDIFF_CONV && rms < RMS_CONV){
        cout << "CONVERGENCE has been reached" << endl;
        break;
    }else if(iter == MAX_ITER){
        cout << "NO CONVERGENCE has been reached" << endl;
    }
    //Save current information for next loop
    eX = eC;
    DX = DC;

} //end of SCF Loop
}


#include <iostream>
#include <vector>
#include <string>
#include <math.h>
#include <ctime>
#include <armadillo>

void plotData(std::vector<double> data);

class Layer {
    arma::mat sigmoid(arma::mat x) {
        return 1.0 / (1.0 + exp(-1.0*x));
    }

    public:
    arma::mat weights;
    arma::mat negativeVisibleProbabilities;
    arma::umat positiveHiddenStates;
    arma::mat positiveAssociations, negativeAssociations;

    Layer() {
        weights.randu(1, 1);
    }

    Layer(int neuronCount, int inputsPerNeuron) {
        arma::arma_rng::set_seed(1);
        weights.randn(inputsPerNeuron, neuronCount);
        weights *= 0.1;
        weights.insert_rows(0, 1);
        weights.insert_cols(0, 1);
    }

    void calculatePositiveAssociations(arma::mat x) {
        arma::mat positiveHiddenProbabilities = sigmoid(x*weights);
        arma::arma_rng::set_seed(1);
        arma::mat randomNormalProbabilties = arma::randu(size(positiveHiddenProbabilities));
        positiveHiddenStates = positiveHiddenProbabilities > randomNormalProbabilties;
        positiveAssociations = x.t()*positiveHiddenProbabilities;
    }

    void calculateNegativeAssociations() {
        arma::mat negativeVisibleActivations = positiveHiddenStates*weights.t();
        negativeVisibleProbabilities = sigmoid(negativeVisibleActivations);
        negativeVisibleProbabilities.col(0) = arma::ones<arma::vec>(negativeVisibleProbabilities.n_rows);
        arma::mat negativeHiddenActivations = negativeVisibleActivations*weights;
        arma::mat negativeHiddenProbabilities = sigmoid(negativeHiddenActivations);
        negativeAssociations = negativeVisibleProbabilities.t()*negativeHiddenProbabilities;
    }

    void run(arma::mat x) {
        calculatePositiveAssociations(x);
        calculateNegativeAssociations();
    }
};

class RBM {
    arma::mat input;
    Layer L1;
    int L1NodeCount = 2;
    double learningRate = 0.0005;
    double exampleCount;

    arma::mat sigmoid_derivative(arma::mat x) {
        return x % (1-x);
    }

    void randomInitWeights(int visibleNodeCount) {
        L1 = Layer(L1NodeCount, visibleNodeCount);
    }

    public:

    RBM(arma::mat i) {
        input = join_rows(arma::ones<arma::mat>(i.n_rows, 1), i);
        randomInitWeights(i.n_cols);
        exampleCount = i.n_rows;
    }

    void train(int numIt) {
        double error;

        for(int i=0; i<numIt; i++) {
            L1.run(input);
            L1.weights += learningRate * ((L1.positiveAssociations-L1.negativeAssociations) / exampleCount);

            if (i%1000 == 0) {
                error = accu(square(input - L1.negativeVisibleProbabilities));
                std::cout << "Step "<< i<<": "<< error << std::endl;
            }
        }
        error = accu(square(input - L1.negativeVisibleProbabilities));
        std::cout << "Step "<< numIt <<": "<< error << std::endl;
    }

    arma::umat run(arma::mat i) {
        arma::mat input = join_rows(arma::ones<arma::mat>(i.n_rows, 1), i);
        L1.calculatePositiveAssociations(input);
        return L1.positiveHiddenStates.cols(1, L1.positiveHiddenStates.n_cols-1);
    }
};

void plotData(std::vector<double> data) {
    FILE *pipe = popen("gnuplot -persist" , "w");

    if (pipe != NULL) {

        fprintf(pipe, "set style line 5 lt rgb 'cyan' lw 3 pt 6 \n");
        fprintf(pipe, "plot '-' with linespoints ls 5 \n");

        for (int i=0; i<data.size(); i++) {
            fprintf(pipe, "%lf %lf\n", double(i), data[i]);
        }
        fprintf(pipe, "e");

        fflush(pipe);
        pclose(pipe);
    }
    else {
        std::cout << "Could not open gnuplot pipe" << std::endl;
    }
}

int main() {
    arma::mat input = {{1,1,1,0,0,0},{1,0,1,0,0,0},{1,1,1,0,0,0},{0,0,1,1,1,0},{0,0,1,1,0,0},{0,0,1,1,1,0}};
    int numIterations = 6000;

    std::cout << "RBM trained on multiple user's movie preferences" << std::endl;
    RBM model(input);

    std::clock_t startTime;
    startTime = std::clock();
    model.train(numIterations);
    std::cout << "Training Time: " << (std::clock() - startTime) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl << std::endl;
    arma::mat test = {{0,0,0,1,1,0}};
    std::cout << "Test User:" << std::endl;
    std::cout << "Likes 1st Harry Potter:\t" << test(0,0) << std::endl;
    std::cout << "Likes Avatar:\t\t" << test(0,1) << std::endl;
    std::cout << "Likes 3rd LOTR:\t\t" << test(0,2) << std::endl;
    std::cout << "Likes Gladiator:\t" << test(0,3) << std::endl;
    std::cout << "Likes Titanic:\t\t" << test(0,4) << std::endl;
    std::cout << "Likes Troll 2:\t\t" << test(0,5) << std::endl << std::endl;
    startTime = std::clock();
    arma::umat hiddenStates = model.run(test);
    std::cout << "Prediction Time: " << (std::clock() - startTime) / (double)(CLOCKS_PER_SEC / 1000) << " ms" << std::endl;
    std::cout << "Hidden Neuron Activations:" << std::endl;
    std::cout << "Likes Oscar Winners:\t" << hiddenStates(0,0) << std::endl;
    std::cout << "Likes SciFi / Fantasy:\t" << hiddenStates(0,1) << std::endl;
    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <random>

template<typename T>
using PCoord = Eigen::Matrix<T, 1, 3>;

int factorial(int n) {
    if(n==0) return 1;
    int ret = 1;
    for(int i=1; i<=n; i++) ret *= i;
    return ret;
}

template<typename T>
class WaveFn {
public:
    virtual ~WaveFn(){};
    // return the value of wave function
    virtual T value(const PCoord<T>&)=0;

    // return three components of grad wave function
    virtual PCoord<T> grad(const PCoord<T>&)=0;

    // return laplacian of the wave function
    virtual T laplace(const PCoord<T>&)=0;
};

// n define the quantum number n of wave function
// for lithium atom only (n=1, 2)
template<typename T, int N>
class SlaterWaveFn: public WaveFn<T> {
public:
    const int n = N;
    const int nterm = 7;
    SlaterWaveFn(const PCoord<T>& r0): r0(r0){
        for(int i=0; i<nterm; i++) {
            // recaluate zeta_val accorindg to orbital
            zeta_val[i] = zeta_val[i]/slap[n-1];
            norm_const[i] = std::pow(2*zeta_val[i], pnu_val[i]-0.5)/
                            std::sqrt(factorial(2*pnu_val[i]));
        }
    }
    ~SlaterWaveFn(){};

    T value(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        T ret = 0.0;
        for(int i=0; i<nterm; i++) {
            ret += norm_const[i]*phi_val[n-1][i]*
                   std::pow(dist, pnu_val[i]-1)*std::exp(-dist*zeta_val[i]);
        }
        return ret;
    }

    PCoord<T> grad(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        auto nvec = (r - r0).normalized();
        auto scalar_derv = 0;
        for(int i=0; i<nterm; i++) {
            scalar_derv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) - zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
        }
        return scalar_derv*nvec;
    }

    T laplace(const PCoord<T>& r) {
        auto dist = (r - r0).norm();
        auto scalar_derv = 0;
        auto scalar_dderv = 0;
        for(int i=0; i<nterm; i++) {
            // (ab)'' = a''b + 2a'b'+ ab''
            scalar_derv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) - zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
            scalar_dderv += norm_const[i]*phi_val[n-1][i]*(
                                (pnu_val[i] - 1)*(pnu_val[i] - 2)*pow(dist, pnu_val[i] - 3) -
                                2*zeta_val[i]*(pnu_val[i] - 1)*pow(dist, pnu_val[i] - 2) +
                                zeta_val[i]*zeta_val[i]*pow(dist, pnu_val[i] - 1))*
                                std::exp(-dist*zeta_val[i]);
        }
        scalar_dderv = scalar_dderv + 2*scalar_derv/dist; // why?
        return  scalar_derv;
    }

private:
    PCoord<T> r0;

    T phi_val[2][7] = {
        {-0.12220686, 1.11273225, 0.04125378, 0.09306499, -0.10260021, -0.00034191, 0.00021963},
        {0.47750469, 0.11140449,-1.25954273, -0.18475003, -0.02736293, -0.00025064, 0.00057962}
    };
    T zeta_val[7] = {
        0.72089388, 2.61691643, 0.69257443, 1.37137558, 3.97864549, 13.52900016, 19.30801440
    };
    T norm_const[7];
    int pnu_val[7] = {
        1, 1, 2, 2, 2, 2, 3 // 1s, 1s, 2s, 2s, 2s, 2s, 3s
    };
    T slap[2] = {1.00, 0.95};
};

// For Jastrow term of wave function
template<typename T>
class Jastrow: {
public:
    Jastrow(const PCoord<T> r1, const PCoord<T> r2): r1(r1), r2(r2) {

    }

private:
    PCoord<T> r1, r2;

};

// For lithium atom + Jastrow wave function
// Not a child class of wave function
template<typename T>
class SlaterDet: {
public:
    // Radom initialization, r0 is the position of nucleus
    SlaterDet(const PCoord<T> r0): r0(r0) {
        r1 = PCoord<T>::Random();
        r2 = PCoord<T>::Random();
        r3 = PCoord<T>::Random();
    }

    // Given initial coordinate
    SlaterDet(const PCoord<T> r0, const PCoord<T> r1, const PCoord<T> r2, const PCoord<T> r3)
        :r0(r0), r1(r1), r2(r2), r3(r3) {}

    ~SlaterDate() {
        delete s1, s2, s3;
    }

    T eval() {
        // For initialize evaluation and validation
        sdet << s1->value(r1), s2 -> value(r1), 0.0,
                s1->value(r2), s2 -> value(r2), 0.0,
                0.0, 0.0, s3 -> value(r3);
        inv_sdet = sdet.inverse();
        return sdet.determinant();
    }

    void update(PCoord<T> r, int i) {
        // update the slater det matrix and the inverse matrix
        std::uniform_real_distribution<T> rnum(0, 1);
        PCoord<T> svec;
        switch (i) {
            case 0:
                auto r = r1 + PCoord<T>::Random();
                svec << s1 -> value(r), s2 -> value(r), 0.0;
                break;
            case 1:
                auto r = r2 + PCoord<T>::Random();
                svec << s1 -> value(r), s2 -> value(r), 0.0;
                break;
            case 2;
                auto r = r3 + PCoord<T>::Random();
                svec << 0.0, 0.0, s3->value(r);
                break;
            default:
                break;
        }

        // Calculate Dnew/Dold
        auto ratio = (svec * inv_sdet).sum();
        // apply Sherman–Morrison formula
        // see https://en.wikipedia.org/wiki/Sherman%E2%80%93Morrison_formula
        if(ratio > 1 || ratio > rnum(gen)) {
            auto irow = inv_sdet.rows(i)/ratio;
            inv_sdet = inv_sdet - svec*inv_sdet*inv_sdet.transpose()/ratio;
            inv_sdet.rows(i) = irow;
            update = true;
        } else update = false;
    }

    // do we need this?
    // T value() {
    // }

    PCoord<T> grad(int i) {
        PCoord<T> svec;
        switch (i) {
            case 0:
                svec << s1 -> grad(r1), s2 -> grad(r1), 0.0;
                break;
            case 1:
                svec << s1 -> grad(r2), s2 -> grad(r2), 0.0;
                break;
            case 2;
                svec << 0.0, 0.0, s3->grad(r3);
                break;
            default:
                throw std::runtime_error("No such index" + std::to_string(i) + " exists");
        }
        return inv_sde * svec;
    }

    T energy() {
        // two different cases, whether/not sdet is updated
        if(update) {
            return 0;
        } else {
            return 0;
        }
    }

    T laplace(int i) {
        PCoord<T> svec;
        switch (i) {
            case 0:
                svec << s1 -> laplace(r1), s2 -> laplace(r1), 0.0;
                break;
            case 1:
                svec << s1 -> laplace(r2), s2 -> laplace(r2), 0.0;
                break;
            case 2;
                svec << 0.0, 0.0, s3->laplace(r3);
                break;
            default:
                throw std::runtime_error("No such index" + std::to_string(i) + " exists");
                break;
        }
        return inv_sde * svec;
    }

private:
    PCoord<T> r0;
    PCoord<T> r1, r2, r3;
    SlaterWaveFn<T, 1>* s1;
    SlaterWaveFn<T, 1>* s2; // 1s alpha, 1s beta
    SlaterWaveFn<T, 2>* s3; // 2s alpha
    SlaterDet<T, 3, 3> sdet, inv_sdet; // Slater determinant
    std::random_device rd;
    std::mt19937 rgen{rd()}
    bool update = false;
};