#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_adaptive_adams_coefficients

#include <boost/test/unit_test.hpp>

#include <boost/numeric/odeint/stepper/detail/adaptive_adams_coefficients.hpp>

#include <vector>

#include <boost/mpl/list.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/range_c.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;

typedef double value_type;

BOOST_AUTO_TEST_SUITE( adaptive_adams_coefficients_test )

typedef boost::mpl::range_c< size_t , 2 , 10 > vector_of_steps;
BOOST_AUTO_TEST_CASE_TEMPLATE( test_step, step_type, vector_of_steps )
{
    const static size_t steps = step_type::value;

    typedef std::vector<double> deriv_type;
    typedef double time_type;

    typedef detail::adaptive_adams_coefficients<steps, deriv_type, time_type> aac_type;

    std::vector<double> deriv;
    deriv.push_back(-1);

    time_type t = 0.0;
    time_type dt = 0.1;

    aac_type coeff;
    for(size_t i=0; i<steps; ++i)
    {
        coeff.predict(t, dt);
        coeff.do_step(deriv);
        coeff.confirm();

        t+= dt;

        if(coeff.m_eo < steps)
            coeff.m_eo ++;
    }

    std::vector<value_type> v(10);
    v[0] = 1.0/1.0;
    v[1] = 1.0/2.0;
    v[2] = 5.0/12.0;
    v[3] = 9.0/24.0;
    v[4] = 251.0/720.0;
    v[5] = 95.0/288.0;
    v[6] = 19087.0/60480.0;
    v[7] = 5257.0/17280.0;
    v[8] = 5311869667636789.0/18014398509481984.0;

    for(size_t i=0; i<steps; ++i)
    {
        BOOST_CHECK_SMALL(coeff.beta[1][i] - 1.0, 1e-15);

        if(i == 0)
            BOOST_CHECK_SMALL(coeff.phi[2][i].m_v[0] + 1, 1e-15);
        else if (i == steps-1 && steps%2 == 1)
            BOOST_CHECK_SMALL(coeff.phi[2][i].m_v[0] - 1, 1e-14);
        else if (i == steps-1 && steps%2 == 0)
            BOOST_CHECK_SMALL(coeff.phi[2][i].m_v[0] + 1, 1e-14);
        else
            BOOST_CHECK_SMALL(coeff.phi[2][i].m_v[0], 1e-15);

        BOOST_CHECK_SMALL(coeff.g[i] - v[i], 1e-15);
    }
}

BOOST_AUTO_TEST_CASE( test_copy )
{
    typedef std::vector<double> deriv_type;
    typedef double time_type;

    typedef detail::adaptive_adams_coefficients<3, deriv_type, time_type> aac_type;
    aac_type c1;

    deriv_type deriv(1);
    deriv[0] = 1.0;

    time_type t = 0.0;
    time_type dt = 0.01;

    for(size_t i=0; i<3; ++i)
    {
        c1.predict(t, dt);
        c1.do_step(deriv);
        c1.confirm();

        t+= dt;

        if(c1.m_eo < 3)
            c1.m_eo ++;
    }

    aac_type c2(c1);
    BOOST_CHECK_EQUAL(c1.phi[0][0].m_v[0], c2.phi[0][0].m_v[0]);
    BOOST_CHECK(&(c1.phi[0][0].m_v) != &(c2.phi[0][0].m_v));

    aac_type c3;
    deriv_type *p1 = &(c3.phi[0][0].m_v);

    c3 = c1;
    BOOST_CHECK(p1 == (&(c3.phi[0][0].m_v)));
    BOOST_CHECK_EQUAL(c1.phi[0][0].m_v[0], c3.phi[0][0].m_v[0]);
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_MODULE pcraster model_engine areaoperations
#include <boost/test/unit_test.hpp>
#include "com_csfcell.h"
#include "calc_orderoperations.h"

#define protected public
#include "calc_areaoperations.h"

// NOTE use string failureExpected in files expected to fail, see style guide

BOOST_AUTO_TEST_CASE(testA)
{
  using namespace calc;

  {
   AreaTotalOperation ao;
   UINT1 id[1]  = { 1 };
   REAL4 val[1] = { 8 };
   ao.apply(val,id,1);
   BOOST_CHECK(ao.d_map.size()==1);
   BOOST_CHECK(val[0]==8);
  }
  {
   AreaTotalOperation ao;

   UINT1 id[4]  = { 1, 1, MV_UINT1, 2  };
   REAL4 val[4] = { 8, 8, 100,      10 };

   REAL4 res[4] = {16,16, 100,      10 };
   pcr::setMV(res[2]);

   ao.apply(val,id,4);
   BOOST_CHECK(ao.d_map.size()==2);
   BOOST_CHECK(ao.d_map[1].nr()==2);
   BOOST_CHECK(ao.d_map[2].nr()==1);
   BOOST_CHECK(ao.d_map[1].sum()==16);
   BOOST_CHECK(ao.d_map[2].sum()==10);

   BOOST_CHECK(val[0]==16);
   BOOST_CHECK(val[1]==16);
   BOOST_CHECK(pcr::isMV(val[2]));
   BOOST_CHECK(val[3]==10);

  }
/*
  {
   AreaTotalOperation ao;
   UINT1 id[4]  = { 1, 1, MV_UINT1, 2 };
   REAL4 val[4] = { 8, 4, 4,        1 };
   ao.apply(id,4,val,4);
   BOOST_CHECK(ao.d_map.size()==2);
   BOOST_CHECK(ao.d_map[1].nr()==2);
   BOOST_CHECK(ao.d_map[2].nr()==1);
   double res[2]={-1,-1};
   ao.setResults(res,2);
   BOOST_CHECK(res[0]==6);
   BOOST_CHECK(res[1]==1);
  }
*/
}

BOOST_AUTO_TEST_CASE(testAreaOrder)
{
  using namespace calc;

   REAL4 exprV[9]      = { 9,2,3,4,5,6,7,8       ,-999 };
   pcr::setMV(exprV[8]);

   UINT1 areaClassV[9] = { 1,1,0,0,0,0,1,MV_UINT1,0 };
   REAL4 expectedV[9]  = { 3,1,1,2,3,4,2,-999    ,0 };
   REAL4 result[9]     = { 0,0,0,0,0,0,0,0       ,0 };

   VSpatial<double,REAL4> expr(exprV);
   VSpatial<INT4,UINT1>   areaClass(areaClassV);

   areaOrderOperation(result, expr,areaClass,9);

   for(size_t i=0; i< 7 ; ++i)
    BOOST_CHECK(expectedV[i]==result[i]);
   BOOST_CHECK(pcr::isMV(result[7]));
   BOOST_CHECK(pcr::isMV(result[8]));
}


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

/* GTestReplicate.cpp - Testing the functionality of replicating one
 * slot from a vector acress the whole vector (or replicating each slot
 * to a full cipehrtext)
 */

#include <cassert>
#include <NTL/lzz_pXFactoring.h>

#include <helib/helib.h>
#include <helib/replicate.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {
struct Parameters {
    Parameters(long m, long p, long r, long d, long L, long bnd, long B) :
        m(m),
        p(p),
        r(r),
        d(d),
        L(L),
        bnd(bnd),
        B(B)
    {};

  long m; // cyclotomic ring
  long p; // plaintext base
  long r; // lifting
  long d; // degree of the field extension
          // d == 0 => factors[0] defines extension
  long L; // heuristic
  long bnd; // recursion bound for replication
  long B; // bound for # of replications

  friend std::ostream& operator<<(std::ostream& os, const Parameters params)
  {
      return os << "{"
          << "m=" << params.m << ","
          << "p=" << params.p << ","
          << "r=" << params.r << ","
          << "d=" << params.d << ","
          << "L=" << params.L << ","
          << "bnd=" << params.bnd << ","
          << "B=" << params.B
          << "}";
  };
};

class GTestReplicate : public ::testing::TestWithParam<Parameters> {
    protected:
        static void printContextAndG(const helib::Context& context, const NTL::ZZX& G)
        {
            if (!helib_test::noPrint) {
                context.zMStar.printout();
                std::cout << std::endl;
                std::cout << "G = " << G << "\n";
            }
        };

        static NTL::ZZX createG(const helib::Context& context, long p, long d)
        {
            return (d == 0) ? context.alMod.getFactorsOverZZ()[0] : helib::makeIrredPoly(p, d);
        };

        GTestReplicate() :
            m(GetParam().m),
            p(GetParam().p),
            r(GetParam().r),
            d(GetParam().d),
            L(GetParam().L),
            bnd(GetParam().bnd),
            B(GetParam().B),
            context((helib::setDryRun(helib_test::dry), helib::setTimersOn(), m), p, r),
            secretKey((buildModChain(context, L, /*c=*/2), context)),
            G(createG(context, p, d)),
            publicKey((printContextAndG(context, G),
                    secretKey.GenSecKey(), // A +-1/0 secret key
                    addSome1DMatrices(secretKey),
                    secretKey)),
            ea(context, G),
            xp0(ea),
            xp1(ea),
            xc0(publicKey),
            xc1(publicKey)
        {};

        virtual void SetUp() override
        {
            random(ea, xp0);
            random(ea, xp1);

            ea.encrypt(xc0, publicKey, xp0);
            ea.encode(poly_xp1, xp1);
            xc1 = xc0;

#ifdef DEBUG_PRINTOUT
            helib::dbgKey = &secretKey;
            helib::dbgEa = const_cast<helib::EncryptedArray*>(context.ea);
#endif // DEBUG_PRINTOUT
        };

        virtual void TearDown() override
        {
          helib::cleanupGlobals();
        }

        const long m;
        const long p;
        const long r;
        const long d;
        const long L;
        const long bnd;
        const long B;
        helib::Context context;
        helib::SecKey secretKey;
        NTL::ZZX G;
        const helib::PubKey& publicKey;
        helib::EncryptedArray ea;
        helib::PlaintextArray xp0;
        helib::PlaintextArray xp1;
        helib::Ctxt xc0;
        helib::Ctxt xc1;
        NTL::ZZX poly_xp1; // To be default constructed
};

::testing::AssertionResult replicationSucceeds(const helib::Ctxt& c1, const helib::Ctxt& c0, long i,
			    const helib::SecKey& sKey, const helib::EncryptedArray& ea)
{
  helib::PlaintextArray pa0(ea), pa1(ea);
  ea.decrypt(c0, sKey, pa0);
  ea.decrypt(c1, sKey, pa1);
  replicate(ea, pa0, i);

  if(equals(ea, pa1, pa0)) // replication succeeded
      return ::testing::AssertionSuccess();
  else
      return ::testing::AssertionFailure() << "Replication failed";
};

class StopReplicate { };

// A class that handles the replicated ciphertexts one at a time
class ReplicateTester : public helib::ReplicateHandler {
public:
  const helib::SecKey& sKey;
  const helib::EncryptedArray& ea;
  const helib::PlaintextArray& pa;
  long B;

  double t_last, t_total;
  long pos;
  bool error;

  ReplicateTester(const helib::SecKey& _sKey, const helib::EncryptedArray& _ea,
                  const helib::PlaintextArray& _pa, long _B)
  : sKey(_sKey), ea(_ea), pa(_pa), B(_B)
  {
    t_last = NTL::GetTime();
    t_total = 0.0;
    pos = 0;
    error = false;
  }

  // This method is called for every replicated ciphertext: in the i'th time
  // that it is called, the cipehrtext will have in all the slots the content
  // of the i'th input slot. In this test program we only decrypt and check
  // the result, in a real program it will do something with the cipehrtext.
  virtual void handle(const helib::Ctxt& ctxt) {

    double t_new = NTL::GetTime();
    double t_elapsed = t_new - t_last;
    t_total += t_elapsed;

    // Decrypt and check
    helib::PlaintextArray pa1 = pa;
    helib::replicate(ea, pa1, pos);
    helib::PlaintextArray pa2(ea);

    if (pos==0 && !helib_test::noPrint) CheckCtxt(ctxt, "replicateAll");

    ea.decrypt(ctxt, sKey, pa2);
    if (!equals(ea, pa1, pa2)) error = true; // record the error, if any
    t_last = NTL::GetTime();

    pos++;
    if (B > 0 && pos >= B) throw StopReplicate();
  }
};

TEST_P(GTestReplicate, replicateWorks)
{
  if (!helib_test::noPrint) {
      std::cout << "** Testing replicate():\n";
      CheckCtxt(xc1, "before replicate");
  }
  helib::replicate(ea, xc1, ea.size()/2);
  EXPECT_TRUE(replicationSucceeds(xc1, xc0, ea.size()/2, secretKey, ea));
  if (!helib_test::noPrint) CheckCtxt(xc1, "after replicate");
};

TEST_P(GTestReplicate, repeatedReplicationWorks)
{
  // Get some timing results
  for (long i=0; i<20 && i<ea.size(); i++) {
    xc1 = xc0;
    FHE_NTIMER_START(replicate);
    helib::replicate(ea, xc1, i);
    EXPECT_TRUE(replicationSucceeds(xc1, xc0, i, secretKey, ea));
    FHE_NTIMER_STOP(replicate);
  }
  if(!helib_test::noPrint) {
      helib::printAllTimers();
  }
};

TEST_P(GTestReplicate, replicateAllReplicatesAccurately)
{
    if(!helib_test::noPrint) {
        std::cout << "\n** Testing replicateAll()... " << std::flush;
    }
#ifdef DEBUG_PRINTOUT
    helib::replicateVerboseFlag = true;
#else
    helib::replicateVerboseFlag = false;
#endif
  ReplicateTester handler(secretKey, ea, xp0, B);
  try {
    FHE_NTIMER_START(replicateAll);
    helib::replicateAll(ea, xc0, &handler, bnd);
  }
  catch (StopReplicate) {
  }
  EXPECT_FALSE(handler.error);
  if(!helib_test::noPrint) {
  std::cout << "total time=" << handler.t_total << " ("
	    << ((B>0)? B : ea.size())
	    << " vectors)\n";
  }
};

INSTANTIATE_TEST_SUITE_P(typicalParameters, GTestReplicate, ::testing::Values(
            //SLOW
            Parameters(1247, 2, 1, 1, 250, 64, 0)
            //FAST
            //Parameters(91, 2, 1, 1, 250, 64, 0)
            ));

} // namespace


//==============================================================================
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/table.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/plus.hpp>
#include <boost/fusion/include/at.hpp>
#include <iostream>

#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/metric/cycles_per_element.hpp>
#include <nt2/sdk/bench/protocol/max_duration.hpp>
#include <nt2/sdk/bench/setup/geometric.hpp>
#include <nt2/sdk/bench/setup/combination.hpp>
#include <nt2/sdk/bench/stats/median.hpp>

using namespace nt2::bench;
using namespace nt2;

template<typename T> struct rgb2grey_nt2
{
  template<typename Setup>
  rgb2grey_nt2(Setup const& s)
                    :  height(boost::fusion::at_c<0>(s))
                    ,  width(boost::fusion::at_c<1>(s))
                    ,  size_(height*width)
  {
    gr.resize(nt2::of_size(height,width));
    r.resize(nt2::of_size(height,width));
    g.resize(nt2::of_size(height,width));
    b.resize(nt2::of_size(height,width));

    for(std::size_t i=1; i<=size_; i++)
      r(i) = g(i) = b(i) = gr(i) = T(i-1);
  }

  void operator()()
  {
    gr = T(0.3) * r + T(0.59) * g + T(0.11) * b;
  }

  friend std::ostream& operator<<(std::ostream& os, rgb2grey_nt2<T> const& p)
  {
    return os << "(" << p.size() << ")";
  }

  std::size_t size() const { return size_; }

  private:
  std::size_t height, width, size_;
  nt2::table<T> r, g, b, gr;
};

NT2_REGISTER_BENCHMARK_TPL( rgb2grey_nt2, (float) )
{
  std::size_t hmin  = args("hmin" , 32);
  std::size_t hmax  = args("hmax" , 128);
  std::size_t hstep = args("hstep", 2);
  std::size_t wmin  = args("wmin" , hmin);
  std::size_t wmax  = args("wmax" , hmax);
  std::size_t wstep = args("wstep", hstep);

  run_during_with< rgb2grey_nt2<T> >( 1.
                                    , and_( geometric(hmin,hmax,hstep)
                                          , geometric(wmin,wmax,wstep)
                                          )
                                    , cycles_per_element<stats::median_>()
                                    );
}


#include <toynet/w2v.h>
#include <toynet/math.h>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace toynet {

std::vector<int> get_context(const std::vector<int>& words, int index, int historyN, int futureN)
{
    std::vector<int> ret;
    for (int j = std::max(0, index - historyN);  j < index;  ++j)
        ret.push_back(words[j]);
    for (int j = 1;  j <= futureN && index+j < words.size();  ++j)
        ret.push_back(words[index+j]);
    return ret;
}

void gradient_descent(ublas::matrix<double>& out, const ublas::matrix<double>& gradients, double lr)
{
    out -= gradients * lr;
}

CBOWModelGradients::CBOWModelGradients(int W, int D)
    : P(ublas::zero_matrix<double>(W, D))
    , O(ublas::zero_matrix<double>(W, D))
{
}

CBOWModel::CBOWModel(int W, int D, int historyN, int futureN)
    : W(W)
    , D(D)
    , historyN(historyN)
    , futureN(futureN)
    , P(W, D)
    , O(W, D)
{
}

void CBOWModel::save(std::ostream& os) const
{
    boost::archive::text_oarchive oa(os);
    oa << *this;
}

void CBOWModel::load(std::istream& is)
{
    boost::archive::text_iarchive ia(is);
    ia >> *this;
}

std::vector<std::pair<double, int>> CBOWModel::predict(const std::vector<int>& context) const
{
    ublas::vector<double> smax = predict_helper(context);
    std::vector<std::pair<double, int>> ret(W);
    for (int i = 0;  i < W;  ++i)
        ret[i] = std::make_pair(smax[i], i);
    std::sort(ret.begin(), ret.end(), std::greater<>());
    return ret;
}

double CBOWModel::predict(const std::vector<int>& context, int word) const
{
    ublas::vector<double> smax = predict_helper(context);
    return smax[word];
}

ublas::vector<double> CBOWModel::predict_helper(const std::vector<int>& context) const
{
    // average embedding of all context words
    ublas::vector<double> avg(D, 0.0);
    for (int wordidx : context)
        avg += ublas::row(P, wordidx);
    avg /= context.size();
    // output layer (before softmax)
    ublas::vector<double> out(W, 0.0);
    for (int i = 0;  i < W;  ++i)
        out[i] = dot_product(avg, row(O, i));
    // softmax
    ublas::vector<double> smax = softmax(out);
    return smax;
}

double CBOWModel::avg_log_prob(const std::vector<int>& words) const
{
    double sum = 0.0;
    for (int i = 0;  i < words.size();  ++i) {
        std::vector<int> context = get_context(words, i, historyN, futureN);
        double p = predict(context, words[i]);
        sum += std::log(p);
    }
    return sum / words.size();
}

CBOWModelGradients CBOWModel::gradients() const
{
    CBOWModelGradients ret(W, D);
    // TODO!
    return ret;
}

void CBOWModel::update(const CBOWModelGradients& gradients, double lr)
{
    gradient_descent(P, gradients.P, lr);
    gradient_descent(O, gradients.O, lr);
}

SimpleReporter::SimpleReporter(std::ostream& os)
    : os(os)
{
}

SimpleLearningRate::SimpleLearningRate(double lr, int cutoff, double base)
    : lr(lr)
    , cutoff(cutoff)
    , base(base)
{
}

void SimpleReporter::operator()(const ReportData& data) const
{
    os << "epoch " << data.epoch
       << " prob " << data.avg_log_prob
       << " lr " << data.lr
       << "\n";
}

double SimpleLearningRate::operator()(int epoch) const
{
    return (epoch <= cutoff)
         ? lr
         : lr * std::pow(base, epoch - cutoff);
}

Trainer::Trainer()
    : epochs(1)
    , D(50)
    , historyN(4)
    , futureN(4)
    , initReporter(nullptr)
    , epochReporter(nullptr)
    , exitReporter(nullptr)
    , learningRate(nullptr)
{
}

Trainer& Trainer::setEpochs(int epochs)
{
    this->epochs = epochs;
    return *this;
}

Trainer& Trainer::setEmbeddingSize(int D)
{
    this->D = D;
    return *this;
}

Trainer& Trainer::setHistoryN(int historyN)
{
    this->historyN = historyN;
    return *this;
}

Trainer& Trainer::setFutureN(int futureN)
{
    this->futureN = futureN;
    return *this;
}

Trainer& Trainer::setInitReporter(const Reporter *initReporter)
{
    this->initReporter = initReporter;
    return *this;
}

Trainer& Trainer::setEpochReporter(const Reporter *epochReporter)
{
    this->epochReporter = epochReporter;
    return *this;
}

Trainer& Trainer::setExitReporter(const Reporter *exitReporter)
{
    this->exitReporter = exitReporter;
    return *this;
}

Trainer& Trainer::setLearningRate(const LearningRate *learningRate)
{
    this->learningRate = learningRate;
    return *this;
}

CBOWModel Trainer::train(const std::vector<int>& corpus) const
{
    // Find W, the maximum number of words
    int W = *std::max_element(corpus.begin(), corpus.end()) + 1;
    CBOWModel model(W, D, historyN, futureN);
    int e = 0;
    double avg_log_prob = model.avg_log_prob(corpus);
    double lr = (*learningRate)(e);
    if (initReporter)
        (*initReporter)({e, avg_log_prob, lr});
    while (e <= epochs) {
        ++e;
        CBOWModelGradients gradients = model.gradients();
        lr = learningRate ? (*learningRate)(e) : 1.0;
        model.update(gradients, lr);
        avg_log_prob = model.avg_log_prob(corpus);
        if (epochReporter)
            (*epochReporter)({e, avg_log_prob, lr});
        // TODO: compute loss on validation data
    }
    if (exitReporter)
        (*exitReporter)({-1, avg_log_prob, lr});
    return model;
}

} // namespace toynet


#include <tuple>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

#include "vice/functors.h"
#include "vice/integrate.h"
#include "vice/benchmark.h"

namespace py = pybind11;

//
// Trapezoid functions
//
template <typename Scalar, typename ParamType, typename Integrator>
std::tuple<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Matrix<long int, Eigen::Dynamic, 1>, double>
benchmark_trapezoid (Scalar depth, Scalar duration, Scalar ingress,
                     Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, const Scalar texp,
                     ParamType param)
{
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  auto integrator = Integrator();
  return vice::benchmark::integrate(func, integrator, time, texp, param);
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
trapezoid_exact (Scalar depth, Scalar duration, Scalar ingress,
                 Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, Scalar texp)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fluence(time.rows());
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  for (int i = 0; i < time.rows(); ++i) {
    Scalar lower = time(i) - 0.5*texp;
    Scalar upper = time(i) + 0.5*texp;
    fluence(i) = func.integrate(lower, upper) / texp;
  }
  return fluence;
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
trapezoid_flux (Scalar depth, Scalar duration, Scalar ingress,
                Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> flux(time.rows());
  vice::functors::TrapFunctor<Scalar> func(depth, duration, ingress);
  for (int i = 0; i < time.rows(); ++i) {
    flux(i) = func(time(i));
  }
  return flux;
}

//
// Transit functions
//
template <typename Scalar, typename ParamType, typename Integrator>
std::tuple<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>, Eigen::Matrix<long int, Eigen::Dynamic, 1>, double>
benchmark_transit (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
                   Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, const Scalar texp,
                   ParamType param)
{
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  auto integrator = Integrator();
  return vice::benchmark::integrate(func, integrator, time, texp, param);
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
transit_exact (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
               Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time, Scalar texp,
               Scalar tol, unsigned max_depth)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fluence(time.rows());
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  for (int i = 0; i < time.rows(); ++i) {
    Scalar lower = time(i) - 0.5*texp;
    Scalar upper = time(i) + 0.5*texp;
    fluence(i) = func.integrate(lower, upper, tol, max_depth) / texp;
  }
  return fluence;
}

template <typename Scalar>
Eigen::Matrix<Scalar, Eigen::Dynamic, 1>
transit_flux (Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> u, Scalar r, Scalar b, Scalar tau,
              Eigen::Ref<const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> time)
{
  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> flux(time.rows());
  vice::functors::StarryFunctor<Scalar> func(u, r, b, tau);
  for (int i = 0; i < time.rows(); ++i) {
    flux(i) = func(time(i));
  }
  return flux;
}

//
// Python interface
//
PYBIND11_MODULE(benchmark, m) {

  m.def("trapezoid_flux",            &trapezoid_flux<double>);
  m.def("transit_flux",              &transit_flux<double>);

  m.def("trapezoid_exact",           &trapezoid_exact<double>);
  m.def("transit_exact",             &transit_exact<double>,
                                     py::arg("u"), py::arg("r"), py::arg("b"), py::arg("tau"), py::arg("t"), py::arg("texp"),
                                     py::arg("tol")=1e-12, py::arg("max_depth")=15);

  m.def("trapezoid_riemann",         &benchmark_trapezoid<double, unsigned, vice::integrate::riemann>);
  m.def("trapezoid_trapezoid_fixed", &benchmark_trapezoid<double, unsigned, vice::integrate::trapezoid_fixed>);
  m.def("trapezoid_simpson_fixed",   &benchmark_trapezoid<double, unsigned, vice::integrate::simpson_fixed>);
  m.def("trapezoid_trapezoid_adapt", &benchmark_trapezoid<double, double, vice::integrate::trapezoid_adapt>);
  m.def("trapezoid_simpson_adapt",   &benchmark_trapezoid<double, double, vice::integrate::simpson_adapt>);
  m.def("trapezoid_gauss",           &benchmark_trapezoid<double, double, vice::integrate::quadrature<15>>);

  m.def("transit_riemann",           &benchmark_transit<double, unsigned, vice::integrate::riemann>);
  m.def("transit_trapezoid_fixed",   &benchmark_transit<double, unsigned, vice::integrate::trapezoid_fixed>);
  m.def("transit_simpson_fixed",     &benchmark_transit<double, unsigned, vice::integrate::simpson_fixed>);
  m.def("transit_trapezoid_adapt",   &benchmark_transit<double, double, vice::integrate::trapezoid_adapt>);
  m.def("transit_simpson_adapt",     &benchmark_transit<double, double, vice::integrate::simpson_adapt>);
  m.def("transit_gauss",             &benchmark_transit<double, double, vice::integrate::quadrature<15>>);

}


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/trunc.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( modf_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
  {
    using result = std::pair < A0, A0>;
    BOOST_FORCEINLINE result operator()(A0 const& a0) const
    {
      A0 rounded = bs::trunc(a0);
      A0 rest    = a0-rounded;
      return result(rest, rounded);
    }
   };
} } }

#endif


#include "stdafx.h"

#include "problem.hpp"
#include "utility.hpp"

#include <fstream>
#include <regex>
#include <unordered_map>
#include <vector>

#include <boost/algorithm/string.hpp>

#pragma warning(disable: 26495) // warning C26495: Variable '%s' is uninitialized. Always initialize a member variable (type.6).

struct advent_2017_7 : problem
{
	advent_2017_7() noexcept : problem(2017, 7) {
	}

	struct node
	{
		std::string name;
		std::size_t weight;
		std::size_t total_weight;
		node* parent;
		std::vector<std::string> children;
		std::vector<gsl::not_null<node*>> child_nodes;
	};

protected:
	std::unordered_map<std::string, node> nodes;
	node* tree = nullptr;

	std::size_t set_total_weights(gsl::not_null<node*> n) {
		std::size_t total_weight = n->weight;
		for(const gsl::not_null<node*> ch : n->child_nodes) {
			total_weight += set_total_weights(ch);
		}
		n->total_weight = total_weight;
		return total_weight;
	}

	gsl::not_null<node*> find_bad_node(gsl::not_null<node*> n) {
		auto odd_one = utility::find_odd_one_out(std::begin(n->child_nodes), std::end(n->child_nodes), [](gsl::not_null<node*> lhs, gsl::not_null<node*> rhs) {
			return lhs->total_weight == rhs->total_weight;
		});
		if(odd_one != std::end(n->child_nodes)) {
			return find_bad_node(*odd_one);
		}
		return n;
	}

	void prepare_input(std::ifstream& fin) override {
		std::regex pattern(R"(([[:lower:]]+) \(([[:digit:]]+)\)( -> (.*))?)");
		std::regex child_pattern(R"(([[:lower:]]+)(, )?)");
		for(std::string line; std::getline(fin, line);) {
			std::smatch m;
			std::regex_search(line, m, pattern);
			const std::string name   = m[1];
			const std::size_t weight = std::stoull(m[2]);
			node n{ name, weight, 0, nullptr };
			//node n{ m[1], std::stoull(m[2]), 0, nullptr };
			if(m[4].matched) {
				std::string fragment = m[4].str();
				auto child_it        = std::sregex_iterator(std::begin(fragment), std::end(fragment), child_pattern);
				auto child_end       = std::sregex_iterator();
				for(; child_it != child_end; ++child_it) {
					std::smatch c = *child_it;
					n.children.push_back(c[1]);
				}
			}
			nodes[n.name] = n;
		}
		for(auto it = std::begin(nodes); it != std::end(nodes); ++it) {
			for(auto& child_name : it->second.children) {
				const gsl::not_null<node*> child = gsl::make_not_null(&nodes[child_name]);

				child->parent = &it->second;
				it->second.child_nodes.push_back(child);
			}
		}
		tree = &std::begin(nodes)->second;
		while(tree->parent != nullptr) {
			tree = tree->parent;
		}
		set_total_weights(gsl::make_not_null(tree));
	}

	std::string part_1() override {
		return tree->name;
	}

	std::string part_2() override {
		const gsl::not_null<node*> bad_node = find_bad_node(gsl::make_not_null(tree));
		const std::size_t bad_total         = bad_node->total_weight;
		for(const gsl::not_null<const node*> sib : bad_node->parent->child_nodes) {
			if(sib->total_weight != bad_total) {
				const std::ptrdiff_t discrepancy = gsl::narrow<std::ptrdiff_t>(sib->total_weight) - gsl::narrow<std::ptrdiff_t>(bad_total);
				const std::size_t correct_weight = gsl::narrow<std::size_t>(gsl::narrow<std::ptrdiff_t>(bad_node->weight) + discrepancy);
				return std::to_string(correct_weight);
			}
		}
		__assume(0);
	}
};

REGISTER_SOLVER(2017, 7);


#include <algorithm>

#include <boost/container_hash/hash.hpp>

#include <cbag/layout/via_info.h>
#include <cbag/layout/via_param.h>
#include <cbag/util/unique_heap.h>

namespace cbag {
namespace layout {

constexpr offset_t MAX_INT = std::numeric_limits<offset_t>::max();

venc_data::venc_data() = default;

via_info::via_info() = default;

via_info::via_info(vector &&cdim, vector &&s, std::vector<vector> &&s2_list,
                   std::vector<vector> &&s3_list, std::array<venc_info, 2> &&e_list)
    : cut_dim(std::move(cdim)), sp(std::move(s)), sp2_list(std::move(s2_list)),
      sp3_list(std::move(s3_list)), enc_list(std::move(e_list)) {
    if (sp2_list.empty()) {
        sp2_list.emplace_back(sp);
    }
    if (sp3_list.empty()) {
        sp3_list.insert(sp3_list.end(), sp2_list.begin(), sp2_list.end());
    }
}

cnt_t get_n_max(offset_t dim, offset_t w, offset_t sp) {
    if (sp == MAX_INT) {
        return (dim > w) ? 1 : 0;
    }
    return (dim + sp) / (w + sp);
}

const std::vector<vector> &get_enc_list(const venc_info &enc_info, offset_t width) {
    for (const auto &data : enc_info) {
        if (width <= data.width)
            return data.enc_list;
    }
    return enc_info.back().enc_list;
}

vector get_metal_dim(vector box_dim, vector arr_dim, const venc_info &enc_info, orient_2d_t dir_idx,
                     bool extend) {
    auto pdir_idx = 1 - dir_idx;
    auto wire_w = box_dim[pdir_idx];
    vector ans;
    ans[dir_idx] = MAX_INT;
    ans[pdir_idx] = wire_w;
    for (const auto &enc_vec : get_enc_list(enc_info, wire_w)) {
        vector enc_dim = {2 * enc_vec[0] + arr_dim[0], 2 * enc_vec[1] + arr_dim[1]};
        if (enc_dim[pdir_idx] <= box_dim[pdir_idx]) {
            if (extend || enc_dim[dir_idx] <= box_dim[dir_idx]) {
                // enclosure rule passed, get optimal metal dimension
                ans[dir_idx] = std::min(ans[dir_idx], enc_dim[dir_idx]);
            }
        }
    }
    if (ans[dir_idx] == MAX_INT) {
        // no solution
        ans[0] = 0;
        ans[1] = 0;
    } else {
        ans[dir_idx] = std::max(ans[dir_idx], box_dim[dir_idx]);
    }

    return ans;
}

offset_t get_arr_dim(cnt_t n, offset_t w, offset_t sp) {
    if (sp == MAX_INT)
        if (n != 1)
            return 0;
        else
            return w;
    else
        return n * (w + sp) - sp;
}

via_param via_info::get_via_param(vector box_dim, direction vdir, orient_2d ex_dir,
                                  orient_2d ex_adj_dir, bool extend) const {
    via_param ans;

    auto vidx = to_int(vdir);
    auto dir_idx = to_int(ex_dir);
    auto adj_dir_idx = to_int(ex_adj_dir);

    // get maximum possible number of vias
    vector min_sp = sp;
    for (const auto &arr : sp2_list) {
        min_sp[0] = std::min(min_sp[0], arr[0]);
        min_sp[1] = std::min(min_sp[1], arr[1]);
    }
    for (const auto &arr : sp3_list) {
        min_sp[0] = std::min(min_sp[0], arr[0]);
        min_sp[1] = std::min(min_sp[1], arr[1]);
    }
    auto nx_max = get_n_max(box_dim[0], cut_dim[0], min_sp[0]);
    auto ny_max = get_n_max(box_dim[1], cut_dim[1], min_sp[1]);

    // check if it's possible to palce a via
    if (nx_max == 0 || ny_max == 0)
        return ans;

    // use priority queue to find maximum number of vias
    util::unique_heap<via_cnt_t, boost::hash<via_cnt_t>> heap;
    heap.emplace(nx_max * ny_max, std::array<cnt_t, 2>{nx_max, ny_max});

    std::vector<vector> sp1_list({sp});

    while (!heap.empty()) {
        auto &[via_cnt, num_via] = heap.top();

        // get list of valid via spacing
        const std::vector<vector> *sp_vec_ptr;
        if (num_via[0] == 2 && num_via[1] == 2) {
            sp_vec_ptr = &sp2_list;
        } else if (num_via[0] > 1 && num_via[1] > 1) {
            sp_vec_ptr = &sp3_list;
        } else {
            sp_vec_ptr = &sp1_list;
        }

        // find optimal legal enclosure via
        vector opt_arr_dim;
        std::array<vector, 2> opt_mdim = {vector{MAX_INT, MAX_INT}, vector{MAX_INT, MAX_INT}};
        vector opt_sp = {-1, -1};
        for (const auto &sp_via : *sp_vec_ptr) {
            vector arr_dim = {get_arr_dim(num_via[0], cut_dim[0], sp_via[0]),
                              get_arr_dim(num_via[1], cut_dim[1], sp_via[1])};
            if (arr_dim[0] == 0 || arr_dim[1] == 0)
                continue;

            auto m_dim = get_metal_dim(box_dim, arr_dim, enc_list[0], dir_idx, extend);
            if (m_dim[0] == 0)
                continue;
            auto adj_m_dim = get_metal_dim(box_dim, arr_dim, enc_list[1], adj_dir_idx, extend);
            if (adj_m_dim[0] == 0)
                continue;

            if (m_dim[dir_idx] <= opt_mdim[vidx][dir_idx] &&
                adj_m_dim[adj_dir_idx] <= opt_mdim[1 - vidx][adj_dir_idx]) {
                opt_arr_dim = arr_dim;
                opt_mdim[vidx] = m_dim;
                opt_mdim[1 - vidx] = adj_m_dim;
                opt_sp = sp_via;
            }
        }

        if (opt_sp[0] < 0) {
            // fail to find legal via enclosure
            // continue searching
            if (nx_max > 1)
                heap.emplace((nx_max - 1) * ny_max, std::array<cnt_t, 2>{nx_max - 1, ny_max});
            if (ny_max > 1)
                heap.emplace(nx_max * (ny_max - 1), std::array<cnt_t, 2>{nx_max, ny_max - 1});
        } else {
            // solution found
            // set space to 0 if ony 1 via, this prevents space with MAX_INT values.
            if (num_via[0] == 1)
                opt_sp[0] = 0;
            if (num_via[1] == 1)
                opt_sp[1] = 0;
            auto enc1x = (opt_mdim[0][0] - opt_arr_dim[0]) / 2;
            auto enc1y = (opt_mdim[0][1] - opt_arr_dim[1]) / 2;
            auto enc2x = (opt_mdim[1][0] - opt_arr_dim[0]) / 2;
            auto enc2y = (opt_mdim[1][1] - opt_arr_dim[1]) / 2;

            ans = {num_via[0], num_via[1], cut_dim[0], cut_dim[1], opt_sp[0], opt_sp[1], enc1x,
                   enc1x,      enc1y,      enc1y,      enc2x,      enc2x,     enc2y,     enc2y};
            return ans;
        }

        heap.pop();
    }

    return ans;
}

} // namespace layout
} // namespace cbag


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "consensus/yac/supermajority_checker.hpp"

#include "consensus/yac/impl/supermajority_checker_bft.hpp"
#include "consensus/yac/impl/supermajority_checker_cft.hpp"

#include <gtest/gtest.h>

#include <boost/foreach.hpp>
#include "boost/tuple/tuple.hpp"
#include "logger/logger.hpp"

#include "framework/test_logger.hpp"
#include "module/shared_model/interface_mocks.hpp"

using namespace iroha::consensus::yac;

using ::testing::ReturnRefOfCopy;

static logger::LoggerPtr log_ = getTestLogger("YacCommon");

static const std::map<ConsistencyModel, unsigned int> kf1_param{
    {ConsistencyModel::kCft, detail::kSupermajorityCheckerKfPlus1Cft},
    {ConsistencyModel::kBft, detail::kSupermajorityCheckerKfPlus1Bft}};

class SupermajorityCheckerTest
    : public ::testing::TestWithParam<ConsistencyModel>,
      public SupermajorityChecker {
 public:
  void SetUp() override {}

  unsigned int getK() const {
    return kf1_param.at(GetParam());
  }

  size_t getAllowedFaultyPeers(size_t total_peers) const {
    return (total_peers - 1) / getK();
  }

  size_t getSupermajority(size_t total_peers) const {
    return total_peers - getAllowedFaultyPeers(total_peers);
  }

  size_t getTolerance(size_t total_peers) const {
    return getAllowedFaultyPeers(total_peers) + 1;
  }

  std::string modelToString() const {
    return "`" + std::to_string(getK()) + " * f + 1' "
        + (GetParam() == ConsistencyModel::kBft ? "BFT" : "CFT") + " model";
  }

  bool hasSupermajority(PeersNumberType current,
                        PeersNumberType all) const override {
    return checker->hasSupermajority(current, all);
  }

  bool isTolerated(PeersNumberType number, PeersNumberType all) const {
    return checker->isTolerated(number, all);
  }

  bool canHaveSupermajority(const VoteGroups &votes,
                            PeersNumberType all) const override {
    return checker->canHaveSupermajority(votes, all);
  }

  std::unique_ptr<SupermajorityChecker> checker{
      getSupermajorityChecker(GetParam())};
};

using CftAndBftSupermajorityCheckerTest = SupermajorityCheckerTest;
using BftSupermajorityCheckerTest = SupermajorityCheckerTest;

INSTANTIATE_TEST_SUITE_P(Instance,
                         CftAndBftSupermajorityCheckerTest,
                         ::testing::Values(ConsistencyModel::kCft,
                                           ConsistencyModel::kBft));

INSTANTIATE_TEST_SUITE_P(Instance,
                         BftSupermajorityCheckerTest,
                         ::testing::Values(ConsistencyModel::kBft));

/**
 * @given 2 participants
 * @when check range of voted participants
 * @then correct result
 */
TEST_P(CftAndBftSupermajorityCheckerTest, SuperMajorityCheckWithSize2) {
  log_->info("-----------| F(x, 2), x in [0..3] |-----------");

  size_t A = 2;  // number of all peers
  for (size_t i = 0; i < 4; ++i) {
    if (i >= getSupermajority(A)  // enough votes
        and i <= A                // not more than total peers amount
    ) {
      ASSERT_TRUE(hasSupermajority(i, A))
          << i << " votes out of " << A << " are supermajority in "
          << modelToString();
    } else {
      ASSERT_FALSE(hasSupermajority(i, A))
          << i << " votes out of " << A << " are not a supermajority in "
          << modelToString();
    }
  }
}

/**
 * @given 4 participants
 * @when check range of voted participants
 * @then correct result
 */
TEST_P(CftAndBftSupermajorityCheckerTest, SuperMajorityCheckWithSize4) {
  log_->info("-----------| F(x, 4), x in [0..5] |-----------");

  size_t A = 6;  // number of all peers
  for (size_t i = 0; i < 5; ++i) {
    if (i >= getSupermajority(A)  // enough votes
        and i <= A                // not more than total peers amount
    ) {
      ASSERT_TRUE(hasSupermajority(i, A))
          << i << " votes out of " << A << " are supermajority in "
          << modelToString();
    } else {
      ASSERT_FALSE(hasSupermajority(i, A))
          << i << " votes out of " << A << " are not a supermajority in "
          << modelToString();
    }
  }
}

/**
 * @given 4 participants
 * @when check range of voted participants
 * @then correct result
 */
TEST_P(CftAndBftSupermajorityCheckerTest, ToleranceWithSize4) {
  size_t A = 4;
  for (size_t i = 0; i < 5; ++i) {
    if (i >= getTolerance(A) and i <= A) {
      ASSERT_TRUE(isTolerated(i, A))
          << i << " votes out of " << A << " in " << modelToString();
    } else {
      ASSERT_FALSE(isTolerated(i, A))
          << i << " votes out of " << A << " in " << modelToString();
    }
  }
}

/**
 * \attention this test does not address possible supermajority on other peers
 * due to malicious peers sending then votes for other hashes
 *
 * @given some peers vote all for one option, others vote each for own option,
 * and the rest do not vote
 * @when different amounts of described peer kinds
 * @then correct decision on supermajority possibility
 */
TEST_P(CftAndBftSupermajorityCheckerTest, OneLargeAndManySingleVoteGroups) {
  /**
   * Make vote groups out of given votes amount such that the largest one has
   * the given amount of votes.
   */
  auto makeVoteGroups = [](size_t largest_group, size_t voted_peers) {
    BOOST_ASSERT_MSG(
        largest_group <= voted_peers,
        "A votes group cannot have more votes than the amount of voted peers!");
    const size_t num_groups = voted_peers - largest_group + 1;
    std::vector<size_t> vote_groups(num_groups, 1);
    vote_groups[0] = largest_group;
    return vote_groups;
  };

  struct Case {
    size_t V;  // Voted peers
    size_t A;  // All peers
  };

  for (const auto &c : std::vector<Case>{{6, 7}, {8, 12}}) {
    log_->info("--------| ProofOfReject(x, {0}, {1}), x in [1..{0}] |---------",
               c.V,
               c.A);

    size_t L;  // number of votes for the Leading option
    for (L = 1; L <= c.V; ++L) {
      const auto vote_groups = makeVoteGroups(L, c.V);
      const size_t N = c.A - c.V;              // not yet voted
      const size_t S = getSupermajority(c.A);  // supermajority
      const size_t Lp =  // max possible votes amount for the largest group
          L              // the votes we know
          + N;           // the peers that we have no votes from
      // Check if any peer on the network can get supermajority:
      if (Lp >= S) {
        EXPECT_TRUE(canHaveSupermajority(vote_groups, c.A))
            << "if " << N << " not yet voted peers "
            << "vote for option already having " << L << " votes, "
            << "it will reach supermajority in " << modelToString();
      } else {
        EXPECT_FALSE(canHaveSupermajority(vote_groups, c.A))
            << "even if all " << N << " not yet voted peers "
            << "vote for option already having " << L << " votes, "
            << "it will not reach supermajority in " << modelToString();
      }
    }
  }
}


#include "mp/co_moving_detection.hpp"

#include <iostream>
#include <dlib/svm.h>

#include "mp/feature_computation.hpp"
#include "mp/ground_truth.hpp"

namespace mp {

// Increment if any details in the layout of
// co_moving_classifier::impl change.
static const i32 VERSION = 3;

using sample_type        = dlib::matrix<double, 0, 1>; // 0 -> row size determined at runtime
using vector_normalizer  = dlib::vector_normalizer<sample_type>;
using kernel_type        = dlib::linear_kernel<sample_type>;
using trainer_type       = dlib::svm_c_linear_trainer<kernel_type>; // TODO: Decide on a final trainer type.
using decision_func_type = dlib::decision_function<kernel_type>;
using function_type      = dlib::normalized_function<decision_func_type>;

// Extracts a vector of samples (== feature vectors) and labels (== co-moving/not co-moving)
// from the input data.
static void get_learning_data(const similarity_data &data, const ground_truth &gt,
                              vector<sample_type> &samples, vector<double> &labels)
{
    samples.clear();
    labels.clear();

    samples.reserve(data.pairs.size() * data.duration);
    labels.reserve(data.pairs.size() * data.duration);

    sample_type sample;
    sample.set_size(data.feature_dimension, 1);
    // For each pair ...
    for (auto &pair : data.pairs) {
        const string &left_name = data.devices[pair.left];
        const string &right_name = data.devices[pair.right];

        // ... and each timestamp ...
        for (i64 ts = data.begin_timestamp; ts <= data.end_timestamp; ++ts) {
            // ... get the feature vector and its corresponding ground truth
            auto feature = data.feature_at(pair, ts);
            for (i32 i = 0; i < data.feature_dimension; ++i) {
                sample(i) = feature[i];
            }
            samples.push_back(sample);

            bool co_moving = gt.co_moving_at(ts, left_name, right_name);
            labels.push_back(co_moving ? 1.0 : -1.0);
        }
    }
}

// Trains the given normalizer using the provided samples
// and then normalizes the samples.
static void normalize(vector_normalizer &norm, vector<sample_type> &samples)
{
    norm.train(samples);
    for (auto &sample : samples) {
        sample = norm(sample);
    }
}

struct co_moving_classifier::impl
{
    function_type func;
    size_t        training_dimension;
    sample_type   sample;
};

void co_moving_classifier::print_cross_validation(const similarity_data &data, const ground_truth &gt, std::ostream &o)
{
    vector<sample_type> samples;
    vector<double> labels;
    get_learning_data(data, gt, samples, labels);
    dlib::randomize_samples(samples, labels);

    vector_normalizer norm;
    normalize(norm, samples);

    trainer_type trainer;
    o << "Cross validation results: " << std::endl;
    for (double C = 1; C < 100000; C *= 5) {
        trainer.set_c(C);

        // Two values:
        // - fraction of correctly identified +1 cases
        // - ________________________________ -1 cases
        o << "C = " << C
          << ", accuracy = " << dlib::cross_validate_trainer(trainer, samples, labels, 3);
        o << std::flush;
    }
}

bool co_moving_classifier::co_moving(array_view<const double> a)
{
    if (!m_impl) {
        return false;
    }

    // May only be used with an input dimension
    // that equals the training dimension.
    assert(a.size() == m_impl->training_dimension);

    // Sample is an instance of sample_type that is
    // cached by this instance of co_moving_classifier.
    // This saves us an allocation per classification.
    sample_type &sample = m_impl->sample;
    for (size_t i = 0; i < m_impl->training_dimension; ++i) {
        sample(i) = a[i];
    }
    return m_impl->func(sample) >= 0;
}

void co_moving_classifier::learn(const similarity_data &data, const ground_truth &gt)
{
    trainer_type trainer;
    trainer.set_c(1);

    vector<sample_type> samples;
    vector<double> labels;
    get_learning_data(data, gt, samples, labels);
    dlib::randomize_samples(samples, labels);

    vector_normalizer norm;
    normalize(norm, samples);

    std::unique_ptr<impl> ptr(new impl());
    ptr->training_dimension = data.feature_dimension;
    ptr->func.normalizer = norm; // other samples will be normalized, too.
    ptr->func.function = trainer.train(samples, labels);
    ptr->sample.set_size(ptr->training_dimension, 1);
    m_impl = std::move(ptr);
}

void co_moving_classifier::load(std::istream &i)
{
    i32 version;
    dlib::deserialize(version, i);
    if (version != VERSION) {
        throw std::runtime_error("Serialized classifier has incorrect version " + std::to_string(version)
                                 + ", version must be " + std::to_string(VERSION));
    }

    bool valid;
    dlib::deserialize(valid, i);
    if (valid) {
        std::unique_ptr<impl> ptr(new impl());
        dlib::deserialize(ptr->func, i);
        dlib::deserialize(ptr->training_dimension, i);
        ptr->sample.set_size(ptr->training_dimension, 1);
        m_impl = std::move(ptr);
    } else {
        m_impl.reset();
    }
}

void co_moving_classifier::save(std::ostream &o) const
{
    dlib::serialize(VERSION, o);
    bool valid = static_cast<bool>(m_impl);
    dlib::serialize(valid, o);

    if (valid) {
        dlib::serialize(m_impl->func, o);
        dlib::serialize(m_impl->training_dimension, o);
        // sample does not need to be stored,
        // because its value does not matter and its
        // size equals training_dimension.
    }
}

co_moving_classifier::co_moving_classifier()
    : m_impl()
{}

co_moving_classifier::~co_moving_classifier()
{}

co_moving_classifier::co_moving_classifier(co_moving_classifier &&other)
    : m_impl(std::move(other.m_impl))
{}

co_moving_classifier& co_moving_classifier::operator =(co_moving_classifier &&other)
{
    m_impl = std::move(other.m_impl);
    return *this;
}

} // namespace mp


/*
fitting.cpp

Copyright (c) 2014,2015 Terumasa Tadano

This file is distributed under the terms of the MIT license.
Please see the file 'LICENCE.txt' in the root directory
or http://opensource.org/licenses/mit-license.php for information.
*/

#include <iostream>
#include <fstream>
#include <cmath>
#include <string>
#include <vector>
#include <set>
#include <boost/lexical_cast.hpp>
#include "fitting.h"
#include "files.h"
#include "error.h"
#include "memory.h"
#include "symmetry.h"
#include "system.h"
#include "fcs.h"
#include "interaction.h"
#include "timer.h"
#include "combination.h"
#include "constants.h"
#include "constraint.h"
#include "mathfunctions.h"

#ifdef _USE_EIGEN
#include <Eigen/Dense>
#endif

#include <time.h>

#ifdef _VSL
#include "mkl_vsl.h"

#else
#include <cstdlib>
#endif

using namespace ALM_NS;

Fitting::Fitting(ALM *alm): Pointers(alm){
    seed = (unsigned int) time(NULL);
#ifdef _VSL
    brng = VSL_BRNG_MT19937;
    vslNewStream(&stream, brng, seed);
#else
    std::srand(seed);
#endif
}

Fitting::~Fitting() {
    if (alm->mode == "fitting") {
        memory->deallocate(params);
    }
}

void Fitting::fitmain()
{
    int i;
    int nat = system->nat;
    int natmin = symmetry->natmin;
    int ntran = symmetry->ntran;

    int ndata = system->ndata;
    int nstart = system->nstart;
    int nend = system->nend;
    int nskip = system->nskip;

    int N, M, N_new;
    int maxorder = interaction->maxorder;
    int P = constraint->P;

    int nmulti;
    int ndata_used = nend - nstart + 1;

    double **u, **f;
    double **amat, *fsum;
    double *fsum_orig;
    double *param_tmp;

    std::cout << " FITTING" << std::endl;
    std::cout << " =======" << std::endl << std::endl;

    std::cout << "  Reference files" << std::endl;
    std::cout << "   Displacement: " << files->file_disp << std::endl;
    std::cout << "   Force       : " << files->file_force << std::endl;
    std::cout << std::endl;

    std::cout << "  NSTART = " << nstart << "; NEND = " << nend << std::endl;
    std::cout << "  " << ndata_used << " entries will be used for fitting." << std::endl << std::endl;

    // Read displacement-force training data set from files

    data_multiplier(nat, ndata, nstart, nend, ndata_used, nmulti,
                    symmetry->multiply_data, u, f,
                    files->file_disp, files->file_force);

    N = 0;
    for (i = 0; i < maxorder; ++i) {
        N += fcs->ndup[i].size();
    }
    std::cout << "  Total Number of Parameters : " << N << std::endl << std::endl;

    // Calculate matrix elements for fitting

    M = 3 * natmin * ndata_used * nmulti;

    if (constraint->constraint_algebraic) {

        N_new = 0;
        for (i = 0; i < maxorder; ++i) {
            N_new += constraint->index_bimap[i].size();
        }
        std::cout << "  Total Number of Free Parameters : " << N_new << std::endl << std::endl;

        memory->allocate(amat, M, N_new);
        memory->allocate(fsum, M);
        memory->allocate(fsum_orig, M);

        calc_matrix_elements_algebraic_constraint(M, N, N_new, nat, natmin, ndata_used,
            nmulti, maxorder, u, f, amat, fsum, fsum_orig);

    } else {

        memory->allocate(amat, M, N);
        memory->allocate(fsum, M);

        calc_matrix_elements(M, N, nat, natmin, ndata_used, nmulti, maxorder, u, f, amat, fsum);
    }

    memory->deallocate(u);
    memory->deallocate(f);

    // Execute fitting

    memory->allocate(param_tmp, N);

    if (nskip == 0) {

        // Fitting with singular value decomposition or QR-Decomposition

        if (constraint->constraint_algebraic) {
            fit_algebraic_constraints(N_new, M, amat, fsum, param_tmp,
                                      fsum_orig, maxorder);

        } else if (constraint->exist_constraint) {
            fit_with_constraints(N, M, P, amat, fsum, param_tmp,
                                 constraint->const_mat, constraint->const_rhs);
        } else {
            fit_without_constraints(N, M, amat, fsum, param_tmp);
        }

    } else if (nskip > 0) {

        // Execute fittings consecutively with different input data.

        if (constraint->exist_constraint) {
            fit_consecutively(N, P, natmin, ndata_used, nmulti, nskip, amat, fsum,
                              constraint->const_mat, constraint->const_rhs);
        } else {
            error->exit("fitmain", "nskip has to be 0 when constraint_mode = 0");
        }
    } else {

        // Execute bootstrap simulation for estimating deviations of parameters.

        if (constraint->exist_constraint) {
            fit_bootstrap(N, P, natmin, ndata_used, nmulti, amat, fsum,
                          constraint->const_mat, constraint->const_rhs);
            fit_with_constraints(N, M, P, amat, fsum, param_tmp,
                                 constraint->const_mat, constraint->const_rhs);
        } else {
            error->exit("fitmain", "bootstrap analysis for LSE without constraint is not supported yet");
        }
    }

    // Copy force constants to public variable "params"

    memory->allocate(params, N);

    if (constraint->constraint_algebraic) {

        for (i = 0; i < N; ++i) {
            params[i] = param_tmp[i];
        }
        memory->deallocate(fsum_orig);

    } else {

        for (i = 0; i < N; ++i) params[i] = param_tmp[i];

    }

    memory->deallocate(amat);
    memory->deallocate(fsum);
    memory->deallocate(param_tmp);

    std::cout << std::endl;
    timer->print_elapsed();
    std::cout << " --------------------------------------------------------------" << std::endl;
    std::cout << std::endl;

}

void Fitting::data_multiplier(const int nat, const int ndata, const int nstart, const int nend,
                              const int ndata_used, int &nmulti, const int multiply_data, double **&u, double **&f,
                              const std::string file_disp, const std::string file_force)
{
    int i, j, k;
    int idata, itran, isym;
    int n_mapped;
    double u_rot[3], f_rot[3];
    double u_in, f_in;
    double *u_tmp, *f_tmp;
    std::vector<int> vec_data;
    unsigned int nline_f, nline_u;
    unsigned int nreq;

    std::ifstream ifs_disp, ifs_force;

    ifs_disp.open(file_disp.c_str(), std::ios::in);
    if (!ifs_disp) error->exit("openfiles", "cannot open disp file");
    ifs_force.open(file_force.c_str(), std::ios::in);
    if (!ifs_force) error->exit("openfiles", "cannot open force file");

    nreq = 3 * nat * ndata;

    memory->allocate(u_tmp, nreq);
    memory->allocate(f_tmp, nreq);

    // Read displacements from DFILE

    nline_u = 0;
    while (ifs_disp >> u_in) {
        u_tmp[nline_u++] = u_in;
        if (nline_u == nreq) break;
    }
    if (nline_u < nreq) error->exit("data_multiplier",
        "The number of lines in DFILE is too small for the given NDATA = ", ndata);

    // Read forces from FFILE

    nline_f = 0;
    while (ifs_force >> f_in) {
        f_tmp[nline_f++] = f_in;
        if (nline_f == nreq) break;
    }
    if (nline_f < nreq) error->exit("data_multiplier",
        "The number of lines in FFILE is too small for the given NDATA = ", ndata);

    // Multiply data

    if (multiply_data == 0) {

        std::cout << " MULTDAT = 0: Given displacement-force data sets will be used as is." << std::endl << std::endl;

        nmulti = 1;

        memory->allocate(u, ndata_used * nmulti, 3 * nat);
        memory->allocate(f, ndata_used * nmulti, 3 * nat);

        idata = 0;

        for (i = 0; i < ndata; ++i) {
            if (i < nstart - 1) continue;
            if (i > nend - 1) break;

            for (j = 0; j < nat; ++j) {
                for (k = 0; k < 3; ++k) {
                    u[idata][3 * j + k] = u_tmp[3*nat*i + 3*j + k];
                    f[idata][3 * j + k] = f_tmp[3*nat*i + 3*j + k];
                }
            }
            ++idata;
        }

    } else if (multiply_data == 1) {

        std::cout << "  MULTDAT = 1: Generate symmetrically equivalent displacement-force data sets " << std::endl;
        std::cout << "               by using pure translational operations only." << std::endl << std::endl;

        nmulti = symmetry->ntran;

        memory->allocate(u, ndata_used * nmulti, 3 * nat);
        memory->allocate(f, ndata_used * nmulti, 3 * nat);

        idata = 0;

        for (i = 0; i < ndata; ++i) {
            if (i < nstart - 1) continue;
            if (i > nend - 1) break;

            for (itran = 0; itran < symmetry->ntran; ++itran) {
                for (j = 0; j < nat; ++j) {
                    n_mapped = symmetry->map_sym[j][symmetry->symnum_tran[itran]];

                    for (k = 0; k < 3; ++k) {
                        u[idata][3 * n_mapped + k] = u_tmp[3*nat*i + 3*j + k];
                        f[idata][3 * n_mapped + k] = f_tmp[3*nat*i + 3*j + k];
                    }
                }
                ++idata;
            }
        }

    } else if (multiply_data == 2) {

        std::cout << "  MULTDAT = 2: Generate symmetrically equivalent displacement-force data sets." << std::endl;
        std::cout << "               (including rotational part) " << std::endl << std::endl;

        nmulti = symmetry->nsym;

        memory->allocate(u, ndata_used * nmulti, 3 * nat);
        memory->allocate(f, ndata_used * nmulti, 3 * nat);

        idata = 0;

        for (i = 0; i < ndata; ++i) {
            if (i < nstart - 1) continue;
            if (i > nend - 1) break;

#pragma omp parallel for private(j, n_mapped, k, u_rot, f_rot)
            for (isym = 0; isym < symmetry->nsym; ++isym) {
                for (j = 0; j < nat; ++j) {
                    n_mapped = symmetry->map_sym[j][isym];

                    for (k = 0; k < 3; ++k) {
                        u_rot[k] = u_tmp[3*nat*i + 3*j + k];
                        f_rot[k] = f_tmp[3*nat*i + 3*j + k];
                    }

                    rotvec(u_rot, u_rot, symmetry->symrel[isym]);
                    rotvec(f_rot, f_rot, symmetry->symrel[isym]);

                    for (k = 0; k < 3; ++k) {
                        u[nmulti * idata + isym][3 * n_mapped + k] = u_rot[k];
                        f[nmulti * idata + isym][3 * n_mapped + k] = f_rot[k];
                    }
                }
            }
            ++idata;
        }

    } else {
        error->exit("data_multiplier", "Unsupported MULTDAT");
    }

    memory->deallocate(u_tmp);
    memory->deallocate(f_tmp);

    ifs_disp.close();
    ifs_force.close();
}

void Fitting::fit_without_constraints(int N, int M, double **amat, double *bvec, double *param_out)
{
    int i, j;
    unsigned long k;
    int nrhs = 1, nrank, INFO, LWORK;
    int LMIN, LMAX;
    double rcond = -1.0;
    double f_square = 0.0;
    double *WORK, *S, *amat_mod, *fsum2;

    std::cout << "  Entering fitting routine: SVD without constraints" << std::endl;

    LMIN = std::min<int>(M, N);
    LMAX = std::max<int>(M, N);

    LWORK = 3*LMIN + std::max<int>(2*LMIN, LMAX);
    LWORK = 2 * LWORK;

    memory->allocate(WORK, LWORK);
    memory->allocate(S, LMIN);

    // transpose matrix A
    memory->allocate(amat_mod, M * N);
    memory->allocate(fsum2, LMAX);

    k = 0;
    for (j = 0; j < N; ++j) {
        for (i = 0; i < M; ++i) {
            amat_mod[k++] = amat[i][j];
        }
    }
    for (i = 0; i < M; ++i) {
        fsum2[i] = bvec[i];
        f_square += std::pow(bvec[i], 2);
    }
    for (i = M; i < LMAX; ++i) fsum2[i] = 0.0;

    std::cout << "  SVD has started ... ";

    // Fitting with singular value decomposition
    dgelss_(&M, &N, &nrhs, amat_mod, &M, fsum2, &LMAX, S, &rcond, &nrank, WORK, &LWORK, &INFO);

    std::cout << "finished !" << std::endl << std::endl;

    std::cout << "  RANK of the matrix = " << nrank << std::endl;
    if (nrank < N) error->warn("fit_without_constraints",
        "Matrix is rank-deficient. Force constants could not be determined uniquely :(");

    if (nrank == N) {
        double f_residual = 0.0;
        for (i = N; i < M; ++i) {
            f_residual += std::pow(fsum2[i], 2);
        }
        std::cout << std::endl << "  Residual sum of squares for the solution: " << sqrt(f_residual) << std::endl;
        std::cout << "  Fitting error (%) : "<< sqrt(f_residual/f_square) * 100.0 << std::endl;
    }

    for (i = 0; i < N; ++i) {
        param_out[i] = fsum2[i];
    }

    memory->deallocate(WORK);
    memory->deallocate(S);
    memory->deallocate(fsum2);
    memory->deallocate(amat_mod);
}

void Fitting::fit_with_constraints(int N, int M, int P,
                                   double **amat, double *bvec, double *param_out,
                                   double **cmat, double *dvec)
{
    int i, j;
    unsigned long k;
    int nrank;
    double f_square, f_residual;
    double *fsum2;

    std::cout << "  Entering fitting routine: QRD with constraints" << std::endl;

    memory->allocate(fsum2, M);

#ifdef _USE_EIGEN

    double **mat_tmp2;
    memory->allocate(mat_tmp2, M + P, N);
    for (i = 0; i < M; ++i) {
        for (j = 0; j < N; ++j) {
            mat_tmp2[i][j] = amat[i][j];
        }
    }
    for (i = 0; i < P; ++i) {
        for (j = 0; j < N; ++j) {
            mat_tmp2[M + i][j] = cmat[i][j];
        }
    }

    nrank = getRankEigen(M+P, N, mat_tmp2);
    memory->deallocate(mat_tmp2);

#else

    double *mat_tmp;

    memory->allocate(mat_tmp, (M + P) * N);

    k = 0;

    for (j = 0; j < N; ++j) {
        for (i = 0; i < M; ++i) {
            mat_tmp[k++] = amat[i][j];
        }
        for (i = 0; i < P; ++i) {
            mat_tmp[k++] = cmat[i][j];
        }
    }

    nrank = rankQRD((M+P), N, mat_tmp, eps12);
    memory->deallocate(mat_tmp);

#endif

    if (nrank != N) {
        std::cout << std::endl;
        std::cout << " **************************************************************************" << std::endl;
        std::cout << "  WARNING : rank deficient.                                                " << std::endl;
        std::cout << "  rank ( (A) ) ! = N            A: Fitting matrix     B: Constraint matrix " << std::endl;
        std::cout << "       ( (B) )                  N: The number of parameters                " << std::endl;
        std::cout << "  rank = " << nrank << " N = " << N << std::endl << std::endl;
        std::cout << "  This can cause a difficulty in solving the fitting problem properly      " << std::endl;
        std::cout << "  with DGGLSE, especially when the difference is large. Please check if    " << std::endl;
        std::cout << "  you obtain reliable force constants in the .fcs file.                    " << std::endl << std::endl;
        std::cout << "  This issue may be resolved by setting MULTDAT = 2 in the &fitting field. " << std::endl;
        std::cout << "  If not, you may need to reduce the cutoff radii and/or increase NDATA    " << std::endl;
        std::cout << "  by giving linearly-independent displacement patterns.                    " << std::endl;
        std::cout << " **************************************************************************" << std::endl;
        std::cout << std::endl;
    }

    f_square = 0.0;
    for (i = 0; i < M; ++i) {
        fsum2[i] = bvec[i];
        f_square += std::pow(bvec[i], 2);
    }
    std::cout << "  QR-Decomposition has started ...";

    double *amat_mod, *cmat_mod;
    memory->allocate(amat_mod, M * N);
    memory->allocate(cmat_mod, P * N);

    // transpose matrix A and C
    k = 0;
    for (j = 0; j < N; ++j) {
        for (i = 0; i < M; ++i) {
            amat_mod[k++] = amat[i][j];
        }
    }
    k = 0;
    for (j = 0; j < N; ++j) {
        for (i = 0; i < P; ++i) {
            cmat_mod[k++] = cmat[i][j];
        }
    }

    // Fitting

    int LWORK = P + std::min<int>(M, N) + 10 * std::max<int>(M, N);
    int INFO;
    double *WORK, *x;
    memory->allocate(WORK, LWORK);
    memory->allocate(x, N);

    dgglse_(&M, &N, &P, amat_mod, &M, cmat_mod, &P, fsum2, dvec, x, WORK, &LWORK, &INFO);

    std::cout << " finished. " << std::endl;

    f_residual = 0.0;
    for (i = N - P; i < M; ++i) {
        f_residual += std::pow(fsum2[i], 2);
    }
    std::cout << std::endl << "  Residual sum of squares for the solution: " << sqrt(f_residual) << std::endl;
    std::cout << "  Fitting error (%) : "<< std::sqrt(f_residual/f_square) * 100.0 << std::endl;

    // copy fcs to bvec

    for (i = 0; i < N; ++i) {
        param_out[i] = x[i];
    }

    memory->deallocate(amat_mod);
    memory->deallocate(cmat_mod);
    memory->deallocate(WORK);
    memory->deallocate(x);
    memory->deallocate(fsum2);
}

void Fitting::fit_algebraic_constraints(int N, int M, double **amat, double *bvec,
                                        double *param_out, double *bvec_orig,
                                        const int maxorder)
{
    int i, j;
    unsigned long k;
    int nrhs = 1, nrank, INFO, LWORK;
    int LMIN, LMAX;
    double rcond = -1.0;
    double f_square = 0.0;
    double *WORK, *S, *amat_mod, *fsum2;

    std::cout << "  Entering fitting routine: SVD with constraints considered algebraically." << std::endl;

    LMIN = std::min<int>(M, N);
    LMAX = std::max<int>(M, N);

    LWORK = 3*LMIN + std::max<int>(2*LMIN, LMAX);
    LWORK = 2 * LWORK;

    memory->allocate(WORK, LWORK);
    memory->allocate(S, LMIN);

    // transpose matrix A
    memory->allocate(amat_mod, M * N);
    memory->allocate(fsum2, LMAX);

    k = 0;
    for (j = 0; j < N; ++j) {
        for (i = 0; i < M; ++i) {
            amat_mod[k++] = amat[i][j];
        }
    }
    for (i = 0; i < M; ++i) {
        fsum2[i] = bvec[i];
        f_square += std::pow(bvec_orig[i], 2);
    }
    for (i = M; i < LMAX; ++i) fsum2[i] = 0.0;

    std::cout << "  SVD has started ... ";

    // Fitting with singular value decomposition
    dgelss_(&M, &N, &nrhs, amat_mod, &M, fsum2, &LMAX, S, &rcond, &nrank, WORK, &LWORK, &INFO);

    std::cout << "finished !" << std::endl << std::endl;

    std::cout << "  RANK of the matrix = " << nrank << std::endl;
    if (nrank < N) error->warn("fit_without_constraints",
        "Matrix is rank-deficient. Force constants could not be determined uniquely :(");

    if (nrank == N) {
        double f_residual = 0.0;
        for (i = N; i < M; ++i) {
            f_residual += std::pow(fsum2[i], 2);
        }
        std::cout << std::endl << "  Residual sum of squares for the solution: " << sqrt(f_residual) << std::endl;
        std::cout << "  Fitting error (%) : "<< sqrt(f_residual/f_square) * 100.0 << std::endl;
    }

    int ishift = 0;
    int iparam = 0;
    double tmp;
    int inew, iold;

    for (i = 0; i < maxorder; ++i) {
        for (j = 0; j < constraint->const_fix[i].size(); ++j) {
            param_out[constraint->const_fix[i][j].p_index_target + ishift] = constraint->const_fix[i][j].val_to_fix;
        }

        for (boost::bimap<int, int>::const_iterator it = constraint->index_bimap[i].begin();
            it != constraint->index_bimap[i].end(); ++it) {
                inew = (*it).left + iparam;
                iold = (*it).right + ishift;

                param_out[iold] = fsum2[inew];
        }

        for (j = 0; j < constraint->const_relate[i].size(); ++j) {
            tmp = 0.0;

            for (k = 0; k < constraint->const_relate[i][j].alpha.size(); ++k) {
                tmp += constraint->const_relate[i][j].alpha[k] * param_out[constraint->const_relate[i][j].p_index_orig[k] + ishift];
            }
            param_out[constraint->const_relate[i][j].p_index_target + ishift] = -tmp;
        }

        ishift += fcs->ndup[i].size();
        iparam += constraint->index_bimap[i].size();
    }

    memory->deallocate(WORK);
    memory->deallocate(S);
    memory->deallocate(fsum2);
    memory->deallocate(amat_mod);
}

void Fitting::fit_bootstrap(int N, int P, int natmin, int ndata_used, int nmulti,
                            double **amat, double *bvec, double **cmat, double *dvec)
{
    int i, j;
    unsigned long k, l;
    int M_Start, M_End;
    int mset;
    unsigned int iboot;
    int M;

    mset = 3 * natmin * nmulti;

    M_Start = 0;
    M_End = mset * ndata_used;

    M = M_End - M_Start;

    std::string file_fcs_bootstrap;
    file_fcs_bootstrap = files->job_title + ".fcs_bootstrap";

    std::ofstream ofs_fcs_boot;
    ofs_fcs_boot.open(file_fcs_bootstrap.c_str(), std::ios::out);
    if(!ofs_fcs_boot) error->exit("fit_bootstrap", "cannot open file_fcs_bootstrap");

    ofs_fcs_boot.setf(std::ios::scientific);

    double f_residual, f_square;
    double *fsum2;
    int INFO;
    double *WORK, *x;
    double *amat_mod, *cmat_mod;
    double *const_tmp;

    int *rnd_index;
    int iloc;

    memory->allocate(x, N);
    memory->allocate(cmat_mod, P * N);
    memory->allocate(const_tmp, P);

    std::cout << "  NSKIP < 0: Bootstrap analysis for error estimation." << std::endl;
    std::cout << "             The number of trials is NBOOT (=" << nboot << ")" << std::endl;
    std::cout << std::endl;
    std::cout << "  Relative errors and FCs are stored in file: " << file_fcs_bootstrap << std::endl;

    ofs_fcs_boot << "# Relative Error(%), FCs ..." ;

    for (i = 0; i < interaction->maxorder; ++i) {
        ofs_fcs_boot << std::setw(10) << fcs->ndup[i].size();
    }
    ofs_fcs_boot << std::endl;

    memory->allocate(fsum2, M);
    memory->allocate(amat_mod, N * M);
    memory->allocate(rnd_index, ndata_used);

    int LWORK = P + std::min<int>(M, N) + 100 * std::max<int>(M, N);
    memory->allocate(WORK, LWORK);

    for (iboot = 0; iboot < nboot; ++iboot) {
#ifdef _VSL
        // Use Intel MKL VSL if available
        viRngUniform(VSL_METHOD_IUNIFORM_STD, stream, ndata_used, rnd_index, 0, ndata_used);
#else
        for (i = 0; i < ndata_used; ++i) {
            rnd_index[i] = std::rand() % ndata_used; // random number uniformly distributed in [0, ndata_used)
        }
#endif

        f_square = 0.0;
        k = 0;
        for (i = 0; i < ndata_used; ++i) {
            iloc = rnd_index[i];
            for (j = iloc * mset; j < (iloc + 1) * mset; ++j) {
                fsum2[k++] = bvec[j];
                f_square += std::pow(bvec[j], 2);
            }
        }
        l = 0;
        for (j = 0; j < N; ++j) {
            for (i = 0; i < ndata_used; ++i) {
                iloc = rnd_index[i];
                for (k = iloc * mset; k < (iloc + 1) * mset; ++k) {
                    amat_mod[l++] = amat[k][j];
                }
            }
        }

        k = 0;
        for (j = 0; j < N; ++j) {
            for (i = 0; i < P; ++i) {
                cmat_mod[k++] = cmat[i][j];
            }
        }

        for (i = 0; i < P; ++i) {
            const_tmp[i] = dvec[i];
        }

        dgglse_(&M, &N, &P, amat_mod, &M, cmat_mod, &P, fsum2, const_tmp, x, WORK, &LWORK, &INFO);

        f_residual = 0.0;
        for (i = N - P; i < M; ++i) {
            f_residual += std::pow(fsum2[i], 2);
        }

        ofs_fcs_boot << 100.0 * std::sqrt(f_residual/f_square);

        for (i = 0; i < N; ++i) {
            ofs_fcs_boot << std::setw(15) << x[i];
        }
        ofs_fcs_boot << std::endl;
    }
    ofs_fcs_boot.close();

    memory->deallocate(x);
    memory->deallocate(fsum2);
    memory->deallocate(cmat_mod);
    memory->deallocate(const_tmp);
    memory->deallocate(amat_mod);
    memory->deallocate(rnd_index);
    memory->deallocate(WORK);

    std::cout << "  Bootstrap analysis finished." << std::endl;
    std::cout << "  Normal fitting will be performed" << std::endl;
}

void Fitting::fit_consecutively(int N, int P, const int natmin, const int ndata_used, const int nmulti, const int nskip,
                                double **amat, double *bvec, double **cmat, double *dvec)
{
    int i, j;
    unsigned long k;
    int iend;
    int M_Start, M_End;
    int mset;
    int M;

    mset = 3 * natmin * nmulti;

    M_Start = 0;

    std::string file_fcs_sequence;
    file_fcs_sequence = files->job_title + ".fcs_sequence";

    std::ofstream ofs_fcs_seq;
    ofs_fcs_seq.open(file_fcs_sequence.c_str(), std::ios::out);
    if(!ofs_fcs_seq) error->exit("fit_consecutively", "cannot open file_fcs_sequence");

    ofs_fcs_seq.setf(std::ios::scientific);

    double f_residual, f_square;
    double *fsum2;
    int INFO;
    double *WORK, *x;
    double *amat_mod, *cmat_mod;
    double *const_tmp;

    memory->allocate(x, N);
    memory->allocate(cmat_mod, P * N);
    memory->allocate(const_tmp, P);

    std::cout << "  NSKIP > 0: Fitting will be performed consecutively" << std::endl;
    std::cout << "             with variously changing NEND as NEND = NSTART + i*NSKIP" << std::endl;
    std::cout << std::endl;
    std::cout << "  Relative errors and FCs will be stored in the file " << file_fcs_sequence << std::endl;

    ofs_fcs_seq << "# Relative Error(%), FCS..." ;

    for (i = 0; i < interaction->maxorder; ++i) {
        ofs_fcs_seq << std::setw(10) << fcs->ndup[i].size();
    }
    ofs_fcs_seq << std::endl;

    for (iend = 1; iend <= ndata_used; iend += nskip) {

        M_End = mset * iend;
        M = M_End - M_Start;

        memory->allocate(fsum2, M);

        f_square = 0.0;
        j = 0;
        for (i = M_Start; i < M_End; ++i) {
            fsum2[j++] = bvec[i];
            f_square += std::pow(bvec[i], 2);
        }

        memory->allocate(amat_mod, M * N);

        // Transpose matrix A
        k = 0;
        for (j = 0; j < N; ++j) {
            for (i = M_Start; i < M_End; ++i) {
                amat_mod[k++] = amat[i][j];
            }
        }

        k = 0;
        for (j = 0; j < N; ++j) {
            for (i = 0; i < P; ++i) {
                cmat_mod[k++] = cmat[i][j];
            }
        }

        for (i = 0; i < P; ++i) {
            const_tmp[i] = dvec[i];
        }

        // Fitting

        int LWORK = P + std::min<int>(M, N) + 100 * std::max<int>(M, N);
        memory->allocate(WORK, LWORK);

        dgglse_(&M, &N, &P, amat_mod, &M, cmat_mod, &P, fsum2, const_tmp, x, WORK, &LWORK, &INFO);

        memory->deallocate(amat_mod);
        memory->deallocate(WORK);

        f_residual = 0.0;
        for (i = N - P; i < M; ++i) {
            f_residual += std::pow(fsum2[i], 2);
        }

        ofs_fcs_seq << 100.0 * std::sqrt(f_residual/f_square);

        for (i = 0; i < N; ++i) {
            ofs_fcs_seq << std::setw(15) << x[i];
        }
        ofs_fcs_seq << std::endl;
        memory->deallocate(fsum2);
    }

    for (i = 0; i < N; ++i) {
        bvec[i] = x[i];
    }

    memory->deallocate(cmat_mod);
    memory->deallocate(const_tmp);
    memory->deallocate(x);

    ofs_fcs_seq.close();

    std::cout << "  Consecutive fitting finished." << std::endl;
}

void Fitting::calc_matrix_elements(const int M, const int N, const int nat, const int natmin,
                                   const int ndata_fit, const int nmulti, const int maxorder,
                                   double **u, double **f, double **amat, double *bvec)
{
    int i, j;
    int irow;
    int ncycle;

    std::cout << "  Calculation of matrix elements for direct fitting started ... ";
    for (i = 0; i < M; ++i) {
        for (j = 0; j < N; ++j) {
            amat[i][j] = 0.0;
        }
        bvec[i] = 0.0;
    }

    ncycle = ndata_fit * nmulti;

#ifdef _OPENMP
#pragma omp parallel private(irow, i, j)
#endif
    {
        int *ind;
        int mm, order, iat, k;
        int im, idata, iparam;
        double amat_tmp;

        memory->allocate(ind, maxorder + 1);

#ifdef _OPENMP
#pragma omp for schedule(guided)
#endif
        for (irow = 0; irow < ncycle; ++irow) {

            // generate r.h.s vector B
            for (i = 0; i < natmin; ++i) {
                iat = symmetry->map_p2s[i][0];
                for (j = 0; j < 3; ++j) {
                    im = 3 * i + j + 3 * natmin * irow;
                    bvec[im] = f[irow][3 * iat + j];
                }
            }

            // generate l.h.s. matrix A

            idata = 3 * natmin * irow;
            iparam = 0;

            for (order = 0; order < maxorder; ++order) {

                mm = 0;

                for (std::vector<int>::iterator iter = fcs->ndup[order].begin(); iter != fcs->ndup[order].end(); ++iter) {
                    for (i = 0; i < *iter; ++i) {
                        ind[0] = fcs->fc_set[order][mm].elems[0];
                        k = idata + inprim_index(fcs->fc_set[order][mm].elems[0]);
                        amat_tmp = 1.0;
                        for (j = 1; j < order + 2; ++j) {
                            ind[j] = fcs->fc_set[order][mm].elems[j];
                            amat_tmp *= u[irow][fcs->fc_set[order][mm].elems[j]];
                        }
                        amat[k][iparam] -= gamma(order + 2, ind) * fcs->fc_set[order][mm].coef * amat_tmp;
                        ++mm;
                    }
                    ++iparam;
                }
            }
        }

        memory->deallocate(ind);

    }

    std::cout << "done!" << std::endl << std::endl;
}

void Fitting::calc_matrix_elements_algebraic_constraint(const int M, const int N, const int N_new, const int nat,
                                                        const int natmin, const int ndata_fit, const int nmulti,
                                                        const int maxorder, double **u, double **f, double **amat,
                                                        double *bvec, double *bvec_orig)
{
    int i, j;
    int irow;
    int ncycle;

    std::cout << "  Calculation of matrix elements for direct fitting started ... ";

    ncycle = ndata_fit * nmulti;

#ifdef _OPENMP
#pragma omp parallel for private(j)
#endif
    for (i = 0; i < M; ++i) {
        for (j = 0; j < N_new; ++j) {
            amat[i][j] = 0.0;
        }
        bvec[i] = 0.0;
        bvec_orig[i] = 0.0;
    }

#ifdef _OPENMP
#pragma omp parallel private(irow, i, j)
#endif
    {
        int *ind;
        int mm, order, iat, k;
        int im, idata, iparam;
        int ishift;
        int iold, inew;
        double amat_tmp;
        double **amat_orig;
        double **amat_mod;

        memory->allocate(ind, maxorder + 1);
        memory->allocate(amat_orig, 3 * natmin, N);
        memory->allocate(amat_mod, 3 * natmin, N_new);

#ifdef _OPENMP
#pragma omp for schedule(guided)
#endif
        for (irow = 0; irow < ncycle; ++irow) {

            // generate r.h.s vector B
            for (i = 0; i < natmin; ++i) {
                iat = symmetry->map_p2s[i][0];
                for (j = 0; j < 3; ++j) {
                    im = 3 * i + j + 3 * natmin * irow;
                    bvec[im] = f[irow][3 * iat + j];
                    bvec_orig[im] = f[irow][3 * iat + j];
                }
            }

            for (i = 0; i < 3 * natmin; ++i) {
                for (j = 0; j < N; ++j) {
                    amat_orig[i][j] = 0.0;
                }
                for (j = 0; j < N_new; ++j) {
                    amat_mod[i][j] = 0.0;
                }
            }

            // generate l.h.s. matrix A

            idata = 3 * natmin * irow;
            iparam = 0;

            for (order = 0; order < maxorder; ++order) {

                mm = 0;

                for (std::vector<int>::iterator iter = fcs->ndup[order].begin(); iter != fcs->ndup[order].end(); ++iter) {
                    for (i = 0; i < *iter; ++i) {
                        ind[0] = fcs->fc_set[order][mm].elems[0];
                  //      k = idata + inprim_index(fcs->fc_set[order][mm].elems[0]);
                        k = inprim_index(ind[0]);

                        amat_tmp = 1.0;
                        for (j = 1; j < order + 2; ++j) {
                            ind[j] = fcs->fc_set[order][mm].elems[j];
                            amat_tmp *= u[irow][fcs->fc_set[order][mm].elems[j]];
                        }
                        amat_orig[k][iparam] -= gamma(order + 2, ind) * fcs->fc_set[order][mm].coef * amat_tmp;
                        ++mm;
                    }
                    ++iparam;
                }
            }

            ishift = 0;
            iparam = 0;

            for (order = 0; order < maxorder; ++order) {

                for (i = 0; i < constraint->const_fix[order].size(); ++i) {

                    for (j = 0; j < 3 * natmin; ++j) {
                        bvec[j + idata] -=  constraint->const_fix[order][i].val_to_fix
                                         * amat_orig[j][ishift + constraint->const_fix[order][i].p_index_target];
                    }
                }

                for (boost::bimap<int, int>::const_iterator it = constraint->index_bimap[order].begin();
                    it != constraint->index_bimap[order].end(); ++it) {
                        inew = (*it).left + iparam;
                        iold = (*it).right + ishift;

                    for (j = 0; j < 3 * natmin; ++j) {
                            amat_mod[j][inew] = amat_orig[j][iold];
                   }
                }

                for (i = 0; i < constraint->const_relate[order].size(); ++i) {

                    iold = constraint->const_relate[order][i].p_index_target + ishift;

                    for (j = 0; j < constraint->const_relate[order][i].alpha.size(); ++j) {

                        inew = constraint->index_bimap[order].right.at(constraint->const_relate[order][i].p_index_orig[j]) + iparam;
                        for (k = 0; k < 3 * natmin; ++k) {
                            amat_mod[k][inew] -= amat_orig[k][iold] * constraint->const_relate[order][i].alpha[j];
                        }
                    }
                }

                ishift += fcs->ndup[order].size();
                iparam += constraint->index_bimap[order].size();
            }

            for (i = 0; i < 3 * natmin; ++i) {
                for (j = 0; j < N_new; ++j) {
                    amat[i + idata][j] = amat_mod[i][j];
                }
            }

        }

        memory->deallocate(ind);
        memory->deallocate(amat_orig);
        memory->deallocate(amat_mod);
    }

    std::cout << "done!" << std::endl << std::endl;
}

int Fitting::inprim_index(const int n)
{
    int in;
    int atmn = n / 3;
    int crdn = n % 3;

    for (int i = 0; i < symmetry->natmin; ++i) {
        if (symmetry->map_p2s[i][0] == atmn) {
            in = 3 * i + crdn;
            break;
        }
    }
    return in;
}

double Fitting::gamma(const int n, const int *arr)
{
    int *arr_tmp, *nsame;
    int i;
    int ind_front, nsame_to_front;

    memory->allocate(arr_tmp, n);
    memory->allocate(nsame, n);

    for (i = 0; i < n; ++i) {
        arr_tmp[i] = arr[i];
        nsame[i] = 0;
    }

    ind_front = arr[0];
    nsame_to_front = 1;

    interaction->insort(n, arr_tmp);

    int nuniq = 1;
    int iuniq = 0;

    nsame[0] = 1;

    for (i = 1; i < n; ++i) {
        if (arr_tmp[i] == arr_tmp[i-1]) {
            ++nsame[iuniq];
        } else {
            ++nsame[++iuniq];
            ++nuniq;
        }

        if (arr[i] == ind_front) ++nsame_to_front;
    }

    int denom = 1;

    for (i = 0; i < nuniq; ++i) {
        denom *= factorial(nsame[i]);
    }

    memory->deallocate(arr_tmp);
    memory->deallocate(nsame);

    return static_cast<double>(nsame_to_front) / static_cast<double>(denom);
}

int Fitting::factorial(const int n)
{
    if (n == 1 || n == 0) {
        return 1;
    } else {
        return n * factorial(n - 1);
    }
}

#ifdef _USE_EIGEN
int Fitting::getRankEigen(const int m, const int n, double **mat)
{
    using namespace Eigen;

    MatrixXd mat_tmp(m, n);

    int i, j;

    for (i = 0; i < m; ++i) {
        for (j = 0; j < n; ++j) {
            mat_tmp(i,j) = mat[i][j];
        }
    }
    ColPivHouseholderQR<MatrixXd> qr(mat_tmp);
    return qr.rank();
}
#endif

int Fitting::rankQRD(const int m, const int n, double *mat, const double tolerance)
{
    // Return the rank of matrix mat revealed by the column pivoting QR decomposition
    // The matrix mat is destroyed.

    int m_ = m;
    int n_ = n;

    int LDA = m_;

    int LWORK = 10 * n_;
    int INFO;
    int *JPVT;
    double *WORK, *TAU;

    int nmin = std::min<int>(m_, n_);

    memory->allocate(JPVT, n_);
    memory->allocate(WORK, LWORK);
    memory->allocate(TAU, nmin);

    for (int i = 0; i < n_; ++i) JPVT[i] = 0;

    dgeqp3_(&m_, &n_, mat, &LDA, JPVT, TAU, WORK, &LWORK, &INFO);

    memory->deallocate(JPVT);
    memory->deallocate(WORK);
    memory->deallocate(TAU);

    if (std::abs(mat[0]) < eps) return 0;

    double **mat_tmp;
    memory->allocate(mat_tmp, m_, n_);

    unsigned long k = 0;

    for (int j = 0; j < n_; ++j) {
        for (int i = 0; i < m_; ++i) {
            mat_tmp[i][j] = mat[k++];
        }
    }

    int nrank = 0;
    for (int i = 0; i < nmin; ++i) {
        if (std::abs(mat_tmp[i][i]) > tolerance * std::abs(mat[0])) ++nrank;
    }

    memory->deallocate(mat_tmp);

    return nrank;
}

int Fitting::rankSVD(const int m, const int n, double *mat, const double tolerance)
{
    int i;
    int m_ = m;
    int n_ = n;

    int LWORK = 10 * m;
    int INFO;
    int *IWORK;
    int ldu = 1, ldvt = 1;
    double *s, *WORK;
    double u[1], vt[1];

    int nmin = std::min<int>(m, n);

    memory->allocate(IWORK, 8 * nmin);
    memory->allocate(WORK, LWORK);
    memory->allocate(s, nmin);

    char mode[]  = "N";

    dgesdd_(mode, &m_, &n_, mat, &m_, s, u, &ldu, vt, &ldvt, WORK, &LWORK, IWORK, &INFO);

    int rank = 0;
    for (i = 0; i < nmin; ++i) {
        if (s[i] > s[0] * tolerance) ++rank;
    }

    memory->deallocate(WORK);
    memory->deallocate(IWORK);
    memory->deallocate(s);

    return rank;
}

int Fitting::rankSVD2(const int m_in, const int n_in, double **mat, const double tolerance)
{
    // Reveal the rank of matrix mat without destroying the matrix elements

    int i, j, k;
    double *arr;

    int m = m_in;
    int n = n_in;

    memory->allocate(arr, m*n);

    k = 0;

    for (j = 0; j < n; ++j ) {
        for (i = 0; i < m; ++i) {
            arr[k++] = mat[i][j];
        }
    }

    int LWORK = 10 * m;
    int INFO;
    int *IWORK;
    int ldu = 1, ldvt = 1;
    double *s, *WORK;
    double u[1], vt[1];

    int nmin = std::min<int>(m, n);

    memory->allocate(IWORK, 8 * nmin);
    memory->allocate(WORK, LWORK);
    memory->allocate(s, nmin);

    char mode[]  = "N";

    dgesdd_(mode, &m, &n, arr, &m, s, u, &ldu, vt, &ldvt, WORK, &LWORK, IWORK, &INFO);

    int rank = 0;
    for (i = 0; i < nmin; ++i){
        if (s[i] > s[0] * tolerance) ++rank;
    }

    memory->deallocate(IWORK);
    memory->deallocate(WORK);
    memory->deallocate(s);
    memory->deallocate(arr);

    return rank;
}

/*
void Fitting::calc_covariance(int m, int n)
{
Eigen::MatrixXd Atmp(m, n), Hess(n, n);
int i, j;

for (i = 0; i < m; ++i){
for (j = 0; j < n; ++j){
Atmp(i,j) = amat[i][j];
}
}

Hess = (Atmp.transpose()*Atmp).inverse();

for (i = 0; i < n; ++i){
for (j = 0; j < n; ++j){
varcovar[i][j] = Hess(i, j);
}
}

}
*/


#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/complex.h>
#include <pybind11/numpy.h>

#include <vector>
#include <string>
#include <cstdarg>
#include <cstring>
#include <cstddef>
#include <iterator>

#include <boost/lexical_cast.hpp>

#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Delaunay_mesh_face_base_2.h>
#include <CGAL/Triangulation_vertex_base_with_info_2.h>

namespace py = pybind11;

using K = CGAL::Exact_predicates_inexact_constructions_kernel;
using Vb = CGAL::Triangulation_vertex_base_with_info_2<unsigned int, K>;
using Tds = CGAL::Triangulation_data_structure_2<Vb>;
using DT = CGAL::Delaunay_triangulation_2<K, Tds>;

using Point = K::Point_2;
using Vertex_handle = DT::Vertex_handle;
using Vi = DT::Finite_vertices_iterator;

template <typename T>
class TypedInputIterator
{
public:
    using iterator_category = std::input_iterator_tag;
    using difference_type = std::ptrdiff_t;
    using value_type = T;
    using pointer = T*;
    using reference = T&;

    explicit TypedInputIterator(py::iterator& py_iter) :
        py_iter_(py_iter)
    {
    }

    explicit TypedInputIterator(py::iterator&& py_iter) :
        py_iter_(py_iter)
    {
    }

    value_type operator*()
    {
        return (*py_iter_).template cast<value_type>();
    }

    TypedInputIterator operator++(int)
    {
        auto copy = *this;
        ++py_iter_;
        return copy;
    }

    TypedInputIterator& operator++()
    {
        ++py_iter_;
        return *this;
    }

    bool operator!=(TypedInputIterator &rhs)
    {
        return py_iter_ != rhs.py_iter_;
    }

    bool operator==(TypedInputIterator &rhs)
    {
        return py_iter_ == rhs.py_iter_;
    }

private:
    py::iterator py_iter_;
};

PYBIND11_MODULE(delaunay_class, m)
{
    py::class_<Point>(m, "Point")
            .def(py::init<int, int>(),  py::arg("x"), py::arg("y"))
            .def(py::init<double, double>(), py::arg("x"), py::arg("y"))
            .def_property_readonly("x", &Point::x)
            .def_property_readonly("y", &Point::y)
            .def("__repr__",
            [](const Point &p) {
                std::string r("Point(");
                r += boost::lexical_cast<std::string>(p.x());
                r += ", ";
                r += boost::lexical_cast<std::string>(p.y());
                r += ")";
                return r;
            })
            ;

    py::class_<Vertex_handle>(m, "VertexHandle")
        .def_property_readonly(
        "point",
        [](const Vertex_handle& vertex_handle)
        {
            return vertex_handle->point();
        })
        ;

        py::class_<DT>(m, "DelaunayTriangulation")

            .def(py::init())

            .def("insert", [](DT & dt, const std::vector<double> & p) {
                  std::vector< std::pair<Point,unsigned> > points;
                  int num_points = p.size()/2;
                  // start adding at the end of the current table
                  int start = dt.number_of_vertices();
                  for(std::size_t i = 0; i < num_points; ++i)
                  {
                    // add index information to form face table later
                     points.push_back( std::make_pair( Point(p[i*2+0],p[i*2+1]), start) );
                     start += 1;
                  }
                  return dt.insert(points.begin(),points.end());
                })

            .def("remove", [](DT & dt, const std::vector<unsigned int> & to_remove) {
                    int num_to_remove= to_remove.size();
                    std::vector<Vertex_handle> handles;
                    for (Vi vi = dt.finite_vertices_begin(); vi != dt.finite_vertices_end(); vi++){
                        handles.push_back(vi);
                    }
                    for(std::size_t i=0; i < num_to_remove; ++i){
                        dt.remove(handles[to_remove[i]]);
                    }
                    return dt;
                })

            .def("move", [](DT & dt, const std::vector<unsigned int> & to_move, const std::vector<double> & new_positions){
                    std::vector<Vertex_handle> handles;
                    std::vector<Point> new_pos;
                    int num_to_move= to_move.size();
                    // store all vertex handles
                    for (Vi vi = dt.finite_vertices_begin(); vi != dt.finite_vertices_end(); vi++){
                        handles.push_back(vi);
                    }
                    // store new positions as a vector of Point
                    for(std::size_t i = 0; i < num_to_move; ++i)
                    {
                       new_pos.push_back(Point(new_positions[2*i], new_positions[2*i+1]));
                    }
                    //
                    for(std::size_t i = 0; i < num_to_move; ++i)
                    {
                       dt.move( handles[to_move[i]], new_pos[i] );
                    }
                    return dt;
                    })

            .def("number_of_vertices", &DT::number_of_vertices)

            .def("number_of_faces", [](DT & dt){
                int count=0;
                for(DT::Finite_faces_iterator fit = dt.finite_faces_begin();
                fit != dt.finite_faces_end(); ++fit) {
                    count += 1;
                }
                    return count;
                })

            .def("finite_vertices", [](DT & dt) -> py::iterator
             {
                 return py::make_iterator(dt.finite_vertices_begin(), dt.finite_vertices_end());
             })

            .def("get_finite_cells", [](DT & dt)
            {
              // ouput the face table
              // YOU MUST CALL get_finite_vertices before if any incremental operations
              // were performed
              std::vector<int> faces;
              faces.resize(dt.number_of_faces()*3);

              int i=0;
              for(DT::Finite_faces_iterator fit = dt.finite_faces_begin();
                fit != dt.finite_faces_end(); ++fit) {

                DT::Face_handle face = fit;
                faces[i*3]=face->vertex(0)->info();
                faces[i*3+1]=face->vertex(1)->info();
                faces[i*3+2]=face->vertex(2)->info();
                i+=1;
              }
              ssize_t              soint      = sizeof(int);
              ssize_t              num_faces = faces.size()/3;
              ssize_t              ndim      = 2;
              std::vector<ssize_t> shape     = {num_faces, 3};
              std::vector<ssize_t> strides   = {soint*3, soint};

              // return 2-D NumPy array
              return py::array(py::buffer_info(
                faces.data(),                           /* data as contiguous array  */
                sizeof(int),                          /* size of one scalar        */
                py::format_descriptor<int>::format(), /* data type                 */
                2,                                    /* number of dimensions      */
                shape,                                   /* shape of the matrix       */
                strides                                  /* strides for each axis     */
              ));
            })

            .def("get_finite_vertices", [](DT & dt)
             {
               // ouput the vertices
               std::vector<double> vertices;
               vertices.resize(dt.number_of_vertices()*2);

               int i=0;
               for(DT::Finite_vertices_iterator fit = dt.finite_vertices_begin();
                 fit != dt.finite_vertices_end(); ++fit) {

                 Vertex_handle vertex = fit;
                 // critical! update the point index table so faces comes out correctly
                 vertex->info() = i;
                 vertices[i*2]=vertex->point().x();
                 vertices[i*2+1]=vertex->point().y();
                 i+=1;
               }
               ssize_t              sdble   = sizeof(double);
               ssize_t              num_vertices = vertices.size()/2;
               ssize_t              ndim      = 2;
               std::vector<ssize_t> shape     = {num_vertices, 2};
               std::vector<ssize_t> strides   = {sdble*2, sdble};

               // return 2-D NumPy array
               return py::array(py::buffer_info(
                 vertices.data(),                           /* data as contiguous array  */
                 sizeof(double),                          /* size of one scalar        */
                 py::format_descriptor<double>::format(), /* data type                 */
                 2,                                    /* number of dimensions      */
                 shape,                                   /* shape of the matrix       */
                 strides                                  /* strides for each axis     */
               ));
             })
            ;

    py::class_<DT::Finite_vertices_iterator::value_type>(m, "Vertex")
            .def_property_readonly(
                "point", [](DT::Finite_vertices_iterator::value_type& vertex)
                {
                    return vertex.point();
                }
            )
            ;

    py::class_<DT::Finite_faces_iterator::value_type>(m, "Face")
            .def("vertex_handle",
                [](DT::Finite_faces_iterator::value_type& face, int index)
                {
                    return face.vertex(index);
                },
                py::arg("index")
            )
            ;

}


// g++ -O3 -std=c++17 -fopenmp stifness_assembly.cpp -I"/usr/include/tbb"
// -I"/usr/include/eigen3" && ./a.out

// // #include <tbb/task_scheduler_init.h>
// #include <tbb/tbb.h>

// clang-format off
// Eigen::initParallel(); //TODO try this

// #include <Eigen/Core>
// #include <Eigen/Dense>
// #include <Eiegn/Sparse>
#include <iostream>

void test() {

   // Eigen::MatrixXd HHH=Eigen::MatrixXd::Zero(6,6);
   // Eigen::MatrixXd KKK=Eigen::MatrixXd::Zero(6,6);
   // HHH+=KKK;
   // HHH=HHH.transpose();

}

// Eigen::SparseMatrix<double, Eigen::RowMajor> K(n, n);
// K.coeffs() = 0.0;
// auto Ke = Eigen::MatrixXd::Ones(n_el, n_el);

// #pragma omp critical
//     {
//       for (auto b = 0; b < dofs.size(); b++) {
//         for (auto a = 0; a < dofs.size(); a++) {
// #pragma omp critical
//           K.coeffRef(int(dofs[a]), int(dofs[b])) += Ke(a, b);
//         }
// }
// }