/* 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.
 */
#include <NTL/ZZ.h>
#include <algorithm>
#include <complex>

#include "norms.h"
#include "helib.h"
#include "debugging.h"
#include "ArgMap.h"

NTL_CLIENT
using namespace helib;

bool verbose=false;

// Compute the L-infinity distance between two vectors
double calcMaxDiff(const vector<cx_double>& v1,
                   const vector<cx_double>& v2){

  if(lsize(v1)!=lsize(v2))
    NTL::Error("Vector sizes differ.\nFAILED\n");

  double maxDiff = 0.0;
  for (long i=0; i<lsize(v1); i++) {
    double diffAbs = std::abs(v1[i]-v2[i]);
    if (diffAbs > maxDiff)
      maxDiff = diffAbs;
  }

  return maxDiff;
}
// Compute the max relative difference between two vectors
double calcMaxRelDiff(const vector<cx_double>& v1,
                   const vector<cx_double>& v2)
{
    if(lsize(v1)!=lsize(v2))
        NTL::Error("Vector sizes differ.\nFAILED\n");

    // Compute the largest-magnitude value in the vector
    double maxAbs = 0.0;
    for (auto& x : v1) {
        if (std::abs(x) > maxAbs)
            maxAbs = std::abs(x);
    }
    if (maxAbs<1e-10)
        maxAbs = 1e-10;

    double maxDiff = 0.0;
    for (long i=0; i<lsize(v1); i++) {
        double relDiff = std::abs(v1[i]-v2[i]) / maxAbs;
        if (relDiff > maxDiff)
            maxDiff = relDiff;
    }

    return maxDiff;
}

inline bool cx_equals(const vector<cx_double>& v1,
                      const vector<cx_double>& v2,
                      double epsilon)
{
  return (calcMaxRelDiff(v1,v2) < epsilon);
}

void testBasicArith(const FHEPubKey& publicKey,
                    const FHESecKey& secretKey,
                    const EncryptedArrayCx& ea, double epsilon);
void testComplexArith(const FHEPubKey& publicKey,
                      const FHESecKey& secretKey,
                      const EncryptedArrayCx& ea, double epsilon);
void testRotsNShifts(const FHEPubKey& publicKey,
                     const FHESecKey& secretKey,
                     const EncryptedArrayCx& ea, double epsilon);

void debugCompare(const EncryptedArrayCx& ea, const FHESecKey& sk,
        vector<cx_double>& p, const Ctxt& c, double epsilon)
{
  vector<cx_double> pp;
  ea.decrypt(c, sk, pp);
  std::cout << "    relative-error="<<calcMaxRelDiff(p,pp)
            << ", absolute-error="<<calcMaxRelDiff(p,pp)<<endl;
//  if (!cx_equals(pp, p, epsilon)) {
//    std::cout << "oops:\n"; std::cout << p << "\n";
//    std::cout << pp << "\n";
//    exit(0);
//  }
}

void negateVec(vector<cx_double>& p1)
{
  for (auto& x: p1) x = -x;
}
void add(vector<cx_double>& to, const vector<cx_double>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (long i=0; i<from.size(); i++) to[i] += from[i];
}
void sub(vector<cx_double>& to, const vector<cx_double>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (long i=0; i<from.size(); i++) to[i] -= from[i];
}
void mul(vector<cx_double>& to, const vector<cx_double>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (long i=0; i<from.size(); i++) to[i] *= from[i];
}
void rotate(vector<cx_double>& p, long amt)
{
  long sz = p.size();
  vector<cx_double> tmp(sz);
  for (long i=0; i<sz; i++)
    tmp[((i+amt)%sz +sz)%sz] = p[i];
  p = tmp;
}

/************** Each round consists of the following:
1. c1.multiplyBy(c0)
2. c0 += random constant
3. c2 *= random constant
4. tmp = c1
5. ea.rotate(tmp, random amount in [-nSlots/2, nSlots/2])
6. c2 += tmp
7. ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
8. c1.negate()
9. c3.multiplyBy(c2)
10. c0 -= c3
**************/
void testGeneralOps(const FHEPubKey& publicKey, const FHESecKey& secretKey,
                    const EncryptedArrayCx& ea, double epsilon,
                    long nRounds)
{
  long nslots = ea.size();
  char buffer[32];

  vector<cx_double> p0, p1, p2, p3;
  ea.random(p0);
  ea.random(p1);
  ea.random(p2);
  ea.random(p3);

  Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  ea.encrypt(c0, publicKey, p0, /*size=*/1.0);
  ea.encrypt(c1, publicKey, p1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, p2, /*size=*/1.0);
  ea.encrypt(c3, publicKey, p3, /*size=*/1.0);

  resetAllTimers();
  FHE_NTIMER_START(Circuit);

  for (long i = 0; i < nRounds; i++) {

    if (verbose) std::cout << "*** round " << i << "..."<<endl;

     long shamt = RandomBnd(2*(nslots/2) + 1) - (nslots/2);
                  // random number in [-nslots/2..nslots/2]
     long rotamt = RandomBnd(2*nslots - 1) - (nslots - 1);
                  // random number in [-(nslots-1)..nslots-1]

     // two random constants
     vector<cx_double> const1, const2;
     ea.random(const1);
     ea.random(const2);

     ZZX const1_poly, const2_poly;
     ea.encode(const1_poly, const1, /*size=*/1.0);
     ea.encode(const2_poly, const2, /*size=*/1.0);

     mul(p1, p0);     // c1.multiplyBy(c0)
     c1.multiplyBy(c0);
     if (verbose) {
       CheckCtxt(c1, "c1*=c0");
       debugCompare(ea, secretKey, p1, c1, epsilon);
     }

     add(p0, const1); // c0 += random constant
     c0.addConstant(const1_poly);
     if (verbose) {
       CheckCtxt(c0, "c0+=k1");
       debugCompare(ea, secretKey, p0, c0, epsilon);
     }
     mul(p2, const2); // c2 *= random constant
     c2.multByConstant(const2_poly);
     if (verbose) {
       CheckCtxt(c2, "c2*=k2");
       debugCompare(ea, secretKey, p2, c2, epsilon);
     }
     vector<cx_double> tmp_p(p1); // tmp = c1
     Ctxt tmp(c1);
     sprintf(buffer, "tmp=c1>>=%d", (int)shamt);
     rotate(tmp_p, shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
     ea.rotate(tmp, shamt);
     if (verbose) {
       CheckCtxt(tmp, buffer);
       debugCompare(ea, secretKey, tmp_p, tmp, epsilon);
     }
     add(p2, tmp_p);  // c2 += tmp
     c2 += tmp;
     if (verbose) {
       CheckCtxt(c2, "c2+=tmp");
       debugCompare(ea, secretKey, p2, c2, epsilon);
     }
     sprintf(buffer, "c2>>>=%d", (int)rotamt);
     rotate(p2, rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
     ea.rotate(c2, rotamt);
     if (verbose) {
       CheckCtxt(c2, buffer);
       debugCompare(ea, secretKey, p2, c2, epsilon);
     }
     negateVec(p1); // c1.negate()
     c1.negate();
     if (verbose) {
       CheckCtxt(c1, "c1=-c1");
       debugCompare(ea, secretKey, p1, c1, epsilon);
     }
     mul(p3, p2); // c3.multiplyBy(c2)
     c3.multiplyBy(c2);
     if (verbose) {
       CheckCtxt(c3, "c3*=c2");
       debugCompare(ea, secretKey, p3, c3, epsilon);
     }
     sub(p0, p3); // c0 -= c3
     c0 -= c3;
     if (verbose) {
       CheckCtxt(c0, "c0=-c3");
       debugCompare(ea, secretKey, p0, c0, epsilon);
     }
  }

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  FHE_NTIMER_STOP(Circuit);

  vector<cx_double> pp0, pp1, pp2, pp3;

  ea.decrypt(c0, secretKey, pp0);
  ea.decrypt(c1, secretKey, pp1);
  ea.decrypt(c2, secretKey, pp2);
  ea.decrypt(c3, secretKey, pp3);

  std::cout << "Test "<<nRounds<<" rounds of mixed operations, ";
  if (cx_equals(pp0, p0,conv<double>(epsilon*c0.getPtxtMag()))
      && cx_equals(pp1, p1,conv<double>(epsilon*c1.getPtxtMag()))
      && cx_equals(pp2, p2,conv<double>(epsilon*c2.getPtxtMag()))
      && cx_equals(pp3, p3,conv<double>(epsilon*c3.getPtxtMag())))
    std::cout << "PASS\n\n";
  else {
    std::cout << "FAIL:\n";
    std::cout << "  max(p0)="<<largestCoeff(p0)
              << ", max(pp0)="<<largestCoeff(pp0)
              << ", maxDiff="<<calcMaxDiff(p0,pp0) << endl;
    std::cout << "  max(p1)="<<largestCoeff(p1)
              << ", max(pp1)="<<largestCoeff(pp1)
              << ", maxDiff="<<calcMaxDiff(p1,pp1) << endl;
    std::cout << "  max(p2)="<<largestCoeff(p2)
              << ", max(pp2)="<<largestCoeff(pp2)
              << ", maxDiff="<<calcMaxDiff(p2,pp2) << endl;
    std::cout << "  max(p3)="<<largestCoeff(p3)
              << ", max(pp3)="<<largestCoeff(pp3)
              << ", maxDiff="<<calcMaxDiff(p3,pp3) << endl<<endl;
  }

  if (verbose) {
    std::cout << endl;
    printAllTimers();
    std::cout << endl;
  }
  resetAllTimers();
   }

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

  // Commandline setup

  ArgMap amap;

  long m=16;
  long r=8;
  long L=0;
  double epsilon=0.01; // Accepted accuracy
  long R=1;
  long seed=0;
  bool debug = false;

  amap.arg("m", m, "Cyclotomic index");
  amap.note("e.g., m=1024, m=2047");
  amap.arg("r", r, "Bits of precision");
  amap.arg("R", R, "number of rounds");
  amap.arg("L", L, "Number of bits in modulus", "heuristic");
  amap.arg("ep", epsilon, "Accepted accuracy");
  amap.arg("seed", seed, "PRG seed");
  amap.arg("verbose", verbose, "more printouts");
  amap.arg("debug", debug, "for debugging");

  amap.parse(argc, argv);

  if (seed)
    NTL::SetSeed(ZZ(seed));

  if (R<=0) R=1;
  if (R<=2)
    L = 100*R;
  else
    L = 220*(R-1);

  if (verbose) {
    cout << "** m="<<m<<", #rounds="<<R<<", |q|="<<L
         << ", epsilon="<<epsilon<<endl;
  }
  epsilon /= R;
  try{

    // FHE setup keys, context, SKMs, etc

    FHEcontext context(m, /*p=*/-1, r);
    context.scale=4;
    buildModChain(context, L, /*c=*/2);

    FHESecKey secretKey(context);
    secretKey.GenSecKey(); // A +-1/0 secret key
    addSome1DMatrices(secretKey); // compute key-switching matrices

    const FHEPubKey publicKey = secretKey;
    const EncryptedArrayCx& ea = context.ea->getCx();

    if (verbose) {
      ea.getPAlgebra().printout();
      cout << "r = " << context.alMod.getR() << endl;
      cout << "ctxtPrimes="<<context.ctxtPrimes
           << ", specialPrimes="<<context.specialPrimes<<endl<<endl;
    }
    if (debug) {
        dbgKey = & secretKey;
        dbgEa = (EncryptedArray*) context.ea;
    }
#ifdef DEBUG_PRINTOUT
          dbgKey = & secretKey;
          dbgEa = (EncryptedArray*) context.ea;
#endif //DEBUG_PRINTOUT

    // Run the tests.
    testBasicArith(publicKey, secretKey, ea, epsilon);
    testComplexArith(publicKey, secretKey, ea, epsilon);
    testRotsNShifts(publicKey, secretKey, ea, epsilon);
    testGeneralOps(publicKey, secretKey, ea, epsilon*R, R);
  }
  catch (exception& e) {
    cerr << e.what() << endl;
    cerr << "***Major FAIL***" << endl;
  }

  return 0;
}

void testBasicArith(const FHEPubKey& publicKey,
                    const FHESecKey& secretKey,
                    const EncryptedArrayCx& ea, double epsilon)
{
  if (verbose)  cout << "Test Arithmetic ";
  // Test objects

  Ctxt c1(publicKey), c2(publicKey), c3(publicKey);

  vector<cx_double> vd;
  vector<cx_double> vd1, vd2, vd3;
  ea.random(vd1);
  ea.random(vd2);

  // test encoding of shorter vectors
  vd1.resize(vd1.size()-2);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  vd1.resize(vd1.size()+2, 0.0);

  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  // Test - Multiplication
  c1 *= c2;
  for (long i=0; i<lsize(vd1); i++) vd1[i] *= vd2[i];

  ZZX poly;
  ea.random(vd3);
  ea.encode(poly, vd3, /*size=*/1.0);
  c1.addConstant(poly); // vd1*vd2 + vd3
  for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];

  // Test encoding, encryption of a single number
  double xx = NTL::RandomLen_long(16)/double(1L<<16); // random in [0,1]
  ea.encryptOneNum(c2, publicKey, xx);
  c1 += c2;
  for (auto& x : vd1) x += xx;

  // Test - Multiply by a mask
  vector<long> mask(lsize(vd1), 1);
  for (long i=0; i*(i+1)<lsize(mask); i++) {
    mask[i*i] = 0;
    mask[i*(i+1)] = -1;
  }

  ea.encode(poly,mask, /*size=*/1.0);
  c1.multByConstant(poly); // mask*(vd1*vd2 + vd3)
  for (long i=0; i<lsize(vd1); i++) vd1[i] *= mask[i];

  // Test - Addition
  ea.random(vd3);
  ea.encrypt(c3, publicKey, vd3, /*size=*/1.0);
  c1 += c3;
  for (long i=0; i<lsize(vd1); i++) vd1[i] += vd3[i];

  c1.negate();
  c1.addConstant(to_ZZ(1));
  for (long i=0; i<lsize(vd1); i++) vd1[i] = 1.0 - vd1[i];

  // Diff between approxNums HE scheme and plaintext floating
  ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
  printVec(cout<<"res=", vd, 10)<<endl;
  printVec(cout<<"vec=", vd1, 10)<<endl;
#endif
  if (verbose)
    cout << "(max |res-vec|_{infty}="<< calcMaxDiff(vd, vd1) << "): ";

  if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag())))
    cout << "GOOD\n";
  else {
    cout << "BAD:\n";
    std::cout << "  max(vd)="<<largestCoeff(vd)
              << ", max(vd1)="<<largestCoeff(vd1)
              << ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl;
  }
}

void testComplexArith(const FHEPubKey& publicKey,
                      const FHESecKey& secretKey,
                      const EncryptedArrayCx& ea, double epsilon)
{

  // Test complex conjugate
  Ctxt c1(publicKey), c2(publicKey);

  vector<cx_double> vd;
  vector<cx_double> vd1, vd2;
  ea.random(vd1);
  ea.random(vd2);

  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  if (verbose)
    cout << "Test Conjugate: ";
  for_each(vd1.begin(), vd1.end(), [](cx_double& d){d=std::conj(d);});
  c1.complexConj();
  ea.decrypt(c1, secretKey, vd);
#ifdef DEBUG_PRINTOUT
  printVec(cout<<"vd1=", vd1, 10)<<endl;
  printVec(cout<<"res=", vd, 10)<<endl;
#endif
  if (cx_equals(vd, vd1, conv<double>(epsilon*c1.getPtxtMag())))
    cout << "GOOD\n";
  else {
    cout << "BAD:\n";
    std::cout << "  max(vd)="<<largestCoeff(vd)
              << ", max(vd1)="<<largestCoeff(vd1)
              << ", maxDiff="<<calcMaxDiff(vd,vd1) << endl<<endl;
  }

  // Test that real and imaginary parts are actually extracted.
  Ctxt realCtxt(c2), imCtxt(c2);
  vector<cx_double> realParts(vd2), real_dec;
  vector<cx_double> imParts(vd2), im_dec;

  if (verbose)
    cout << "Test Real and Im parts: ";
  for_each(realParts.begin(), realParts.end(), [](cx_double& d){d=std::real(d);});
  for_each(imParts.begin(), imParts.end(), [](cx_double& d){d=std::imag(d);});

  ea.extractRealPart(realCtxt);
  ea.decrypt(realCtxt, secretKey, real_dec);

  ea.extractImPart(imCtxt);
  ea.decrypt(imCtxt, secretKey, im_dec);

#ifdef DEBUG_PRINTOUT
  printVec(cout<<"vd2=", vd2, 10)<<endl;
  printVec(cout<<"real=", realParts, 10)<<endl;
  printVec(cout<<"res=", real_dec, 10)<<endl;
  printVec(cout<<"im=", imParts, 10)<<endl;
  printVec(cout<<"res=", im_dec, 10)<<endl;
#endif
  if (cx_equals(realParts,real_dec,conv<double>(epsilon*realCtxt.getPtxtMag()))
      && cx_equals(imParts, im_dec, conv<double>(epsilon*imCtxt.getPtxtMag())))
    cout << "GOOD\n";
  else {
    cout << "BAD:\n";
    std::cout << "  max(re)="<<largestCoeff(realParts)
              << ", max(re1)="<<largestCoeff(real_dec)
              << ", maxDiff="<<calcMaxDiff(realParts,real_dec) << endl;
    std::cout << "  max(im)="<<largestCoeff(imParts)
              << ", max(im1)="<<largestCoeff(im_dec)
              << ", maxDiff="<<calcMaxDiff(imParts,im_dec) << endl<<endl;
  }
}

void testRotsNShifts(const FHEPubKey& publicKey,
                     const FHESecKey& secretKey,
                     const EncryptedArrayCx& ea, double epsilon)
{

  std::srand(std::time(0)); // set seed, current time.
  int nplaces = rand() % static_cast<int>(ea.size()/2.0) + 1;

  if (verbose)
    cout << "Test Rotation of " << nplaces << ": ";

  Ctxt c1(publicKey);
  vector<cx_double> vd1;
  vector<cx_double> vd_dec;
  ea.random(vd1);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);

#ifdef DEBUG_PRINTOUT
  printVec(cout<< "vd1=", vd1, 10)<<endl;
#endif
  std::rotate(vd1.begin(), vd1.end()-nplaces, vd1.end());
  ea.rotate(c1, nplaces);
  c1.reLinearize();
  ea.decrypt(c1, secretKey, vd_dec);
#ifdef DEBUG_PRINTOUT
  printVec(cout<< "vd1(rot)=", vd1, 10)<<endl;
  printVec(cout<<"res: ", vd_dec, 10)<<endl;
#endif

  if (cx_equals(vd1, vd_dec, conv<double>(epsilon*c1.getPtxtMag())))
    cout << "GOOD\n";
  else {
    cout << "BAD:\n";
    std::cout << "  max(vd)="<<largestCoeff(vd_dec)
              << ", max(vd1)="<<largestCoeff(vd1)
              << ", maxDiff="<<calcMaxDiff(vd_dec,vd1) << endl<<endl;
  }
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include<pybind11/numpy.h>
#include<fstream>
#include<iostream>
#include <vector>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

ArrayXd find_max(VectorXd mMat)
{
	VectorXd::Index maxRow;
	double max = mMat.maxCoeff(&maxRow);
	Matrix<bool, Dynamic, 1> MM = (mMat.array() == max).matrix();
	ArrayXd Index_max = ArrayXd::Zero(mMat.size());
	for (size_t i = 0; i < MM.rows(); i++)
	{
		if (MM(i))
		{
			Index_max[i] = 1.0;
		}

	}
	return Index_max;
}

MatrixXd select_M_PN(MatrixXd C, VectorXd P)
{
	MatrixXd res(C.rows(), int(P.sum()));
	Eigen::Index j = 0;
	for (Eigen::Index i = 0; i < C.cols(); ++i)
	{
		if (P(i) > 0) res.col(j++) = C.col(i);
	}
	return res;

}

MatrixXd select_V_PN(VectorXd V, VectorXd P)
{
	VectorXd res(int(P.sum()));
	Eigen::Index j = 0;
	for (Eigen::Index i = 0; i < V.size(); ++i)
	{
		if (P(i) > 0) res(j++) = V(i);
	}
	return res;

}

VectorXd lsqnonneg(MatrixXd C, VectorXd d,double tol)
/*
min_{x>0}||Cx-d||_2^2
Reference: Lawson and Hanson,
"Solving Least Squares Problems",
Prentice-Hall, 1974.
*/
{
	d = C.transpose() * d;
	C = C.transpose() * C;
	int n = C.rows();
	// Initialize vector of n zeros and Infs(to be used later)
	VectorXd nZeros = VectorXd::Zero(n);
	VectorXd wz = nZeros;
	//Initialize set of non - active columns to null
	VectorXd P = nZeros;
	// Initialize set of active columns to all
	// and the initial point to zeros
	VectorXd Z = VectorXd::Ones(n);
	VectorXd x = nZeros;
	VectorXd w = d - C * x;
	VectorXd resid = w;
	w = C.transpose() * w;
	//Set up iteration criterion
	int outeriter = 0;
	int iter = 0;
	int itmax = 3 * n;
	VectorXd z = nZeros;
	VectorXd x_new = VectorXd::Zero(n+1);
	// Outer loop to put variables into set to hold positive coefficients
	while (Z.sum() > 0 && (w.array()*Z.array() > tol).any())
	{
		outeriter += 1;
		z = nZeros;
		wz = (wz.array()*(1 - P.array())).matrix() - P * DBL_MAX;
		wz = (wz.array()*(1 - Z.array()) + w.array()*Z.array()).matrix();
		ArrayXd t = find_max(wz);
		P = (P.array() * (1 - t) + t).matrix();
		Z = (Z.array()*(1 - t)).matrix();
		if (P.sum() == 1)
		{
			VectorXd::Index maxRow;
			P.maxCoeff(&maxRow);
			double bb = (C.col(maxRow)).transpose() * (C.col(maxRow));
			double aa = ((C.col(maxRow)).transpose() * d);
			z(maxRow) = aa / bb;
		}
		if (P.sum() >= 2)
		{
			MatrixXd C_ing = select_M_PN(C, P);
			VectorXd z_ing = (C_ing.transpose() * C_ing).ldlt().solve(C_ing.transpose() * d);
			Eigen::Index j = 0;
			for (size_t i = 0; i < P.size(); i++)
			{
				if (P(i) > 0) z(i) = z_ing(j++);
			}
		}
		while (((z.array()*P.array() + 1-P.array()) <= 0).any())
		{
			iter = iter + 1;
			if (iter > itmax)
			{
				// cout << "optimfun:lsqnonneg: IterationCountExceeded" << endl;
				x = z;
				double error = (resid.array()*resid.array()).sum();
				x_new << error,	x;
				return x_new;
			}

			// Find indices where intermediate solution z is approximately negative
			vector<double> alpha_v;
			for (Eigen::Index i = 0; i < P.size(); i++)
			{
				if (z(i) <= 0 && P(i) > 0) alpha_v.push_back(x(i) / (x(i) - z(i)));
			}
			x = x + (*min_element(alpha_v.begin(), alpha_v.end())) * (z - x);
			for (size_t i = 0; i < Z.size(); i++)
			{
				if ((abs(x(i)) < tol && P(i) > 0)||Z(i)>0)
				{
					Z(i) = 1;
				}
				else
				{
					Z(i) = 0;
				}

			}
			P = (1 - Z.array()).matrix();
			z = nZeros;
			if (P.sum() == 1)
			{
				VectorXd::Index maxRow;
				P.maxCoeff(&maxRow);
				double bb = (C.col(maxRow)).transpose() * (C.col(maxRow));
				double aa = ((C.col(maxRow)).transpose() * d);
				z(maxRow) = aa / bb;
			}
			if (P.sum() >= 2)
			{
				MatrixXd C_ing = select_M_PN(C, P);
				VectorXd z_ing = (C_ing.transpose() * C_ing).ldlt().solve(C_ing.transpose() * d);
				Eigen::Index j = 0;
				for (size_t i = 0; i < P.size(); i++)
				{
					if (P(i) > 0) z(i) = z_ing(j++);
				}
			}
		}
		x = z;
		resid = (d - C * x);
		w = C.transpose() * resid;
	}
	double error = (resid.array()*resid.array()).sum();
	x_new <<error, x;
	return x_new;
}

namespace py = pybind11;
PYBIND11_MODULE(libNNLS, m)
{
m.doc() = "Non-negative least squares";
m.def("lsqnonneg", &lsqnonneg);
}


#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <vector>
#include <chrono>
#include <unistd.h>
using namespace std;
using namespace Eigen;
using namespace std::chrono;

#define DIM 1000
#define NUM_CORES 4
#define ENABLE_PARALLEL

float get_time()
{
    return (float)duration_cast< milliseconds >(system_clock::now().time_since_epoch()).count();
}

void init_A(MatrixXd &A)
{
    for(int i = 0; i < DIM; i++)
    {
        for(int j = 0; j < DIM; j++)
        {
            A(i,j) = static_cast <float> (rand());
        }
    }
}

void init_b(VectorXd &b)
{
    for(int i = 0; i < DIM; i++)
    {
        b[i] = static_cast <float> (rand());
    }
}

void test_QRpseudoInv(MatrixXd &A, VectorXd &b)
{
    A.colPivHouseholderQr().solve(b);
}

void test_QR(MatrixXd &A, VectorXd &b)
{
    A.householderQr().solve(b);
}

void test_CompleteOrtho(MatrixXd &A, VectorXd &b)
{
    A.completeOrthogonalDecomposition().solve(b);
}

void test_LDLT(MatrixXd &A, VectorXd &b)
{
    A.ldlt().solve(b);
}

int main()
{
    /**********Declaration & Initialization**********/
    MatrixXd A(DIM,DIM);
    VectorXd b(DIM);
    init_A(A);
    init_b(b);
    /**********Declaration & Initialization**********/

#ifdef ENABLE_PARALLEL
    Eigen::initParallel();
    Eigen::setNbThreads(NUM_CORES);
#endif

    auto start_ = chrono::steady_clock::now();
    test_QRpseudoInv(A,b);
    auto end_ = chrono::steady_clock::now();
    cout << "colPivHouseholderQr : "
        << chrono::duration_cast<chrono::nanoseconds>(end_ - start_).count()
        << " ns" << endl;

    start_ = chrono::steady_clock::now();
    test_QR(A,b);
    end_ = chrono::steady_clock::now();
    cout << "householderQr : "
        << chrono::duration_cast<chrono::nanoseconds>(end_ - start_).count()
        << " ns" << endl;

    start_ = chrono::steady_clock::now();
    test_CompleteOrtho(A,b);
    end_ = chrono::steady_clock::now();
    cout << "completeOrthogonalDecomposition : "
        << chrono::duration_cast<chrono::nanoseconds>(end_ - start_).count()
        << " ns" << endl;

    start_ = chrono::steady_clock::now();
    test_LDLT(A,b);
    end_ = chrono::steady_clock::now();
    cout << "LDLT: "
        << chrono::duration_cast<chrono::nanoseconds>(end_ - start_).count()
        << " ns" << endl;

}


#include "cxxopts.hpp"

// see https://www.gitmemory.com/issue/ddemidov/amgcl/162/668662041 for inspiration

#include <amgcl/io/mm.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/amg.hpp>

#if defined(SOLVER_BACKEND_VEXCL)
#  include <amgcl/value_type/static_matrix.hpp>
#  include <amgcl/adapter/block_matrix.hpp>
#  include <amgcl/backend/vexcl.hpp>
#  include <amgcl/backend/vexcl_static_matrix.hpp>
   typedef amgcl::backend::vexcl<double> Backend;
#elif defined(SOLVER_BACKEND_CUDA)
#  include <amgcl/adapter/eigen.hpp>
#  include <amgcl/backend/cuda.hpp>
#  include <amgcl/relaxation/cusparse_ilu0.hpp>
   using Backend = amgcl::backend::cuda<double>;
#else
   using Backend = amgcl::backend::builtin<double>;
#endif

#include <amgcl/make_solver.hpp>
#include <amgcl/make_block_solver.hpp>
#include <amgcl/solver/bicgstab.hpp>
#include <amgcl/solver/gmres.hpp>
#include <amgcl/solver/runtime.hpp>
#include <amgcl/preconditioner/runtime.hpp>
#include <amgcl/amg.hpp>
#include <amgcl/coarsening/smoothed_aggregation.hpp>
#include <amgcl/coarsening/plain_aggregates.hpp>
#include <amgcl/coarsening/ruge_stuben.hpp>
#include <amgcl/coarsening/aggregation.hpp>
#include <amgcl/relaxation/spai0.hpp>
#include <amgcl/relaxation/ilu0.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

// ./amgcl-block-matrices -A ../data/B_test.mtx  -b 3

#include <amgcl/profiler.hpp>

namespace amgcl { profiler<> prof; }
using amgcl::prof;
using amgcl::precondition;

//---------------------------------------------------------------------------
std::tuple<size_t, double> scalar_solve(
        const boost::property_tree::ptree &prm,
        size_t rows,
        std::vector<ptrdiff_t> const &ptr,
        std::vector<ptrdiff_t> const &col,
        std::vector<double>    const &val,
        std::vector<double>    const &rhs,
        std::vector<double>          &x,
        bool reorder
        )
{
    Backend::params bprm;
    // We use the tuple of CRS arrays to represent the system matrix.
    // Note that std::tie creates a tuple of references, so no data is actually
    // copied here:
    auto A = std::tie(rows, ptr, col, val);

#if defined(SOLVER_BACKEND_VEXCL)
    vex::Context ctx(vex::Filter::Env);
    std::cout << ctx << std::endl;
    bprm.q = ctx;
#elif defined(SOLVER_BACKEND_VIENNACL)
    std::cout
        << viennacl::ocl::current_device().name()
        << " (" << viennacl::ocl::current_device().vendor() << ")\n\n";
#elif defined(SOLVER_BACKEND_CUDA)
    cusparseCreate(&bprm.cusparse_handle);
    {
        int dev;
        cudaGetDevice(&dev);

        cudaDeviceProp prop;
        cudaGetDeviceProperties(&prop, dev);
        std::cout << prop.name << std::endl << std::endl;
    }
#endif

    using Solver1 = amgcl::make_solver<
    amgcl::runtime::preconditioner<Backend>,
    amgcl::runtime::solver::wrapper<Backend>>;

    //using Solver2 = amgcl::make_block_solver<
    using Solver2 = amgcl::make_solver<
    // Use AMG as preconditioner:
    amgcl::amg<
        Backend,
        amgcl::coarsening::smoothed_aggregation,
        //amgcl::relaxation::spai0
        amgcl::relaxation::damped_jacobi
        >,
    // And BiCGStab as iterative solver:
    amgcl::solver::bicgstab<Backend>>;

    using Solver3 = amgcl::make_solver<
    // Use AMG as preconditioner:
    amgcl::amg<
        Backend,
        amgcl::coarsening::ruge_stuben,
        amgcl::relaxation::spai0
        //amgcl::relaxation::ilu0
        >,
    // And GMRES as iterative solver:
    amgcl::solver::gmres<Backend>>;

    try {
        Solver2::params prm;
        Solver2::backend_params bprm;
        // prm.precond.direct_coarse = false;
        prm.precond.direct_coarse = true;
        prm.solver.maxiter = 1200;
        prm.solver.verbose = true;
        prm.solver.tol = 1e-5;
        //prm.precond.coarsening ...

#if defined(SOLVER_BACKEND_VEXCL)
        vex::Context ctx(vex::Filter::Env);
        std::cout << ctx << std::endl;
        bprm.q = ctx;
#elif defined(SOLVER_BACKEND_VIENNACL)
        std::cout
            << viennacl::ocl::current_device().name()
            << " (" << viennacl::ocl::current_device().vendor() << ")\n\n";
#elif defined(SOLVER_BACKEND_CUDA)
        cusparseCreate(&bprm.cusparse_handle);
        {
            int dev;
            cudaGetDevice(&dev);

            cudaDeviceProp prop;
            cudaGetDeviceProperties(&prop, dev);
            std::cout << prop.name << std::endl << std::endl;
        }
#endif

        Solver2 solve( A, prm, bprm );

        auto f_b = Backend::copy_vector(rhs, bprm);
        auto x_b = Backend::copy_vector(x,   bprm);

        //auto [iters, error] = solve(A, rhs, x);
        auto [iters, error] = solve(A, *f_b, *x_b);

#if defined(SOLVER_BACKEND_VEXCL)
        vex::copy(*x_b, x);
#elif defined(SOLVER_BACKEND_VIENNACL)
        viennacl::fast_copy(*x_b, x);
#elif defined(SOLVER_BACKEND_CUDA)
        thrust::copy(x_b->begin(), x_b->end(), x.begin());
#else
        std::copy(&(*x_b)[0], &(*x_b)[0] + rows, &x[0]);
#endif

        // Output the number of iterations, the relative error:
        std::cout << "Iters: " << iters << std::endl
                  << "Error: " << error << std::endl;
        // std::cout << "x = [ ";
        // for (auto el : x) {
        //     std::cout << el << ", ";
        // }
        // std::cout << "]\n";
        return {iters, error};
    } catch(std::runtime_error &e) {
        std::cout << "caught exception: " << e.what() << std::endl;
    }

    return {0, 0.0};
}

int main(int argc, char *argv[])
{
    //namespace po = boost::program_options;
    namespace io = amgcl::io;

    using std::vector;
    using std::string;

    cxxopts::Options options("amgcl_block_matrices", "check AMG for different block sizes");
    options
        .allow_unrecognised_options()
        .add_options()
        ("A,matrix", "System matrix in the MatrixMarket format.",
         cxxopts::value<std::string>())
        ("b,block-size", "The block size of the system matrix. ",
         cxxopts::value<int>()->default_value("1"))
        ("h,help", "Print help")
        ;

    auto vm = options.parse(argc, argv);

    if (vm.count("help")) {
        std::cout << options.help() << std::endl;
        return 0;
    }

    for (int i = 0; i < argc; ++i) {
        if (i) std::cout << " ";
        std::cout << argv[i];
    }
    std::cout << std::endl;

    boost::property_tree::ptree prm;
    if (vm.count("prm-file")) {
        read_json(vm["prm-file"].as<string>(), prm);
    }

    if (vm.count("prm")) {
        for(const string &v : vm["prm"].as<vector<string>>()) {
            amgcl::put(prm, v);
        }
    }

    size_t n, block_size = 1;
    vector<ptrdiff_t> ptr, col;
    vector<double> val;

    if (vm.count("block-size")) {
        block_size = vm["block-size"].as<int>();
    }

    if (vm.count("matrix")) {
        string Afile  = vm["matrix"].as<string>();
        auto [rows, cols] = io::mm_reader(Afile)(ptr, col, val);

        std::cout << "rows: " << rows << std::endl;
        std::cout << "cols: " << cols << std::endl;
        n = rows;
    }

    // We use the tuple of CRS arrays to represent the system matrix.
    // Note that std::tie creates a tuple of references, so no data is actually
    // copied here:
    auto A = std::tie(n, ptr, col, val);
    std::vector<double> rhs (n, 0.0);
    std::vector<double> x   (n, 0.0);

    // Set RHS := Ax where x = 1
    for (size_t i = 0; i < n; ++i) {
        double s = 0;
        for (ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
            s += val[j];
        }
        rhs[i] = s;
    }

    if (block_size == 1) { // scalar case
        scalar_solve(prm, n, ptr,col, val, rhs, x, false);
    } else {
        // Compose the solver type
        constexpr size_t B = 6; // 3
        using dmat_type = amgcl::static_matrix<double, B, B>;
        using dvec_type = amgcl::static_matrix<double, B, 1>;
        using SBackend = amgcl::backend::builtin<double>;    // the outer iterative solver backend
        using PBackend = amgcl::backend::builtin<float>;     // the PSolver backend
        using UBackend = amgcl::backend::builtin<dmat_type>; // the USolver backend
        using BlockSolver = amgcl::make_block_solver<
                // preconditioner
                amgcl::amg<
                    UBackend,
                    amgcl::coarsening::aggregation,
                    amgcl::relaxation::ilu0
                    >,
                // solver
                amgcl::solver::gmres<UBackend>>;
        using Solver = amgcl::make_solver<
                // preconditioner
                amgcl::amg<
                    UBackend,
                    amgcl::coarsening::aggregation,
                    amgcl::relaxation::ilu0
                    >,
                // solver
                amgcl::solver::gmres<UBackend>>;
        try {
            // see `tutorial/3.CoupCons3D/coupcons3d.cpp`
#if 1
            BlockSolver::params prm;
            prm.solver.maxiter = 1200;
            prm.solver.verbose = true;
            BlockSolver::backend_params bprm;
            prm.precond.direct_coarse = false;
            prm.precond.allow_rebuild = true;
            auto Ab = amgcl::adapter::block_matrix<dmat_type>(A);
            auto rhsb = reinterpret_cast<dvec_type*>(rhs.data());
            auto xb = reinterpret_cast<dvec_type*>(x.data());
            auto RHS = amgcl::make_iterator_range(rhsb, rhsb + n / 3);
            auto X = amgcl::make_iterator_range(xb, xb + n / block_size);
            Solver solve0( Ab, prm, bprm );
            auto [iters0, error0] = solve0(Ab, RHS, X);
            std::cout << "Iters0: " << iters0 << std::endl
                      << "Error0: " << error0 << std::endl;

            BlockSolver solve( A, prm, bprm );
            auto [iters, error] = solve(A, rhs, x);

            std::cout << "Iters: " << iters << std::endl
                      << "Error: " << error << std::endl;
            // std::cout << "x = [ ";
            // for (auto el : x) {
            //     std::cout << el << ", ";
            // }
            // std::cout << "]\n";
#endif
        } catch(std::runtime_error &e) {
            std::cout << "caught exception: " << e.what() << std::endl;
        }
    }
}


#include "../benchmarks_general.h"

#define BLAZE_DEFAULT_ALIGNMENT_FLAG blaze::unaligned
#define BLAZE_DEFAULT_PADDING_FLAG blaze::unpadded

#include <Fastor/Fastor.h>
#include <Eigen/Core>
#include <blaze/math/StaticMatrix.h>

template<typename T, size_t M, size_t N>
void benchmark_transpose_eigen() {
    using namespace Eigen;
    Matrix<T,M,N,RowMajor> a;
    a.setConstant(3);
    Eigen::Matrix<T,N,M,RowMajor> c = a.transpose();
    benchmarks_general::unused(c);
}

template<typename T, size_t M, size_t N>
void benchmark_transpose_blaze() {
    using namespace blaze;
    StaticMatrix<T,M,N,rowMajor> a(3);
    StaticMatrix<T,N,M,rowMajor> c = trans(a);
    benchmarks_general::unused(c);
}

template<typename T, size_t M, size_t N>
void benchmark_transpose_fastor() {
    using namespace Fastor;
    Tensor<T,M,N> a(3);
    Tensor<T,N,M> c = transpose(a);
    benchmarks_general::unused(c);
}

template<typename T, size_t M>
void benchmark_run() {

    using benchmarks_general::println;
    using benchmarks_general::rtimeit;

    println("Testing size (M, N)", M, M,'\n');

    double etime = rtimeit(static_cast<void (*)()>(&benchmark_transpose_eigen<T,M,M>));
    double btime = rtimeit(static_cast<void (*)()>(&benchmark_transpose_blaze<T,M,M>));
    double ftime = rtimeit(static_cast<void (*)()>(&benchmark_transpose_fastor<T,M,M>));

    println("Elapsed time -> Eigen, Blaze, Fastor\n", etime, btime, ftime,'\n');
}

template<size_t step, size_t from, size_t to>
struct benchmark_generate {
    template<typename T>
    static inline void generate() {
        benchmark_run<T,from>();
        benchmark_generate<step,from+step,to>::template generate<T>();
    }
};
template<size_t step, size_t from>
struct benchmark_generate<step,from,from> {
    template<typename T>
    static inline void generate() {
        benchmark_run<T,from>();
    }
};

int main () {

#ifdef RUN_SINGLE
    benchmark_generate<8,8,128>::template generate<float>();
#else
    benchmark_generate<8,8,128>::template generate<double>();
#endif

    return 0;
}


#include <ros/ros.h>
#include <iostream>
#include <race/enc_values.h>
#include <cmath>
#include <boost/shared_ptr.hpp>
#include <tf/transform_broadcaster.h>

using namespace std;

struct loc {
	float xpos;
	float ypos;
	float yaw;
	loc() {}
	loc(float _xpos, float _ypos, float _yaw) : xpos(_xpos), ypos(_ypos), yaw(_yaw) {}
	loc& operator = (const loc &l) {
		xpos = l.xpos;
		ypos = l.ypos;
		yaw = l.yaw;
	}
};

bool state = false;
loc pre_loc;
loc cur_loc;
int pre_enc_val;
int cur_enc_val;
float yaw_ = 0;
boost::shared_ptr<tf::TransformBroadcaster> tf_pub_;
ros::Publisher odom_pub_;

void encoderCallback(const race::enc_values::ConstPtr& msg) {
	cur_enc_val = msg->enc_val;
	int cur_steering_angle = msg->steering;

	if(!state) {
		state = true;
		pre_enc_val = cur_enc_val;
	}

	ros::Duration dt = ;

	yaw_ += (cur_enc_val - pre_enc_val)/dt;
	float r = (cur_enc_val - pre_enc_val)/yaw;
	float y_ = r - r*cos(yaw);
	float x_ = r*sin(yaw);

	pre_enc_val = cur_enc_val;

	nav_msgs::Odometry::Ptr odom(new nav_msgs::Odometry);
	odom->pose.pose.position.x = x_;
	odom->pose.pose.position.y = y_;
	odom->pose.pose.orientation.x = 0.0;
	odom->pose.pose.orientation.y = 0.0;
	odom->pose.pose.orientation.z = sin(yaw_/2.0);
	odom->pose.pose.orientation.w = cos(yaw_/2.0);

	odom->pose.covariance[0]  = 0.2;
	odom->pose.covariance[7]  = 0.2;
	odom->pose.covariance[35] = 0.4;

	odom->twist.twist.linear.x = (cur_enc_val - pre_enc_val)/dt;
	odom->twist.twist.linear.y = 0.0;
	odom->twist.twist.angular.z = (cur_enc_val - pre_enc_val)/dt;

}

int main(int argc, char** argv) {
	ros::init(argc, argv, "encoder_to_odom_node");
	ros::NodeHandle nh;

	ros::Subscriber encoder_value_sub = nh.subscribe("encoder_value", 10, encoderCallback);
	ros::spin();
}


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 6/16/2020, 5:44:32 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <climits>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/integer/common_factor_rt.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::integer::gcd; // for C++14 or for cpp_int
using boost::integer::lcm; // for C++14 or for cpp_int
using boost::multiprecision::cpp_int;
using ll = long long;
using ld = long double;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1'000'000'007LL};
// constexpr ll MOD{998'244'353LL}; // be careful
constexpr ll MAX_SIZE{3'000'010LL};
// constexpr ll MAX_SIZE{30'000'010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
bool ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
    return true;
  }
  return false;
}
template <typename T>
bool ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
    return true;
  }
  return false;
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(Mint const &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(Mint const &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(Mint const &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(Mint const &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(Mint const &a) const { return Mint(*this) += a; }
  Mint operator-(Mint const &a) const { return Mint(*this) -= a; }
  Mint operator*(Mint const &a) const { return Mint(*this) *= a; }
  Mint operator/(Mint const &a) const { return Mint(*this) /= a; }
  bool operator<(Mint const &a) const { return x < a.x; }
  bool operator<=(Mint const &a) const { return x <= a.x; }
  bool operator>(Mint const &a) const { return x > a.x; }
  bool operator>=(Mint const &a) const { return x >= a.x; }
  bool operator==(Mint const &a) const { return x == a.x; }
  bool operator!=(Mint const &a) const { return !(*this == a); }
  Mint power(ll N) const
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, Mint<MOD> const &rhs) { return rhs + lhs; }
template <ll MOD>
Mint<MOD> operator-(ll lhs, Mint<MOD> const &rhs) { return -rhs + lhs; }
template <ll MOD>
Mint<MOD> operator*(ll lhs, Mint<MOD> const &rhs) { return rhs * lhs; }
template <ll MOD>
Mint<MOD> operator/(ll lhs, Mint<MOD> const &rhs) { return Mint<MOD>{lhs} / rhs; }
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a) { return stream >> a.x; }
template <ll MOD>
ostream &operator<<(ostream &stream, Mint<MOD> const &a) { return stream << a.x; }
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i{2LL}; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i{1LL}; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
// ----- for C++17 -----
template <typename T, typename enable_if<is_integral<T>::value>::type * = nullptr>
size_t popcount(T x) { return bitset<64>(x).count(); }
size_t popcount(string const &S) { return bitset<200010>{S}.count(); }
// ----- Infty -----
template <typename T>
constexpr T Infty() { return numeric_limits<T>::max(); }
template <typename T>
constexpr T mInfty() { return numeric_limits<T>::min(); }
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1'000'000'000'000'010LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- ReadGraph -----
// Referring to ymatsux-san's source code: https://atcoder.jp/contests/abc138/submissions/7016619

struct Edge
{
  int src, dst, id;
  // ll cost;
  Edge() {}
  Edge(int src, int dst, int id) : src{src}, dst{dst}, id{id} {}
  // Edge(int src, int dst, ll cost) : src{src}, dst{dst}, cost{cost} {}

  void added_edge(vector<vector<Edge>> &V)
  {
    V[src].push_back(*this);
  }

  void added_rev(vector<vector<Edge>> &V)
  {
    V[dst].push_back(rev());
  }

  Edge rev()
  {
    Edge edge{*this};
    swap(edge.src, edge.dst);
    return edge;
  }
};

tuple<vector<vector<Edge>>, vector<Edge>> ReadGraphWithEdges(int N, int M, bool is_undirected = true, bool is_one_indexed = true)
{
  vector<vector<Edge>> V(N);
  vector<Edge> E(M);
  for (auto i = 0; i < M; ++i)
  {
    int v, w;
    cin >> v >> w;
    if (is_one_indexed)
    {
      --v;
      --w;
    }
    Edge edge{v, w, i};
    edge.added_edge(V);
    if (is_undirected)
    {
      edge.added_rev(V);
    }
    E.push_back(edge);
  }
  return make_tuple(V, E);
}

vector<vector<Edge>> ReadGraph(int N, int M, bool is_undirected = true, bool is_one_indexed = true)
{
  return get<0>(ReadGraphWithEdges(N, M, is_undirected, is_one_indexed));
}

tuple<vector<vector<Edge>>, vector<Edge>> ReadTreeWithEdges(int N)
{
  return ReadGraphWithEdges(N, N - 1);
}

vector<vector<Edge>> ReadTree(int N)
{
  return ReadGraph(N, N - 1);
}

// ----- Solve -----

using Info = tuple<int, int>;

class Solve
{
  int N, M;
  vector<vector<Edge>> V;
  vector<Info> W;
  vector<ll> P;

public:
  Solve(int N, int M, vector<vector<Edge>> const &V) : N{N}, M{M}, V{V}, W(M), P(M)
  {
    for (auto i{0}; i < M; ++i)
    {
      cin >> get<0>(W[i]) >> get<1>(W[i]);
      get<0>(W[i])--;
      get<1>(W[i])--;
    }
  }

  void flush()
  {
    for (auto i{0}; i < M; ++i)
    {
      P[i] = path(get<0>(W[i]), get<1>(W[i]));
#if DEBUG == 1
      cerr << "P[" << i << "] = " << P[i] << endl;
#endif
    }
    ll ans{0};
    for (auto i{0}; i < (1 << M); ++i)
    {
      if (popcount(i) & 1)
      {
        ans -= mask_to_cnt(i);
      }
      else
      {
        ans += mask_to_cnt(i);
      }
    }
    cout << ans << endl;
  }

private:
  ll mask_to_cnt(int mask)
  {
    return 1LL << (N - 1 - popcount(mask_to_path(mask)));
  }

  ll mask_to_path(int mask)
  {
    ll ans{0};
    for (auto i{0}; i < M; ++i)
    {
      if (mask >> i & 1)
      {
        ans |= P[i];
      }
    }
    return ans;
  }

  ll path(int src, int dst)
  {
    vector<int> parents(N, -1), ids(N, -1);
    dfs(src, parents, ids);
    ll ans{0};
    while (dst != src)
    {
      ans |= 1LL << ids[dst];
      dst = parents[dst];
    }
    return ans;
  }

  void dfs(int src, vector<int> &parents, vector<int> &ids)
  {
    for (auto e : V[src])
    {
      if (parents[e.src] == e.dst)
      {
        continue;
      }
      parents[e.dst] = e.src;
      ids[e.dst] = e.id;
      dfs(e.dst, parents, ids);
    }
  }
};

// ----- main() -----

int main()
{
  int N;
  cin >> N;
  auto V{ReadTree(N)};
  int M;
  cin >> M;
  Solve solve(N, M, V);
  solve.flush();
}


/*
 * variable_selection.cpp
 *
 *  Created on: 19 Aug 2020
 *      Author: kovesarki
 */

#include <Eigen/Dense>
#include "include/monopoly.h"
#include <random>
#include <iostream>
#include <vector>
#include <string>

#include "include/tensor_serie.hh"
#include "include/correlation_tensor_serie.hh"

int main(void) {
	std::vector<std::vector<double> > signal(10000, std::vector<double>(3,0.0) );
	std::vector<std::vector<double> > background(10000, std::vector<double>(3,0.0) );

	std::random_device rd;
	std::mt19937 gen(rd());

	std::normal_distribution<> sx(0,1.01);
	std::normal_distribution<> sy(1.0,0.4);
	std::normal_distribution<> sz(0.7,2.0);

	std::normal_distribution<> bx(0.5,0.9);
	std::normal_distribution<> by(1.4,0.7);
	std::normal_distribution<> bz(0.73,2.01);

	std::vector<std::string >  variable_names(3);
	variable_names[0] = "parameter x";
	variable_names[1] = "parameter y";
	variable_names[2] = "parameter z";

	for(int i=0; i!= signal.size(); i++ ){
		signal[i][0] = sx(gen);
		signal[i][1] = sy(gen);
		signal[i][2] = sz(gen);
	}

	for(int i=0; i!= background.size(); i++ ){
		background[i][0] = bx(gen);
		background[i][1] = by(gen);
		background[i][2] = bz(gen);
	}

	tensor_serie<double> m_x(3,2);
	correlation_tensor_serie<double> signal_fourier(3,1); //number of dimensions, number of degree (linear is enough for comparison)
	for(int i=0; i!= signal.size(); i++){
		m_x.create_diad(signal[i]);
		signal_fourier.fill(m_x,1.0,1.0); //training for target y=1.0,weights=1.0
	}
	signal_fourier.normalize();

	correlation_tensor_serie<double> background_fourier(3,1); //number of dimensions, number of degree (linear is enough for comparison)
	for(int i=0; i!= background.size(); i++) {
		m_x.create_diad(background[i]);
		background_fourier.fill(m_x,1.0,1.0);
	}
	background_fourier.normalize();

	signal_fourier.cov_matrix(1);
	background_fourier.cov_matrix(1);

	double a = signal_fourier.sigma_distance(background_fourier);
	std::cout << "significance is " << a << std::endl;

	std::vector<double> sigmas(variable_names.size(), 0.0);
	std::vector<int> variable_ordering(variable_names.size(), 0.0);

	signal_fourier.variable_selection(background_fourier, sigmas, variable_ordering);

	std::cout << "Variable order, from best to worst. " << std::endl
			  << "Significance is meant for the group of best variables" << std::endl
			  << "(the variable, and all the variables above)." << std::endl;
	for(int i=0; i!= variable_ordering.size(); i++)
	{
	  std::cout << sigmas[variable_ordering[i]] << " \t" << variable_names[variable_ordering[i]] << std::endl;
	}

	std::cout << "varnames["<< variable_ordering.size()<< "] = {" << std::endl;
	for(int i=variable_ordering.size()-1; i!= -1; i--)
	{
	  std::cout << "\"" << variable_names[variable_ordering[i]] << "\"";
	  if(i!=0) {std::cout << ","; }
	  else {std::cout << std::endl << "}";}
	  std::cout << std::endl;
	}
}


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

using namespace std;
using namespace Eigen;

int main()
{
    MatrixXd mat;
    BDCSVD<MatrixXd> svd(mat.Random(5, 6), ComputeFullU | ComputeFullV);
    cout<< svd.singularValues().size();
    //cout<<svd.matrixU();
    return 0;
}


/**
 This file is part of Poisson Image Editing.

 Copyright Christoph Heindl 2015

 Poisson Image Editing is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 Poisson Image Editing is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Poisson Image Editing.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <blend/blend.h>
#include <blend/poisson_solver.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
namespace blend {

    template<class T>
    Eigen::Map< Eigen::Matrix<T, Eigen::Dynamic, 1> > mapChannels(cv::Mat &m, int y, int x) {

        typedef Eigen::Matrix<T, Eigen::Dynamic, 1> V;
        return Eigen::Map<V>(m.ptr<T>(y, x), m.channels());
    }

    void seamlessBlend(cv::InputArray first_,
                       cv::InputArray second_,
                       cv::InputArray mask_,
                       cv::OutputArray destination_)
    {

        cv::Mat first = first_.getMat();
        cv::Mat second = second_.getMat();

        destination_.create(first.size(), first.type());
        cv::Mat dst = destination_.getMat();

        // Target Laplacians are zero
        cv::Mat f(first.size(), CV_MAKE_TYPE(CV_32F, first.channels()));
        f.setTo(0);

        cv::Mat bm(first.size(), CV_8UC1);
        bm.setTo(constants::UNKNOWN);
        cv::Mat bv(first.size(), CV_MAKE_TYPE(CV_32F, first.channels()));
        bv.setTo(0);

        cv::Rect boundsSecond(0, 0, second.cols, second.rows);

        for (int y = 0; y < first.rows; ++y) {
            const uchar *maskRow = mask_.getMat().ptr<uchar>(y);
            for (int x = 0; x < first.cols; ++x) {
                const bool isBorder = (y == 0) || (x == 0) || (y == (first.rows - 1)) || (x == (first.cols - 1));
                const bool isFirst = (maskRow[x] == 255);

                if (isFirst && isBorder) {
                    bm.at<uchar>(y, x) = constants::NEUMANN_BD;
                    bv.at<float>(y, x) = 0.f;
                } else if (!isFirst && boundsSecond.contains(cv::Point(x,y))) {
                    bm.at<uchar>(y, x) = constants::DIRICHLET_BD;
                    mapChannels<float>(bv, y, x) = mapChannels<uchar>(second, y, x).cast<float>() - mapChannels<uchar>(first, y, x).cast<float>();
                } else if (!isFirst) {
                    bm.at<uchar>(y, x) = constants::DIRICHLET_BD;
                    mapChannels<float>(bv, y, x).setZero();
                }
            }
        }

        // Solve Poisson equation
        cv::Mat result;
        solvePoissonEquations(f,
                              bm,
                              bv,
                              result);

        cv::Mat fixed;
        first.convertTo(fixed, result.depth());
        fixed += result;
        fixed.convertTo(dst, dst.depth());
        second.copyTo(dst, (255 - mask_.getMat()));

    }

}


/*
 * G_q.cpp
 *
 *  Created on: 30.09.2010
 *      Author: stephaniebayer
 *
 *
 */

#include "G_q.h"

#include <NTL/ZZ.h>
NTL_CLIENT

#include <time.h>
G_q::G_q() {
	// TODO Auto-generated constructor stub

}

//Constructor creates an instance of G_q subset Z_p with order o and generator g
G_q::G_q(Mod_p gen, long o, long p){

	generator = gen;
	order = to_ZZ(o);
	mod = to_ZZ(p);
	if (gen.get_mod() != p)
		cout  << "The modular value of the generator and p are not equal" << endl;
}

//Constructor creates an instance of G_q subset Z_p with order o and generator g
G_q::G_q(Mod_p gen, long o, ZZ p){

	generator = gen;
	order = to_ZZ(o);
	mod = p;

	if (gen.get_mod() != p)
		cout  << "The modular value of the generator and p are not equal" << endl;

}

//Constructor creates an instance of G_q subset Z_p with order o and generator g
G_q::G_q(Mod_p gen, ZZ o, ZZ p){

	generator = gen;
	order = o;
	mod = p;

	if (gen.get_mod() != p)
		cout  << "The modular value of the generator and p are not equal" << endl;

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(ZZ val, long o, long p){

	generator = Mod_p(val, p);
	order = to_ZZ(o);
	mod = to_ZZ(p);

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(ZZ val, long o, ZZ p){

	generator = Mod_p(val, p);
	order = to_ZZ(o);
	mod = p;

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(ZZ val, ZZ o, ZZ p){

	generator = Mod_p(val, p);
	order = o;
	mod = p;

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(long val, long o, long p){

	generator = Mod_p(val, p);
	order = to_ZZ(o);
	mod = to_ZZ(p);

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(long val, long o, ZZ p){

	generator = Mod_p(val, p);
	order = to_ZZ(o),
	mod = p;

}

//Constructor creates an instance of G_q subset Z_p with order o and generator value val
G_q::G_q(long val,ZZ o, ZZ p){

	generator = Mod_p(val, p);
	order = o;
	mod = p;

}

//Constructor creates an instance of G_q with order o, generator gen and G_q is a subgroup if Z modulo gen.get_mod()
G_q::G_q(Mod_p gen, long o){

	generator = gen;
	order = to_ZZ(o);
	mod = gen.get_mod();
}

//Constructor creates an instance of G_q with order o, generator gen and G_q is a subgroup if Z modulo gen.get_mod()
G_q::G_q(Mod_p gen, ZZ o){

	generator = gen;
	order = o;
	mod = gen.get_mod();
}

//Constructor creates an instance of G_q  subset of Z_p with order o and searchs for the smallest generator
G_q::G_q(long o,ZZ p){

	ZZ i;
	order = to_ZZ(o);
	mod = p;
	for (i = to_ZZ(1); i < p; ++i)
	{
		if (is_generator(i))
		{
			generator = Mod_p(i,p);
			break;
		}
	}
}

//Constructor creates an instance of G_q  subset of Z_p with order o and searchs for the smallest generator
G_q::G_q(ZZ o,ZZ p){

	ZZ i,t;
	order = o;
	mod = p;
	for (i = to_ZZ(2); i < p; ++i)
	{
		t=i%100000;
		if(t==0){
			cout<<";";
		}
		if (is_generator(i))
		{
			generator = Mod_p(i,p);
			break;
		}
	}
}

//Constructor creates an instance of G_q  subset of Z_p with order o and searchs for the smallest generator
G_q::G_q(long o, long p){

	long i;
	order = to_ZZ(o);
	mod = to_ZZ(p);
	for (i = 1; i < p; ++i)
	{
		if (is_generator(i))
		{
			generator = Mod_p(i,p);
			break;
		}
	}
}

G_q::~G_q() {
	// TODO Auto-generated destructor stub
}

//return the generator
Mod_p G_q::get_gen()const{

	return generator;
}

//return the order o
ZZ G_q::get_ord()const{

	return order;
}

//return the modular value mod
ZZ G_q::get_mod()const{

	return mod;
}

//Checks if an element is a generator of the group G
bool G_q::is_generator(const Mod_p& el){
	ZZ pow;
	bool b;
	b=false;
	pow = PowerMod(el.get_val(),order,mod);
	if(pow == to_ZZ(1)& el.get_val()!=1)
	{
		b=true;
	}
	return b;
}

//Checks if an element is a generator of the group G
bool G_q::is_generator(const ZZ& x){
	ZZ pow;
	bool b;
	b=false;
	pow = PowerMod(x,order,mod);

	if(pow == to_ZZ(1) & x!=1)
	{
		b=true;

	}

	return b;
}

//Checks if an element is a generator of the group G
bool G_q::is_generator(const long& x){
	ZZ pow;
	bool b;
	pow = PowerMod(to_ZZ(x),order, mod);
	if(pow == to_ZZ(1)& x!=1)
	{
		b=true;

	}

	return b;
}

//returns the identity of the group
Mod_p G_q::identity(){

	return Mod_p(1, mod);
}

//returns a random element of the group
Mod_p G_q::random_el(){
	ZZ ran,pow;
	Mod_p temp;
	SetSeed(to_ZZ(time(0)));
	ran = RandomBnd(mod);
	temp = generator.expo(ran);

	return temp;

}

//returns a random element of the group, without setting the seed
Mod_p G_q::random_el(int c){
	ZZ ran,pow;
	Mod_p temp;
	ran = RandomBnd(order);
	temp = generator.expo(ran);

	return temp;

}

//returns an element of the group with value v
Mod_p G_q::element(ZZ v){

	return Mod_p(v,mod);

}

//returns an element of the group with value v
Mod_p G_q::element(long v){

	return Mod_p(v,mod);

}

void G_q::operator =(const G_q& H){

	generator = H.get_gen();
	order = H.get_ord();
	mod = H.get_mod();
}

//Output operator, output format is (generator value, order, modular value)
ostream& operator<<(ostream& os, const G_q G){
	return os<< "("<< G.get_gen().get_val()<< ", " << G.get_ord() <<", " << G.get_mod()<<")";
}


//
//  main.cpp
//  lpm
//
//  Created by Seong-Hwan Jun on 2019-05-08.
//

#include <iostream>
#include <string>
#include <boost/algorithm/string.hpp>
#include <boost/program_options.hpp>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include "lpm.hpp"

using namespace std;

void draw_stratified_samples(gsl_rng *random, size_t n_mc_samples, double vec[], double max)
{
    double interval = max/n_mc_samples;
    double sum = 0.0;
    for (size_t i = 0; i < n_mc_samples; i++) {
        vec[i] = gsl_ran_flat(random, sum, sum + interval);
        sum += interval;
    }
}

int main(int argc, char *argv[])
{
    namespace po = boost::program_options;
    /**
     * arguments:
     * mode: model/pg
     * seed
     * model_len: range (i.e., 2-10)
     * n_mc_samples: 30
     * n_pg_iter: 30
     * n_particles: 400
     * n_smc_iter: 100
     * n_kernel_iter: 3
     * n_mh_w_gibbs_iter: 10
     * has_passenger: True
     * swap_prob: 0.1
     * fbp_max: 0
     * bgp_max: 0.2
     * mh_proposal_sd: 0.05
     * n_threads: 8
     * use_lik_tempering: True
     * data_path: data/Experiment2/With_passengers/5/error0.05/rep0/matrix.csv
     **/

    string data_path;
    string output_path;
//    string mode;
    unsigned long seed;
    unsigned int model_len;
    //unsigned int n_mc_samples;
    unsigned int n_mcmc_iter;
//    unsigned int n_particles;
//    unsigned int n_smc_iter;
//    unsigned int n_kernel_iter;
    unsigned int n_mh_w_gibbs_iter;
    unsigned int burn_in;
    unsigned int thinning_interval;
//    unsigned int n_mc_threads;
//    unsigned int n_smc_threads;
//    bool use_lik_tempering;
    double fbp_max, bgp_max;
    bool has_passenger;
    double swap_prob;
    double mh_proposal_sd;
    double prior_passenger_prob;

    po::options_description desc("Program options");
    desc.add_options()
    ("help", "Put a help message here.")
    ("data_path,d", po::value<string>(&data_path)->required(), "Specify path to the data.")
    ("output_path,o", po::value<string>(&output_path)->required(), "Specify output path.")
//    ("mode,m", po::value<string>(&mode)->required(), "Specify mode: model for model selection and pg for particle Gibbs.")
    ("seed,s", po::value<unsigned long>(&seed)->default_value(1), "Specify random seed.")
    ("model_len,l", po::value<unsigned int>(&model_len)->required(), "Specify model length.")
    //("n_mc_samples,M", po::value<unsigned int>(&n_mc_samples), "Specify number of MC samples to use for model selection.")
    ("n_mcmc_iter,p", po::value<unsigned int>(&n_mcmc_iter), "Specify number of iterations for PG/MCMC.")
    //("n_particles,P", po::value<unsigned int>(&n_particles)->required(), "Specify number of particles to use.")
    //("n_smc_iter,S", po::value<unsigned int>(&n_smc_iter)->required(), "Specify number of SMC iterations.")
    //("n_kernel_iter,k", po::value<unsigned int>(&n_kernel_iter)->default_value(5), "Specify number of iterations of MCMC kernels to apply within a SMC move. Specifying 0 reduces to random walk move.")
    ("n_mh_w_gibbs_iter,G", po::value<unsigned int>(&n_mh_w_gibbs_iter)->default_value(10), "Specify number of iterations of MH within Gibbs for inferring the parameters within PG.")
    //("n_mc_threads,t", po::value<unsigned int>(&n_mc_threads)->default_value(1), "Specify number of threads to use for parallel processing of Monte Carlo samples.")
    //("n_smc_threads", po::value<unsigned int>(&n_smc_threads)->default_value(1), "Specify number of threads to use for SMC algorithm.")
    //("use_lik_tempering,T", po::value<bool>(&use_lik_tempering)->default_value(true), "Specify whether to use likelihood tempering.")
    ("fbp_max,f", po::value<double>(&fbp_max)->default_value(0), "Specify max value for flip-back probability. Specify 0 here to use unified error probability.")
    ("bgp_max,b", po::value<double>(&bgp_max)->default_value(0.2), "Specify max value for background mutation probability. Specifying 0 for fbp_max to interpret this quantity as an error probability.")
    ("has_passenger", po::value<bool>(&has_passenger)->default_value(true), "Specify whether to detect passenger genes. 0: false, 1: true.")
    ("swap_prob", po::value<double>(&swap_prob)->default_value(0.2), "Specify pathway swap probability for SMC move.")
    ("mh_proposal_sd", po::value<double>(&mh_proposal_sd)->default_value(0.05), "Specify standard deviation of Gaussian random walk proposal within MHwGibbs for inferring the parameters (for mode = pg).")
    ("burn_in", po::value<unsigned int>(&burn_in)->default_value(0), "Specify burn in for MCMC.")
    ("thinning_interval", po::value<unsigned int>(&thinning_interval)->default_value(1), "Specify thinning interval for MCMC.")
    ("prior_passenger_prob", po::value<double>(&prior_passenger_prob)->default_value(0.95), "Specify prior probability of a gene being a passenger.")
    ;

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

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

    run_mcmc(seed, data_path.c_str(), output_path.c_str(), model_len, n_mcmc_iter, n_mh_w_gibbs_iter, thinning_interval, burn_in, has_passenger, swap_prob, fbp_max, bgp_max, mh_proposal_sd, prior_passenger_prob);

    //if (mode == "mcmc") {
//        run_mcmc(seed, data_path.c_str(), output_path.c_str(), model_len, n_mcmc_iter, n_mh_w_gibbs_iter, thinning_interval, burn_in, has_passenger, swap_prob, fbp_max, bgp_max, mh_proposal_sd, prior_passenger_prob);
//    } else {
//        cerr << "Unknown mode: " << mode << "." << endl;
//    }

//    if (mode == "pg") {
//        if (!vm.count("n_pg_iter")) {
//            cerr << "Please specify number of PG iterations." << endl;
//        }
//        run_pg(seed, data_path.c_str(), output_path.c_str(), model_len, n_mcmc_iter, n_particles, n_smc_iter, n_kernel_iter, n_mh_w_gibbs_iter, has_passenger, swap_prob, fbp_max, bgp_max, mh_proposal_sd, n_smc_threads);
//    } else if (mode == "model") {
//        if (!vm.count("n_mc_samples")) {
//            cerr << "Please specify number of Monte Carlo samples." << endl;
//        }
//        // sample fbps and bgps via stratified sampling
//        double *fbps = new double[n_mc_samples];
//        double *bgps = new double[n_mc_samples];
//        gsl_rng *random = generate_random_object(seed);
//        draw_stratified_samples(random, n_mc_samples, bgps, bgp_max);
//        if (fbp_max == 0.0) {
//            std::copy(bgps, bgps + n_mc_samples, fbps);
//        }
//        unsigned long new_seed = gsl_rng_get(random);
//        model_selection(new_seed, data_path.c_str(), output_path.c_str(), model_len, n_mc_samples, n_particles, n_smc_iter, n_kernel_iter, has_passenger, swap_prob, fbps, bgps, n_mc_threads, n_smc_threads, use_lik_tempering);
//        gsl_rng_free(random);
//    } else if (mode == "mcmc") {
//        run_mcmc(seed, data_path.c_str(), output_path.c_str(), model_len, n_mcmc_iter, n_mh_w_gibbs_iter, thinning_interval, has_passenger, swap_prob, fbp_max, bgp_max, mh_proposal_sd, prior_passenger_prob);
//    } else {
//        cerr << "Unknown mode: " << mode << "." << endl;
//    }
    return 0;
}


#include "hoNDHarrWavelet.h"
#include "hoNDRedundantWavelet.h"
#include "hoNDArray_math.h"
#include <gtest/gtest.h>
#include <boost/random.hpp>

using namespace Gadgetron;
using testing::Types;

template<typename REAL> class hoNDWavelet_test : public ::testing::Test
{
protected:
    virtual void SetUp()
    {
        boost::random::mt19937 rng;
        boost::random::uniform_real_distribution<REAL> uni(0,1);
        std::vector<size_t > dimensions(3,128);

        Array = hoNDArray< std::complex<REAL> >(dimensions);
        std::complex<REAL>* data = Array.get_data_ptr();

        for (size_t i = 0; i < Array.get_number_of_elements(); i++)
            data[i] = std::complex<REAL>(uni(rng), uni(rng));
    }

    hoNDArray< std::complex<REAL> > Array;
};

typedef Types<float, double> realImplementations;
TYPED_TEST_SUITE(hoNDWavelet_test, realImplementations);

TYPED_TEST(hoNDWavelet_test, hoNDHarrWaveletTest1D)
{
    Gadgetron::hoNDHarrWavelet< std::complex<TypeParam> > wav;

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 1;
    size_t level = 3;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v =  Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}

TYPED_TEST(hoNDWavelet_test, hoNDHarrWaveletTest2D)
{
    Gadgetron::hoNDHarrWavelet< std::complex<TypeParam> > wav;

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 2;
    size_t level = 2;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v = Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}

TYPED_TEST(hoNDWavelet_test, hoNDHarrWaveletTest3D)
{
    Gadgetron::hoNDHarrWavelet< std::complex<TypeParam> > wav;

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 3;
    size_t level = 1;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v = Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}

TYPED_TEST(hoNDWavelet_test, hoNDRedundantWaveletTest1D)
{
    Gadgetron::hoNDRedundantWavelet< std::complex<TypeParam> > wav;
    wav.compute_wavelet_filter("db2");

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 1;
    size_t level = 2;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v = Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}

TYPED_TEST(hoNDWavelet_test, hoNDRedundantWaveletTest2D)
{
    Gadgetron::hoNDRedundantWavelet< std::complex<TypeParam> > wav;
    wav.compute_wavelet_filter("db2");

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 2;
    size_t level = 3;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v = Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}

TYPED_TEST(hoNDWavelet_test, hoNDRedundantWaveletTest3D)
{
    Gadgetron::hoNDRedundantWavelet< std::complex<TypeParam> > wav;
    wav.compute_wavelet_filter("db2");

    hoNDArray< std::complex<TypeParam> > r, rr, diff;

    size_t WavDim = 3;
    size_t level = 2;

    wav.transform(this->Array, r, WavDim, level, true);
    wav.transform(r, rr, WavDim, level, false);

    Gadgetron::subtract(this->Array, rr, diff);

    TypeParam v = Gadgetron::nrm2(diff);

    EXPECT_NEAR(v, 0, 0.001);
}


#include <iostream>
#include <fstream>
#include <vector>

#include <spherepix/image.h>
#include <spherepix/camera.h>
#include <spherepix/pixelation.h>
#include <Eigen/Dense>

void numericsTest() {
    Eigen::Vector2f V_total(-1.39383e-05, 7.17674e-06);
    Eigen::Vector2f C_total(-7.66636e-08,  1.24335e-07);
    float M = 1.0f;

    Eigen::Vector2f beta_acc_p = (V_total - C_total) / M;

    std::cout << "beta_acc_p: " << beta_acc_p.transpose() << std::endl;
}

void testMin() {
    std::vector<int> v {7, 8, 5, 2, 0, 9, 10};
    int childrenIndex = std::min_element(v.begin(), v.end()) - v.begin();
    std::cout << "min index: " << childrenIndex << std::endl;
    std::cout << "element: " << v[childrenIndex] << std::endl;
}

void tesCamera() {

    std::cout << "camera test" << std::endl;
    spherepix::PinholeCamera cam(35, 480, 640, 18, 18);

    std::cout << "height: " << cam.height() << std::endl;
    std::cout << "width: " << cam.width() << std::endl;

    std::cout << "camera test: finished" << std::endl;
}

void testSpringDynamics() {
    int N = 512;
    spherepix::Image<float> input = spherepix::createFace_equiangular(N);
    spherepix::Image<float> face = spherepix::regularizeCoordinates(input, 0.0f, 1000);

    // spherepix::Image<float> win = input.subImage(0, 0, 3, 3);
    // spherepix::Image<float> cop(3,3,3);
    // cop.copyFrom(win);
    // for(int i =0; i < win.length(); ++i) {
    //     std::cout << i << ": " << win[i] << " : " << cop[i] << std::endl;
    // }

    face.save("pix512_2.bin");

    std::cout << "ALL GOOD" << std::endl;
}

int main(int argc, char const *argv[])
{
    // std::cout << "imageTest: start" << std::endl;

    // spherepix::Image<float> img {2, 5, 3};
    // std::cout << "width: " << img.width() << std::endl;
    // std::cout << "height: " << img.height() << std::endl;
    // std::cout << "depth: " << img.depth() << std::endl;
    // std::cout << "length: " << img.length() << std::endl;
    // std::cout << "pitch: " << img.pitch() << std::endl;

    // for (int i = 0; i < img.length(); ++i) {
    //     img[i] = i;
    //     std::cout << "[" << i << "]: " << img[i] << std::endl;
    // }

    // for (int r = 0; r < img.height(); ++ r) {
    //     for (int c = 0; c < img.width(); ++ c) {
    //         std::cout << "[" << r << ", " << c << "]: " << img(r, c, 0) << std::endl;
    //     }
    // }

    // float* data = new float[100];
    // spherepix::Image<float> imgExt {10, 10, 1, data};

    numericsTest();
    testMin();
    tesCamera();
    // testSpringDynamics();

    spherepix::Pixelation pix = spherepix::PixelationFactory::createPixelation(spherepix::PixelationMode::MODE_1, 64, 3);
    spherepix::SphericalImage<float> img(pix, 3);

    std::cout << "sphImg face height: " << img.faceHeight() << std::endl;

    spherepix::Image<float>* face = &img[0];
    std::cout << "face depth: " << face->depth() << std::endl;

    return 0;
}


#include "CEGO/CEGO.hpp"
#include "CEGO/minimizers.hpp"
#include <Eigen/Dense>

// autodiff include
#include "Eigen/Dense"
#include <autodiff/forward.hpp>
#include <autodiff/forward/eigen.hpp>

std::atomic_size_t Ncalls(0);

constexpr double Tt = 273.16;
constexpr double Tc = 647.096;

const std::vector<double> b = { 1.99274064, 1.09965342, -0.510839303, -1.75493479, -45.5170352, -6.746944503e5 };
const std::vector<double> c = { 1.0 / 3, 2.0 / 3, 5.0 / 3, 16.0 / 3, 43.0/ 3, 110.0/ 3 };

template <class T, class... Ts> struct is_any : std::disjunction<std::is_same<T, Ts>...> {};

template <typename T, typename B, typename C>
auto evaluate_RHS(const T &theta, const B &b, const C &c) { // theta = 1-T/Tc
    T o = 1.0 + 0*theta;
    for (auto i = 0; i < b.size(); ++i) {
        o += b[i] * pow(theta, c[i]);
    }
    return o;
}

auto evaluate_RHS(const CEGO::EArray<double> &theta, const CEGO::EArray<std::complex<double>> &b, const CEGO::EArray<std::complex<double>> &c) { // theta = 1-T/Tc
    CEGO::EArray<std::complex<double>> o = 1.0 + 0*theta;
    for (auto i = 0; i < b.size(); ++i) {
        o += b[i] * theta.pow(c[i]);
    }
    return o;
}

template <typename Coeffs, typename Theta>
auto eval_fit(const Coeffs &coeffs, const Theta &theta) {
    if constexpr (is_any<Coeffs, std::vector<double>, std::vector<std::complex<double>>, std::vector<CEGO::numberish>>::value) {
        Coeffs b(coeffs.begin(), coeffs.begin() + coeffs.size() / 2);
        Coeffs c(coeffs.begin() + coeffs.size() / 2, coeffs.end());
        return evaluate_RHS(theta, b, c);
    }
    else {
        Coeffs b = coeffs.head(coeffs.size() / 2),
            c = coeffs.tail(coeffs.size() / 2);
        return evaluate_RHS(theta, b, c);
    }
}

template <typename Coeffs, typename Theta, typename Yval>
auto objective(const Coeffs& coeffs, const Theta &theta, const Yval &yval) {
    return (eval_fit(coeffs, theta) - yval).square().sum();
};

template <typename Theta, typename Yval>
auto objective(const CEGO::AbstractIndividual* pind, const Theta& theta, const Yval& yval) {
    return objective(pind->get_coeffs_ArrayXd(), theta, yval);
};

struct Inputs{
    std::string root = "";
    std::size_t parallel_threads = 1;
    std::vector<std::size_t> Nlayersvec = { 1 };
    std::size_t i = 0;
    std::size_t gradmin_mod = 5;
    std::size_t Nmax_gradient = 5;
};

inline void to_json(nlohmann::json& j, const Inputs& f) {
    j = nlohmann::json{ { "root", f.root },{ "parallel_threads", f.parallel_threads },{"i",f.i},{"gradmin_mod",f.gradmin_mod},{"Nmax_gradient",f.Nmax_gradient} };
}

inline void from_json(const nlohmann::json& j, Inputs& f) {
    j.at("root").get_to(f.root);
    j.at("parallel_threads").get_to(f.parallel_threads);
    j.at("i").get_to(f.i);
    j.at("gradmin_mod").get_to(f.gradmin_mod);
    j.at("Nmax_gradient").get_to(f.Nmax_gradient);
}

int get_env_int(const std::string& var, int def) {
    try {
        char* s = std::getenv(var.c_str());
        if (s == nullptr) {
            return def;
        }
        if (strlen(s) == 0) {
            return def;
        }
        return std::stoi(s, nullptr);
    }
    catch (...) {
        return def;
    }
}

void do_one(Inputs& inputs)
{
    std::srand((unsigned int)time(0));

    const CEGO::EArray<double> theta = 1 - CEGO::EArray<double>::LinSpaced(1000, Tt, Tc) / Tc;
    const CEGO::EArray<double> yval = evaluate_RHS(theta, b, c);

    // Construct the bounds
    std::vector<CEGO::Bound> bounds;
    for (auto i = 0; i < b.size(); ++i) {
        double v0 = 0.1 * b[i], v1 = 10 * b[i];
        bounds.push_back(CEGO::Bound(std::make_pair(std::min(v0, v1), std::max(v0, v1))));
    }
    for (auto i = 0; i < c.size(); ++i) {
        double v0 = 0.1 * c[i], v1 = 4 * c[i];
        bounds.push_back(CEGO::Bound(std::make_pair(std::min(v0, v1), std::max(v0, v1))));
    }

    CEGO::CostFunction<CEGO::numberish> cost_wrapper = [&theta, &yval](const CEGO::AbstractIndividual* pind) {return objective(pind, theta, yval); };
    auto Npop_size = 10*bounds.size();
    auto Nlayers = 3;
    auto layers = CEGO::Layers<CEGO::numberish>(cost_wrapper, bounds.size(), Npop_size, Nlayers, 5);
    layers.parallel = (inputs.parallel_threads > 1);
    layers.parallel_threads = inputs.parallel_threads;
    layers.set_builtin_evolver(CEGO::BuiltinEvolvers::differential_evolution);
    layers.set_bounds(bounds);
    auto f = [&theta, &yval](const CEGO::EArray<double>& c) {return objective(c, theta, yval); };
    auto f2 = [&theta, &yval](const CEGO::EArray<std::complex< double >>& c) {return objective(c, theta, yval); };
    layers.add_gradient(f, f2);

    auto flags = layers.get_evolver_flags();
    flags["Nelite"] = 3;
    flags["Fmin"] = 0.1;
    flags["Fmax"] = 1.0;
    flags["CR"] = 1;
    layers.set_evolver_flags(flags);

    std::vector<double> best_costs;
    std::vector<std::vector<double> > objs;
    double VTR = 1e-16, best_cost = 999999.0;
    auto startTime = std::chrono::system_clock::now();
    for (auto counter = 0; counter < 5000; ++counter) {
        layers.do_generation();

        if (counter % inputs.gradmin_mod == 0 && counter > 0) {
            layers.gradient_minimizer();
        }

        // For the overall best result, print it, and write JSON to file
        auto [best_layer, best_coeffs] = layers.get_best();
        if (counter % 50 == 0) {
            std::cout << counter << ": best: " << best_cost << "\n ";
            std::cout << best_coeffs << "\n";
        }
        if (best_cost < VTR){ break; }
    }
    auto endTime = std::chrono::system_clock::now();
    double elap = std::chrono::duration<double>(endTime - startTime).count();
    std::cout << "run:" << elap << " s\n";
    std::cout << "NFE:" << Ncalls << std::endl;
}

int main() {
    Inputs in;
    in.root = "shaped-";
    in.Nlayersvec = {1};
    auto Nrepeats = get_env_int("NREPEATS", 1);
    in.gradmin_mod = get_env_int("GRADMOD", 100);
    in.parallel_threads = get_env_int("NTHREADS", 5);
    in.Nmax_gradient = get_env_int("NMAX_gradient", 5);
    nlohmann::json j = in;
    std::cout << j << std::endl;
    for (in.i = 0; in.i < Nrepeats; ++in.i) {
        do_one(in);
    }
}


//
//  Copyright (c) 2018, Cem Bassoy, cem.bassoy@gmail.com
//  Copyright (c) 2019, Amit Singh, amitsingh19975@gmail.com
//
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
//  The authors gratefully acknowledge the support of
//  Google and Fraunhofer IOSB, Ettlingen, Germany
//

#include <iostream>
#include <algorithm>
#include <vector>

#include <boost/numeric/ublas/tensor/multiplication.hpp>
#include <boost/numeric/ublas/tensor/extents.hpp>
#include "utility.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE (test_tensor_contraction)

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

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

struct fixture
{
  using extents_t = boost::numeric::ublas::extents<>;
  const std::vector<extents_t> extents =
  {
    extents_t{1,1}, // 1
    extents_t{1,2}, // 2
    extents_t{2,1}, // 3
    extents_t{2,3}, // 4
    extents_t{5,4}, // 5
    extents_t{2,3,1}, // 6
    extents_t{4,1,3}, // 7
    extents_t{1,2,3}, // 8
    extents_t{4,2,3}, // 9
    extents_t{4,2,3,5} // 10
  };
};

BOOST_FIXTURE_TEST_CASE_TEMPLATE(test_tensor_mtv, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;
  using extents_base_t = typename extents_t::base_type;

  for(auto const& na : extents) {

    if(ublas::size(na) > 2)
      continue;

    auto a = vector_t(ublas::product(na), value_t{2});
    auto wa = ublas::to_strides(na,layout_t{});
    for(auto m = std::size_t{0}; m < ublas::size(na); ++m){
      auto nb = extents_t {na[m],std::size_t{1}};
      auto wb = ublas::to_strides(nb,layout_t{});
      auto b  = vector_t  (ublas::product(nb), value_t{1} );

      auto nc_base = extents_base_t(std::max(std::size_t{ublas::size(na)-1u}, std::size_t{2}), 1);

      for(auto i = 0ul, j = 0ul; i < ublas::size(na); ++i)
        if(i != m)
          nc_base[j++] = na[i];

      auto nc = extents_t (nc_base);
      auto wc = ublas::to_strides(nc,layout_t{});
      auto c  = vector_t  (ublas::product(nc), value_t{0});

      ublas::detail::recursive::mtv(
        m,
        c.data(), nc.data(), wc.data(),
        a.data(), na.data(), wa.data(),
        b.data());

      auto v = value_t(na[m]);
      BOOST_CHECK(std::equal(c.begin(),c.end(),a.begin(), [v](auto cc, auto aa){return cc == v*aa;}));

//      for(auto i = 0u; i < c.size(); ++i)
//        BOOST_CHECK_EQUAL( c[i] , value_t( static_cast< inner_type_t<value_t> >(na[m]) ) * a[i] );

    }
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_mtm, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;

  for(auto const& na : extents) {

    if(ublas::size(na) != 2)
      continue;

    auto a  = vector_t  (ublas::product(na), value_t{2});
    auto wa = ublas::to_strides(na,layout_t{});

    auto nb = extents_t {na[1],na[0]};
    auto wb = ublas::to_strides(nb,layout_t{});
    auto b  = vector_t  (ublas::product(nb), value_t{1} );

    auto nc = extents_t {na[0],nb[1]};
    auto wc = ublas::to_strides(nc,layout_t{});
    auto c  = vector_t  (ublas::product(nc));

    ublas::detail::recursive::mtm(
      c.data(), nc.data(), wc.data(),
      a.data(), na.data(), wa.data(),
      b.data(), nb.data(), wb.data());

    auto v = value_t(na[1])*a[0];
    BOOST_CHECK(std::all_of(c.begin(),c.end(), [v](auto cc){return cc == v;}));

//    for(auto i = 0u; i < c.size(); ++i)
//      BOOST_CHECK_EQUAL( c[i] , value_t( static_cast< inner_type_t<value_t> >(na[1]) ) * a[0] );

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ttv, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;
  using extents_base_t = typename extents_t::base_type;

  for(auto const& na : extents) {

    auto a = vector_t(ublas::product(na), value_t{2});
    auto wa = ublas::to_strides(na,layout_t{});
    for(auto m = std::size_t{0}; m < ublas::size(na); ++m){
      auto b  = vector_t  (na[m], value_t{1} );
      auto nb = extents_t {na[m],1};
      auto wb = ublas::to_strides(nb,layout_t{});

      auto nc_base = extents_base_t(std::max(std::size_t{ublas::size(na)-1u}, std::size_t{2}),1);

      for(auto i = 0ul, j = 0ul; i < ublas::size(na); ++i)
        if(i != m)
          nc_base[j++] = na[i];

      auto nc = extents_t (nc_base);
      auto wc = ublas::to_strides(nc,layout_t{});
      auto c  = vector_t  (ublas::product(nc), value_t{0});

      ublas::ttv(m+1, ublas::size(na),
                 c.data(), nc.data(), wc.data(),
                 a.data(), na.data(), wa.data(),
                 b.data(), nb.data(), wb.data());

      auto v = value_t(na[m]);
      BOOST_CHECK(std::equal(c.begin(),c.end(),a.begin(), [v](auto cc, auto aa){return cc == v*aa;}));

//      for(auto i = 0u; i < c.size(); ++i)
//        BOOST_CHECK_EQUAL( c[i] , value_t(na[m]) * a[i] );

    }
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ttm, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;

  for(auto const& na : extents) {

    auto a = vector_t(ublas::product(na), value_t{2});
    auto wa = ublas::to_strides(na,layout_t{});
    for(auto m = std::size_t{0}; m < ublas::size(na); ++m){
      const auto nb = extents_t {na[m], na[m] };
      const auto b  = vector_t  (ublas::product(nb), value_t{1} );
      const auto wb = ublas::to_strides(nb,layout_t{});

      const auto& nc = na;
      const auto wc = ublas::to_strides(nc,layout_t{});
      auto c  = vector_t  (ublas::product(nc), value_t{0});

      ublas::ttm(m+1, ublas::size(na),
                 c.data(), nc.data(), wc.data(),
                 a.data(), na.data(), wa.data(),
                 b.data(), nb.data(), wb.data());

      auto v = value_t(na[m]);
      BOOST_CHECK(std::equal(c.begin(),c.end(),a.begin(), [v](auto cc, auto aa){return cc == v*aa;}));

//      for(auto i = 0u; i < c.size(); ++i)
//        BOOST_CHECK_EQUAL( c[i] , value_t( static_cast< inner_type_t<value_t> >(na[m]) ) * a[i] );

    }
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ttt_permutation, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;
  using extents_base_t = typename extents_t::base_type;

  auto compute_factorial = [](auto const& p){
    auto f = 1ul;
    for(auto i = 1u; i <= p; ++i)
      f *= i;
    return f;
  };

  auto compute_inverse_permutation = [](auto const& pi){
    auto pi_inv = pi;
    for(auto j = 0u; j < pi.size(); ++j)
      pi_inv[pi[j]-1] = j+1;
    return pi_inv;
  };

  auto permute_extents = [](auto const& pi, auto const& na){
    auto nb_base = na.base();
    assert(pi.size() == ublas::size(na));
    for(auto j = 0u; j < pi.size(); ++j)
      nb_base[j] = na[pi[j]-1];
    return extents_t(nb_base);
  };

  // left-hand and right-hand side have the
  // the same number of elements

  // computing the inner product with
  // different permutation tuples for
  // right-hand side

  for(auto const& na : extents) {

    auto wa = ublas::to_strides(na,layout_t{});
    auto a  = vector_t(ublas::product(na), value_t{2});
    auto pa  = ublas::size(na);
    auto pia = std::vector<std::size_t>(pa);
    std::iota( pia.begin(), pia.end(), std::size_t{1} );

    auto pib     = pia;
    auto pib_inv = compute_inverse_permutation(pib);

    auto f = compute_factorial(pa);

    // for the number of possible permutations
    // only permutation tuple pib is changed.
    for(auto i = 0u; i < f; ++i) {

      auto nb = permute_extents( pib, na  );
      auto wb = ublas::to_strides(nb,layout_t{});
      auto b  = vector_t(ublas::product(nb), value_t{3});
      auto pb = ublas::size(nb);

      // the number of contractions is changed.
      for(auto q = std::size_t{0}; q <= pa; ++q) {

        auto r  = pa - q;
        auto s  = pb - q;

        auto pc = r+s > 0 ? std::max(std::size_t{r+s},std::size_t{2}) : std::size_t{2};

        auto nc_base = extents_base_t(pc,std::size_t{1});

        for(auto j = 0u; j < r; ++j)
          nc_base[j] = na[pia[j]-1];

        for(auto j = 0u; j < s; ++j)
          nc_base[r+j] = nb[ pib_inv[j]-1 ];

        auto nc = extents_t ( nc_base );
        auto wc = ublas::to_strides(nc,layout_t{});
        auto c  = vector_t  ( ublas::product(nc), value_t(0) );

        ublas::ttt(pa,pb,q,
                   pia.data(), pib_inv.data(),
                   c.data(), nc.data(), wc.data(),
                   a.data(), na.data(), wa.data(),
                   b.data(), nb.data(), wb.data());

        auto acc = std::size_t{1};
        for(auto j = r; j < pa; ++j)
          acc *= na[pia[j]-1];

        auto v = value_t(acc)*a[0]*b[0];

        BOOST_CHECK( std::all_of(c.begin(),c.end(), [v](auto cc){return cc == v; } ) );

      }

      std::next_permutation(pib.begin(), pib.end());
      pib_inv = compute_inverse_permutation(pib);
    }
  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_ttt, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;
  using extents_t = ublas::extents<>;
  using extents_base_t = typename extents_t::base_type;

  // left-hand and right-hand side have the
  // the same number of elements

  // computing the inner product with
  // different permutation tuples for
  // right-hand side

  for(auto const& na : extents) {

    auto wa = ublas::to_strides(na,layout_t{});
    auto a  = vector_t(ublas::product(na), value_t{2});
    auto pa = ublas::size(na);

    auto const& nb = na;
    auto wb = ublas::to_strides(nb,layout_t{});
    auto b  = vector_t(ublas::product(nb), value_t{3});
    auto pb = ublas::size(nb);

    //  std::cout << "na = ";
    //  std::copy(na.begin(), na.end(), std::ostream_iterator<size_type>(std::cout, " "));
    //  std::cout << std::endl;

    //  std::cout << "nb = ";
    //  std::copy(nb.begin(), nb.end(), std::ostream_iterator<size_type>(std::cout, " "));
    //  std::cout << std::endl;

    // the number of contractions is changed.
    for( auto q = std::size_t{0}; q <= pa; ++q) { // pa

      auto r  = pa - q;
      auto s  = pb - q;

      auto pc = r+s > 0 ? std::max(std::size_t{r+s},std::size_t{2}) : std::size_t{2};

      auto nc_base = extents_base_t(pc,std::size_t{1});

      for(auto i = 0u; i < r; ++i)
        nc_base[i] = na[i];

      for(auto i = 0u; i < s; ++i)
        nc_base[r+i] = nb[i];

      auto nc = extents_t ( nc_base );
      auto wc = ublas::to_strides(nc,layout_t{});
      auto c  = vector_t  ( ublas::product(nc), value_t{0} );

      //   std::cout << "nc = ";
      //   std::copy(nc.begin(), nc.end(), std::ostream_iterator<size_type>(std::cout, " "));
      //   std::cout << std::endl;

      ublas::ttt(pa,pb,q,
                 c.data(), nc.data(), wc.data(),
                 a.data(), na.data(), wa.data(),
                 b.data(), nb.data(), wb.data());

      auto acc = std::size_t{1};
      for(auto i = r; i < pa; ++i)
        acc *= na[i];

      auto v = value_t(acc)*a[0]*b[0];

      BOOST_CHECK( std::all_of(c.begin(),c.end(), [v](auto cc){return cc == v; } ) );
    }

  }
}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_inner, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using vector_t  = std::vector<value_t>;

  for(auto const& n : extents) {

    auto a = vector_t(ublas::product(n), value_t{2});
    auto b = vector_t(ublas::product(n), value_t{3});
    auto w = ublas::to_strides(n,layout_t{});

    auto c = ublas::inner(ublas::size(n), n.data(), a.data(), w.data(), b.data(), w.data(), value_t(0));
    auto cref = std::inner_product(a.begin(), a.end(), b.begin(), value_t(0));

    BOOST_CHECK_EQUAL( c , cref );

  }

}

BOOST_FIXTURE_TEST_CASE_TEMPLATE( test_tensor_outer, value,  test_types, fixture )
{
  namespace ublas = boost::numeric::ublas;
  using value_t   = typename value::first_type;
  using layout_t  = typename value::second_type;
  using extents_t = ublas::extents<>;
  using vector_t  = std::vector<value_t>;

  for(auto const& na : extents) {

    auto a = vector_t(ublas::product(na), value_t{2});
    auto wa = ublas::to_strides(na,layout_t{});

    for(auto const& nb : extents) {

      auto b = vector_t(ublas::product(nb), value_t{3});
      auto wb = ublas::to_strides(nb,layout_t{});

      auto c = vector_t(ublas::product(nb)*ublas::product(na));
      auto nc_base = typename extents_t::base_type(ublas::size(na)+ublas::size(nb));

      for(auto i = 0u; i < ublas::size(na); ++i)
        nc_base[i] = na[i];
      for(auto i = 0u; i < ublas::size(nb); ++i)
        nc_base[i+ublas::size(na)] = nb[i];

      auto nc = extents_t(nc_base);
      auto wc = ublas::to_strides(nc,layout_t{});

      ublas::outer(c.data(), ublas::size(nc), nc.data(), wc.data(),
                   a.data(), ublas::size(na), na.data(), wa.data(),
                   b.data(), ublas::size(nb), nb.data(), wb.data());

      for(auto const& cc : c)
        BOOST_CHECK_EQUAL( cc , a[0]*b[0] );
    }

  }

}

BOOST_AUTO_TEST_SUITE_END()