/*
 * CurrentFlowGroupCloseness.cpp
 *
 *      Author: gstoszek
 */

#include "CurrentFlowGroupCloseness.h"
#include "Centrality.h"
#include "../algebraic/CSRMatrix.h"
#include "../numerics/LAMG/Lamg.h"
#include "../auxiliary/Log.h"
#include <chrono>
#include <stdlib.h>
#include <cmath>
#include <algorithm>
#include "EffectiveResistanceDistance.h"
#include "ERDLevel.h"
#include "../components/ConnectedComponents.h"
#include <armadillo>

namespace NetworKit {

   CurrentFlowGroupCloseness::CurrentFlowGroupCloseness(Graph& G,const count k, const count CB,const double epsilon) : G(G),k(k),CB(CB),epsilon(epsilon){
     if (G.isDirected()) throw std::runtime_error("Graph is directed graphs!");
     ConnectedComponents cc(G);
     cc.run();
     if (cc.getPartition().numberOfSubsets() > 1) throw std::runtime_error("Graph has more then one component!");
     if(k>=G.numberOfNodes()) throw std::runtime_error("Size of Group greater then number of nodes!");

     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;

     numberOfCoarsedNodes=0;
     S.resize(k);
     n=G.numberOfNodes();

     end = std::chrono::high_resolution_clock::now();
     diff = end-start;
     std::cout << "Constructor finished in " << diff.count() << "(s)" << "\n\n";
           std::cout << "Number of Nodes=" << G.numberOfNodes() << "\n";
   }

   void CurrentFlowGroupCloseness::run() {
     bool coarse;
     count ID;
     count minDegree;
     auto start = std::chrono::high_resolution_clock::now();
     auto end = std::chrono::high_resolution_clock::now();
     std::chrono::duration<double> diff;
     std::vector<std::tuple<count,count,count>> c_indices;
     arma::Mat<double> L;
     ID=0;
     if(CB>0){
       ID++;
       minDegree=1;
       mergePeripheralNodes();
       ID++;
       minDegree=updateMinDegree();
       coarse=true;
       while((minDegree<CB)&&(coarse)){
         c_indices=coarsingIndices(minDegree, false);
         coarseGraph(c_indices,ID);
         ID++;
         minDegree=updateMinDegree();

         if(!(c_indices.size()>1)||!(G.numberOfNodes()>k)){
           coarse=false;
         }
        }
      }
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Coarsening " << diff.count() << "(s)" << "\n\n";
      std::cout << "Laplacian size afvoidter coarsening" << L.n_rows << "\n";
      start = std::chrono::high_resolution_clock::now();
      /*changed
      //ERD.computeFromLaplacian(vecOfNodes,L);
      */
      L=computePinvOfLaplacian();
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Initial EffectiveResistanceDistanceMatrix finished in " << diff.count() << "(s)" << "\n\n";
      //greedy();
      //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
      start = std::chrono::high_resolution_clock::now();
      /*
      if(CB>1){
        ID=LevelList[LevelList.size()-1].getID();
        std::cout<<"ID="<<ID<<"\n";
        while(ID>1){
          uncoarseEfffectiveResistanceDistanceMatrix(ID);
          //greedy();
          //std::cout<<"Level:" <<ID<<" with the value: " << CFGCC << "\n";
          ID--;
        }
      }
      */
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Uncoarsening in " << diff.count() << "(s)" << "\n\n";
      start = std::chrono::high_resolution_clock::now();
      greedy(L);
      end = std::chrono::high_resolution_clock::now();
      diff = end-start;
      std::cout << "Greedy in " << diff.count() << "(s)" << "\n\n";
    }
    std::vector<node> CurrentFlowGroupCloseness::getNodesofGroup(){
      return S;
    }
    double CurrentFlowGroupCloseness::getCFGCC() {
      return CFGCC;
    }
    void CurrentFlowGroupCloseness::greedy(arma::Mat<double> L){
      count sampleSize;
      node s,v,w;
      double centrality,prevCFGCC,bestMarginalGain,distance;
      std::vector<bool> V;
      std::vector<node> vecOfNodes,vecOfSamples, vecOfPeriphs;
      std::vector<count> reverse;
      std::vector<double> mindst, dst, bst, zeroVec, marginalGain;

      vecOfSamples.resize(0);
      vecOfPeriphs.resize(0);
      vecOfNodes = G.nodes();
      reverse.resize(G.upperNodeIdBound());
      for(count i=0;i<vecOfNodes.size();i++){
        reverse[vecOfNodes[i]]=i;
      }

      for(count i=0;i<vecOfNodes.size();i++){
        v=vecOfNodes[i];
        if(vecOfPeripheralNodes[v])
          vecOfPeriphs.push_back(v);
        else{
          vecOfSamples.push_back(v);
        }
      }
      sampleSize=(count)(log(vecOfSamples.size())/(2*epsilon*epsilon));
      if(sampleSize>vecOfSamples.size()){
        sampleSize=vecOfSamples.size();
      }
      CFGCC=n*n*n;
      prevCFGCC=CFGCC;
      V.resize(G.numberOfNodes(),true);
      mindst.resize(G.numberOfNodes(),n*n);
      zeroVec.resize(G.numberOfNodes(),0.);
      marginalGain.resize(G.numberOfNodes(),CFGCC);
      for(count i=0;i<k;i++){
        std::random_shuffle (vecOfSamples.begin(), vecOfSamples.end());
        bestMarginalGain=0.;
        for (count j=0; j<vecOfSamples.size();j++) {
          v=vecOfSamples[j];
          if(V[v] && (bestMarginalGain<marginalGain[v])){
            dst=mindst;
            centrality = 0.;
            for (count l = 0; l < sampleSize; l++) {
              w=vecOfSamples[l];
              distance=L(reverse[v],reverse[v])+L(reverse[w],reverse[w])-2*L(reverse[v],reverse[w]);
              if (distance< mindst[w]){
                dst[w]=distance;
              }
              centrality +=dst[w];
            }
            centrality*=((double)(vecOfSamples.size()+numberOfCoarsedNodes)/(double)(sampleSize));
            for (count l = 0 ; l < vecOfPeriphs.size(); l++) {
              w=vecOfPeriphs[l];
              distance=L(reverse[v],reverse[v])+L(reverse[w],reverse[w])-2*L(reverse[v],reverse[w]);
              if (distance< mindst[w]){
                dst[w]=distance;
              }
              centrality += dst[w];
            }
            marginalGain[v]=prevCFGCC-centrality;
            if (centrality < CFGCC) {
              CFGCC = centrality;
              bst=dst;
              s = v;
              bestMarginalGain=marginalGain[v];
            }
            //std::cout<<"c("<<v<<")"<<centrality<<"\n";
          }
        }
        S[i]=s;
        V[s]=false;
        mindst=bst;
        prevCFGCC=CFGCC;
      }
     CFGCC = (double)(n)/CFGCC;
    }
    count CurrentFlowGroupCloseness::updateMinDegree(){
      bool search;
      count min;
      count i;
      std::vector<node> vecOfNodes;
      node v;
      search=true;
      min=n;
      i=0;
      vecOfNodes=G.nodes();
      while((i<vecOfNodes.size())&&(search)){
        v=vecOfNodes[i];
        if((G.degree(v)<min) && (G.degree(v)>1)){
          min=G.degree(v);
          if(min==2){
            search=false;
          }
        }
        i++;
      }
      return min;
    }
    std::vector<std::tuple<count,count,count>> CurrentFlowGroupCloseness::coarsingIndices(count courseningDegree, bool Random){
        bool search;
        count c_i,s_i,w_i,l;
        node c,s,w;
        std::vector<bool> vecOfFreeNodes;
        std::vector<node> vecOfNodes, vecOfCoarseNodes, vecOfNeighbours;
        std::vector<std::tuple<count,count,count>> indices;

        vecOfCoarseNodes.resize(0);
        vecOfFreeNodes.resize(G.upperNodeIdBound(),true);

        vecOfNodes=G.nodes();
        for(count i=0;i<vecOfNodes.size();i++){
          c=vecOfNodes[i];
          if(G.degree(c)==courseningDegree){
            vecOfCoarseNodes.push_back(i);
          }
        }
        if(Random){
          std::random_shuffle (vecOfCoarseNodes.begin(), vecOfCoarseNodes.end());
        }
        for(count i=0;i<vecOfCoarseNodes.size();i++){
          c=vecOfCoarseNodes[i];
          if(vecOfFreeNodes[c]){
            search=true;
            l=0;
            vecOfNeighbours=G.neighbors(c);
            while((search)&&(l<vecOfNeighbours.size())){
              s=vecOfNeighbours[l];
              if(vecOfFreeNodes[s]){
                l++;
                while((search)&&(l<vecOfNeighbours.size())){
                  w=vecOfNeighbours[l];
                  if(vecOfFreeNodes[w]){
                    vecOfFreeNodes[s]=false;
                    vecOfFreeNodes[c]=false;
                    vecOfFreeNodes[w]=false;
                    search=false;
                    indices.push_back(std::make_tuple(c,s,w));
                  }
                  else{
                    l++;
                  }
                }
              }
              else{
                l++;
              }
            }//End while
          }
        }
        return indices;
      }
    /**************************************************************************
    void CurrentFlowGroupCloseness::uncoarseEfffectiveResistanceDistanceMatrix(count ID){
      bool search;
      count l,j;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<std::tuple<node,node,count,double,double,double>> vecOfTriangles;
      std::tuple<node,node,count,double,double,double> triangle;

      vecOfTriangles=LevelList[ID-1].getVecOfTriangles();
      for(count i=0;i<vecOfTriangles.size();i++){
        j=vecOfTriangles.size()-1-i;
        triangle=vecOfTriangles[j];
        ERD.uncoarseTriangle(vecOfNodes,triangle);
        c=std::get<0>(triangle);
        s=std::get<1>(triangle);
        w=std::get<2>(triangle);
        edgeWeightcs=std::get<3>(triangle);
        edgeWeightcw=std::get<4>(triangle);
        edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
        edgeWeightsw=1./edgeWeightsw;
        //vecOfNodes.push_back(c);
        //Adj[s].push_back (c);
        //Adj[c].push_back(s);
        if(std::get<4>(triangle)>0){
          //Adj[c].push_back (w);
          //Adj[w].push_back(c);
        }
        if(!(std::get<5>(triangle)==edgeWeightsw)){
          search = true;
          l= 0;
          while((l<Adj[s].size())&&(search)){
            if(Adj[s][l]==w){
              Adj[s].erase(Adj[s].begin()+l);
              search=false;
            }
            else{
              l++;
            }
          }//end while
          search = true;
          l= 0;
          while((l<Adj[w].size())&&(search)){
            if(Adj[w][l]==s){
                Adj[w].erase(Adj[w].begin()+l);
                search=false;
              }
          else{
            l++;
          }
          }//end while
        }//end if
        numberOfCoarsedNodes--;
      }//end for
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::mergePeripheralNodes(){
      bool search;
      count l;
      node c,s,w;
      double edgeWeightcs,edgeWeightss,edgeWeightsw;
      std::vector<node> vecOfNodes,vecOfSupernodes;
      std::vector<std::pair<node,node>> mapping;
      std::vector<std::tuple<node,node,node,double,double,double>> vecOfTriangles;

      vecOfNodes=G.nodes();
      vecOfSupernodes.resize(0);
      mapping.resize(0);
      vecOfTriangles.resize(0);
      for(count c_i=0;c_i<vecOfNodes.size();c_i++){
        c=vecOfNodes[c_i];
        if(G.degree(c)==1){
          s=G.randomNeighbor(c);
          if(std::find(vecOfSupernodes.begin(), vecOfSupernodes.end(), s) == vecOfSupernodes.end()){
            vecOfSupernodes.push_back(s);
            mapping.push_back (std::make_pair(s,c));
            vecOfPeripheralNodes[c]=true;
          }
          else{
            search = true;
            l= 0;
            while(search){
              if(vecOfSupernodes[l]==s){
                w=mapping[l].second;
                search=false;
              }
              l++;
            }//end while
            edgeWeightcs=G.weight(c,s);
            edgeWeightsw=G.weight(s,w);
            edgeWeightsw=1./edgeWeightcs+1./edgeWeightsw;
            edgeWeightsw=1./edgeWeightsw;
            G.setWeight(s,w,edgeWeightsw);
            G.removeNode(c);
            vecOfTriangles.push_back(std::make_tuple(c,s,w,edgeWeightcs,0.,edgeWeightsw));
          }//end else
        }
      }
      ERDLevel Level(1,vecOfTriangles);
      LevelList.push_back(Level);
    }
    /***************************************************************************/
    void CurrentFlowGroupCloseness::coarseGraph(std::vector<std::tuple<count,count,count>> matchings,count ID){
      bool search;
      count a,l,c_i,s_i,w_i;
      node c,s,w;
      double edgeWeightcs,edgeWeightcw,edgeWeightsw;
      std::vector<node> vecOfNodes;
      std::vector<std::tuple<node,node,node,double,double,double>> vecOfTriangles;

      vecOfNodes=G.nodes();
      vecOfTriangles.resize(0);

      for(count i=0;i<matchings.size();i++){
        search=false;
        c=std::get<0>(matchings[i]);
        s=std::get<1>(matchings[i]);
        w_i=std::get<2>(matchings[i]);
        w=vecOfNodes[w_i];
        edgeWeightcs=G.weight(c,s);
        edgeWeightcw=G.weight(c,w);
        edgeWeightsw=1./edgeWeightcs+1/edgeWeightcw;
        edgeWeightsw=1./edgeWeightsw;
        edgeWeightsw+=G.weight(s,w);
        G.setWeight(s,w,edgeWeightsw);
        vecOfTriangles.push_back(std::make_tuple(c,s,w,edgeWeightcs,edgeWeightcw,edgeWeightsw));
        G.removeNode(c);
        numberOfCoarsedNodes++;
      }
      /*
      arma::uvec indices(vecOfNodes.size()-matchings.size());
      a=0;
      l=0;
      for(count i=0;i<vecOfNodes.size();i++){
        if(std::get<0>(matchings[l])==i){
          if(l<matchings.size()-1)
            l++;
        }
        else{
          indices(a)=i;
          a++;
        }
      }
      for(count i=0;i<matchings.size();i++){
        vecOfNodes.erase(vecOfNodes.begin()+std::get<0>(matchings[i])-i);
      }
      L=L.submat(indices, indices);
      */
      ERDLevel Level(ID,vecOfTriangles);
      LevelList.push_back(Level);
    }

    arma::Mat<double> CurrentFlowGroupCloseness::computePinvOfLaplacian(){
      node v;
      node w;
      count n;
      double factor;
      std::vector<node> vecOfNodes;
      vecOfNodes=G.nodes();
      n=vecOfNodes.size();
      arma::Mat<double> L(n,n);
      L.zeros();
      for(count i=0;i<n;i++){
        v=vecOfNodes[i];
        for(count j=i+1;j<n;j++){
          w=vecOfNodes[j];
          if(G.hasEdge(v,w)){
            L(i,j)=-G.weight(v,w);
            L(j,i)=-G.weight(v,w);
            L(i,i)+=G.weight(v,w);
            L(j,j)+=G.weight(v,w);
          }
        }
      }
      arma::Mat<double> J(n,n);
      factor=1./n;
      J.fill(factor);
      L= L+J;
      L=arma::inv_sympd(L);
      L= L-J;
      return L;
    }
} /* namespace NetworKit*/


// This file is part of LatNet Builder.
//
// Copyright (C) 2012-2021  The LatNet Builder author's, supervised by Pierre L'Ecuyer, Universite de Montreal.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "latbuilder/SizeParam.h"
#include "latbuilder/Util.h"
#include "latticetester/Util.h"
#include "latticetester/IntFactor.h"
#include <NTL/GF2XFactoring.h>

namespace LatBuilder {

//===============================================================================================================
template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::SizeParam(uInteger primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>>(primeBase == 0 ? 0 : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if (primeBase >= 2 and LatticeTester::IntFactor<std::int64_t>::isPrime(primeBase, 0) == LatticeTester::COMPOSITE)
      throw std::invalid_argument("SizeParam: primeBase is not prime");
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::SizeParam(Polynomial primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>>(IsZero(primeBase) ? Polynomial(0) : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if ( !IterIrredTest(primeBase))
      throw std::invalid_argument("SizeParam: primeBase is not prime");

}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::SizeParam(uInteger primeBase, Level maxLevel):
   BasicSizeParam<SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>>(primeBase == 0 ? 0 : intPow(primeBase, maxLevel)),
   m_base(primeBase),
   m_maxLevel(maxLevel)
{
   if (primeBase >= 2 and LatticeTester::IntFactor<std::int64_t>::isPrime(primeBase, 0) == LatticeTester::COMPOSITE)
      throw std::invalid_argument("SizeParam: primeBase is not prime");
}

//===================================================================================================================

template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::SizeParam(uInteger numPoints):
   BasicSizeParam<SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>>(numPoints)
{
   if (numPoints == 0) {
      m_base = 0;
      m_maxLevel = 0;
   }
   else if (numPoints == 1) {
      m_base = 1;
      m_maxLevel = 0;
   }
   else {
      const auto factors = primeFactorsMap(numPoints);
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.first;
      m_maxLevel = factor.second;
   }
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::SizeParam(Polynomial modulus):
   BasicSizeParam<SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>>(modulus)
{
   if (IsZero(modulus)) {
      m_base = Polynomial(0);
      m_maxLevel = 0;
   }
   else {
      NTL::vector< NTL::Pair< Polynomial, long > > factors ;
      CanZass(factors, modulus); // calls "Cantor/Zassenhaus" algorithm from <NTL/GF2XFactoring.h>
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.a; // = factor.first
      m_maxLevel = factor.b; // = factor.second
   }
}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::SizeParam(uInteger numPoints):
   BasicSizeParam<SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>>(numPoints)
{
   if (numPoints == 0) {
      m_base = 0;
      m_maxLevel = 0;
   }
   else if (numPoints == 1) {
      m_base = 1;
      m_maxLevel = 0;
   }
   else {
      const auto factors = primeFactorsMap(numPoints);
      if (factors.size() != 1)
         throw std::runtime_error("not an integer power of a prime base");
      const auto& factor = *factors.begin();
      m_base = factor.first;
      m_maxLevel = factor.second;
   }
}

//=======================================================================================================================

template<>
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return base() == 0 ? 0 : intPow(base(), level);
}

template<>
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return IsZero(base())  ? 0 : intPow( 2, deg(base())*level );
}

template<>
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::size_type
SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>::numPointsOnLevel(Level level) const
{
   if (level > maxLevel())
      throw std::invalid_argument("level > maxLevel");
   return base() == 0 ? 0 : intPow(base(), level);
}
//========================================================================================================================

template<>
size_t
SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>::totient() const
{ return base() == 0 ? 0 : (base() - 1) * this->numPoints() / base(); }

template<>
size_t
SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>::totient() const
{ return IsZero(base()) == 0 ? 0 : (intPow(2,deg(base())) - 1) * this->numPoints() / intPow(2,deg(base())); }

//=========================================================================================================================

template<LatticeType LR>
void
SizeParam<LR,EmbeddingType::MULTILEVEL>::normalize(Real& merit) const
{ merit /= this->numPoints(); }

template<LatticeType LR>
void
SizeParam<LR,EmbeddingType::MULTILEVEL>::normalize(RealVector& merit) const
{
   if (merit.size() != maxLevel() + 1)
      throw std::logic_error("merit vector size and maximum level do not match");
   for (Level level = 0; level < merit.size(); level++)
      merit[level] /= numPointsOnLevel(level);
}
//===========================================================================================================================
template<LatticeType LR>
std::ostream&
SizeParam<LR,EmbeddingType::MULTILEVEL>::format(std::ostream& os) const
{ os << base(); if (maxLevel() != 1) os << "^" << maxLevel(); return os; }
//===========================================================================================================================

template class SizeParam<LatticeType::ORDINARY,EmbeddingType::MULTILEVEL>;
template class SizeParam<LatticeType::POLYNOMIAL,EmbeddingType::MULTILEVEL>;
template class SizeParam<LatticeType::DIGITAL,EmbeddingType::MULTILEVEL>;

}


#include <iostream>
#include <armadillo>
#include <complex>
#include <cstdlib>
#include <ctime>
using namespace std;
using namespace arma;

int main() {
	// Input: sizeCells of lattice in z direction and # of k-points in the direction of kPointVec
	int sizeCells = 5;
	int k_points_max=1+19*1;
	rowvec kPointVec(3); // Defined below

	// Parameters for the Hamiltonian and data structures (Don't need to change those)
	double tHopping = 1;
	double deltatHopping = 0.4;
	complex<double> spinOrbitCoupling(0,1);

	// Misc
	double delta = 0.01; // small number for numerical comparisons
	int index =0;
	const complex<double> ii(0, 1);

	// Geometry
	double latVecNorm=1/sqrt(2); // cubic cell size a = 1
	double sublatVecNorm=sqrt(3)/4;
	int nSites=3*sizeCells;

	rowvec dummyVec(3), dummyVec2(3), dummyVec3(3), dummyVec4(3);
	cx_mat dummyMat(2,2);
	mat sublatOne(nSites, 4); // Sites at origin of a unit cell
	mat sublatTwo(nSites, 4); // Sites at sublatVec in a unit cell
	rowvec sublatVec(3); // 2nd lattice site within a unit cell
	rowvec latVec1(3), latVec2(3), latVec3(3), latVec4(3), latVec5(3); // lattice vectors
	rowvec bulklatVec1(3), bulklatVec2(3), bulklatVec3(3), GVec1(3), GVec2(3);
	sublatVec << 0.25 << 0.25 << 0.25;
	bulklatVec1 << 0 << 0.5 << 0.5;
	bulklatVec2 << 0.5 << 0 << 0.5;
	bulklatVec3 << 0.5 << 0.5 << 0;

	// Variables below are needed for periodic b.c.s
	// Unit cell
	latVec1 << 0.5 << -0.5 << 0;
	latVec2 << 0 << 0.5 << -0.5;
	latVec3 << 1 << 1 << 1;
	// Two more atoms from sublattice One within the primary unit cell
	latVec4 << 0 << 0.5 << 0.5;
	latVec5 << 0.5 << 1 << 0.5;
	// Reciprocal lattice vectors for slab
	GVec1 << 4*M_PI/3 << -2*M_PI/3 << -2*M_PI/3;
	GVec2 << 2*M_PI/3 << 2*M_PI/3 << -4*M_PI/3;

	// Define Pauli matrices
	cx_mat unity(2,2), sigma_x(2,2), sigma_y(2,2), sigma_z(2,2); // Pauli matrices
	mat dummy_unity(2,2), dummy_sigma_x(2,2), dummy_sigma_y(2,2), dummy_sigma_z(2,2);
	dummy_unity(0,0)=1; dummy_unity(1,1)=1; dummy_sigma_x(1,0)=1; dummy_sigma_x(0,1)=1;
	dummy_sigma_y(0,1)=-1; dummy_sigma_y(1,0)=1; dummy_sigma_z(0,0)=1; dummy_sigma_z(1,1)=-1;
	unity.set_real(dummy_unity);
	sigma_x.set_real(dummy_sigma_x);
	sigma_y.set_imag(dummy_sigma_y);
	sigma_z.set_real(dummy_sigma_z);

	// Set up data structures for the slab
	cx_mat Hamiltonian(4*nSites,4*nSites);
	Hamiltonian.zeros();
	vec eigvals(4*nSites);
	cx_mat eigvecs(4*nSites,4*nSites);
	mat bandstructure(k_points_max,4*nSites);

	// Set up primary unit cell of the slab
	for (int k=0; k<sizeCells; k++) {
		dummyVec=k*latVec3;
		sublatOne(index,0)=99; sublatOne(index+1,0)=99; sublatOne(index+2,0)=99;
		sublatOne(index,span(1,3))=dummyVec;
		sublatOne(index+1,span(1,3))=(dummyVec+latVec4);
		sublatOne(index+2,span(1,3))=(dummyVec+latVec5);
		sublatTwo(index,0)=99; sublatTwo(index+1,0)=99; sublatTwo(index+2,0)=99;
		sublatTwo(index,span(1,3))=(dummyVec+sublatVec);
		sublatTwo(index+1,span(1,3))=(dummyVec+latVec4+sublatVec);
		sublatTwo(index+2,span(1,3))=(dummyVec+latVec5+sublatVec);
		index=index+3;
	}

	// Loop over k-points
	for (int m=0; m<k_points_max; m++ ){
		kPointVec=GVec1*m/20;

		// Set up the Hamiltonian in a different way (results equivalent)
		for (int n=0; n<nSites; n++){
			// Go over unit cells in width (i,j; periodic), height (k) and across sites in each cell (l)
			for (int i=-2; i<3; i++) {
				for (int j=-2; j<3; j++) {
					for (int k=0; k<sizeCells; k++) {
						for (int l=0; l<3; l++) {
							switch(l) {
								case 0:
									dummyVec=i*latVec1+j*latVec2+k*latVec3-sublatOne(n,span(1,3));
									break;
								case 1:
									dummyVec=i*latVec1+j*latVec2+k*latVec3+latVec4-sublatOne(n,span(1,3));
									break;
								case 2:
									dummyVec=i*latVec1+j*latVec2+k*latVec3+latVec5-sublatOne(n,span(1,3));
									break;
								default:
									cout << "Error, l should be 0,1,2" << endl;
							}
							dummyVec2=dummyVec+sublatVec; // used to find hoppings between sublattices
							index=3*k+l; // used when have a site within primary cell
							dummyVec3=i*latVec1+j*latVec2; // used if have an atom outside primary cell
							dummyVec4=dummyVec+sublatOne(n,span(1,3))-sublatTwo(n,span(1,3));

							// Set up nearest neighbour hoppings (between sublattices)
							// Set up hoppings into sublatOne
							if (dot(dummyVec2, dummyVec2) < sublatVecNorm*sublatVecNorm+delta) {
								// hopping to the n-th site from along [111] within the primary cell
								if (dot(dummyVec, dummyVec) < delta) {
									Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=(tHopping+deltatHopping)*unity;
									if (n != index || i!=0 || j!=0) {cout << "Error for deltatHopping" << endl; }
									// hopping in other directions
								} else {
									if (i==0 && j==0) {
										// Hoppings withing the primary cell
										Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=tHopping*unity;
										if (i!=0 || j!=0 || n==index) {cout << "Error for tHopping within primary cell" << endl; }
										// hopping to sublatOne from outside the primary cell
									} else {
										// hoppings from sublatTwo of index to sublatOne of n
										Hamiltonian(span(4*n+0,4*n+1),span(4*index+2,4*index+3))+=tHopping*exp(ii*dot(dummyVec3,kPointVec))*unity;
										if (n == index) {cout << "Error for tHopping" << endl; }
									}
								}
							}

							// Set up hoppings into sublatTwo
							if (dot(dummyVec4, dummyVec4) < sublatVecNorm*sublatVecNorm+delta) {
								// hopping to the n-th site from along [111] within the primary cell
								if (dot(dummyVec4+sublatVec,dummyVec4+sublatVec) < delta) {
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=(tHopping+deltatHopping)*unity;
									if (n != index || i!=0 || j!=0) {cout << "Error for deltatHopping" << endl; }
									// hopping in other directions
								} else {
									if (i==0 && j==0) {
										// Hoppings withing the primary cell
										Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=tHopping*unity;
										if (i!=0 || j!=0 || n==index) {cout << "Error for tHopping within primary cell" << endl; }
										// hopping to sublatTwo from outside the primary cell
									} else {
										// hoppings from sublatOne of index to sublatTwo of n
										Hamiltonian(span(4*n+2,4*n+3),span(4*index+0,4*index+1))+=tHopping*exp(ii*dot(dummyVec3,kPointVec))*unity;
										if (n == index) {cout << "Error for tHopping" << endl; }
									}
								}
							}
							if ( (dot(dummyVec,dummyVec) < latVecNorm*latVecNorm+delta)  &&
									(dot(dummyVec,dummyVec) > sublatVecNorm*sublatVecNorm+delta) ) {
								// Matrix elements are direction-dependent
								if ( (dummyVec(0) > 0 && dummyVec(1) > 0) || (dummyVec(0) > 0 &&
											dummyVec(2) > 0) || (dummyVec(1) > 0 && dummyVec(2) > 0) ) {
									dummyVec = cross(sublatVec, dummyVec-sublatVec);
								} else if ( (dummyVec(1) < 0 && dummyVec(2) < 0) || (dummyVec(0) > 0 &&
											dummyVec(1) < 0) || (dummyVec(0) > 0 && dummyVec(2) < 0) ) {
									dummyVec = cross(-bulklatVec1+sublatVec, dummyVec+bulklatVec1-sublatVec);
								} else if ( (dummyVec(0) < 0 && dummyVec(1) > 0) || (dummyVec(0) < 0 &&
											dummyVec(2) < 0) || (dummyVec(1) > 0 && dummyVec(2) < 0) ) {
									dummyVec = cross(-bulklatVec2+sublatVec, dummyVec+bulklatVec2-sublatVec);
								} else if ( (dummyVec(0) < 0 && dummyVec(2) > 0) || (dummyVec(1) < 0 &&
											dummyVec(2) > 0) || (dummyVec(0) < 0 && dummyVec(1) < 0) ) {
									dummyVec = cross(-bulklatVec3+sublatVec, dummyVec+bulklatVec3-sublatVec);
								} else {cout << "Error in NNN hopping" << endl; }
								dummyMat=dummyVec(0)*sigma_x+dummyVec(1)*sigma_y+dummyVec(2)*sigma_z;
								if (i==0 && j==0) {
									Hamiltonian(span(4*n,4*n+1),span(4*index,4*index+1))+=spinOrbitCoupling*dummyMat;
									// Minus sign because for sublatTwo dummyVec is minus that of sublatOne
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+2,4*index+3))+= -1.0*spinOrbitCoupling*dummyMat;
								} else {
									// Hoppings within sublatOne
									Hamiltonian(span(4*n,4*n+1),span(4*index,4*index+1))+=exp(ii*dot(dummyVec3, kPointVec))*spinOrbitCoupling*dummyMat;
									// Hoppings within sublatTwo
									Hamiltonian(span(4*n+2,4*n+3),span(4*index+2,4*index+3))+= -1.0*exp(ii*dot(dummyVec3,kPointVec))*spinOrbitCoupling*dummyMat;
								}

							}
						}
					}
				}
			}
		}

		// abs returns a matrix, max returns a vector, 2nd max - largest value
		if (max(max(abs(Hamiltonian-Hamiltonian.t()))) > delta ) {
			cout << "Error, Hamiltonian is not Hermitian by at least " << delta << endl;
		}

		eig_sym(eigvals, eigvecs, Hamiltonian);
		bandstructure.row(m)=eigvals.t();
		cout << endl;
	//	Hamiltonian.save("Hamiltonian.txt", arma_ascii);
		cout << "Calculated k-point";
		kPointVec.print();
		Hamiltonian.zeros();
	}

	bandstructure.save("output_bs.txt", raw_ascii);
	//sublatOne.save("output.txt",raw_ascii);
	//sublatTwo.save("output2.txt",raw_ascii);
}


/**
 * MIT License
 *
 * Copyright (c) 2018 Parsiad Azimzadeh
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#ifndef MFHOWARDS_BELLMAN_EQ_FROM_LAMBDAS_HPP
#define MFHOWARDS_BELLMAN_EQ_FROM_LAMBDAS_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Sparse>

#include <functional>       // std::function
#include <initializer_list> // std::initializer_list
#include <limits>           // std::numeric_limits
#include <utility>          // std::forward

namespace mfhowards {
namespace {
template <typename CtrlT> class bellman_eq_from_lambdas;
}
} // namespace mfhowards

namespace Eigen {
namespace internal {
template <typename CtrlT>
struct traits<mfhowards::bellman_eq_from_lambdas<CtrlT>>
    : public Eigen::internal::traits<Eigen::SparseMatrix<double>> {};
} // namespace internal
} // namespace Eigen

namespace mfhowards {
namespace {
template <typename CtrlT>
class bellman_eq_from_lambdas
    : public Eigen::EigenBase<bellman_eq_from_lambdas<CtrlT>> {

public:
  typedef double Scalar;
  typedef double RealScalar;
  typedef int StorageIndex;

  enum {
    ColsAtCompileTime = Eigen::Dynamic,
    MaxColsAtCompileTime = Eigen::Dynamic,
    IsRowMajor = false
  };

private:
  typedef std::function<Scalar(int, int, CtrlT)> FA;
  typedef std::function<Scalar(int, CtrlT)> Fb;

  int n;
  std::vector<CtrlT> ctrl_set;
  FA A;
  Fb b;
  CtrlT *curr_ctrls;

  template <typename Rhs>
  using Helper = Eigen::Product<bellman_eq_from_lambdas<CtrlT>, Rhs,
                                Eigen::AliasFreeProduct>;

public:
  int rows() const { return n; }
  int cols() const { return n; }

  template <typename Rhs>
  Helper<Rhs> operator*(const Eigen::MatrixBase<Rhs> &x) const {
    return Helper<Rhs>(*this, x.derived());
  }

  template <typename T>
  bellman_eq_from_lambdas(const int n, T &&ctrl_set, FA A, Fb b)
      : ctrl_set(std::forward<T>(ctrl_set)) {
    this->n = n;
    this->A = A;
    this->b = b;
    curr_ctrls = new CtrlT[n];
  }

  bellman_eq_from_lambdas(const int n, std::initializer_list<CtrlT> list, FA A,
                          Fb b)
      : bellman_eq_from_lambdas(n, std::vector<CtrlT>(list), A, b) {}

  ~bellman_eq_from_lambdas() { delete[] curr_ctrls; }

  bellman_eq_from_lambdas(const bellman_eq_from_lambdas &) = delete;
  bellman_eq_from_lambdas &operator=(const bellman_eq_from_lambdas &) = delete;

  void improve(const Eigen::VectorXd &x) {
    Eigen::VectorXd best =
        Eigen::VectorXd::Ones(rows()) * std::numeric_limits<double>::infinity();
    for (auto c : ctrl_set) {
      for (int i = 0; i < rows(); ++i) {
        double tmp = 0;
        for (int j = 0; j < cols(); ++j) {
          tmp += A(i, j, c) * x(j) - b(i, c);
        }
        if (tmp < best(i)) {
          best(i) = tmp;
          curr_ctrls[i] = c;
        }
      }
    }
  }

  Eigen::VectorXd rhs() const {
    Eigen::VectorXd rhs(rows());
    for (int i = 0; i < rows(); ++i) {
      rhs(i) = b(i, curr_ctrls[i]);
    }
    return rhs;
  }

  template <typename, typename, typename, typename, int>
  friend class Eigen::internal::generic_product_impl;
};
} // namespace
} // namespace mfhowards

namespace Eigen {
namespace internal {
template <typename CtrlT, typename Rhs>
struct generic_product_impl<mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs,
                            SparseShape, DenseShape, GemvProduct>
    : generic_product_impl_base<
          mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs,
          generic_product_impl<mfhowards::bellman_eq_from_lambdas<CtrlT>,
                               Rhs>> {

  typedef
      typename Product<mfhowards::bellman_eq_from_lambdas<CtrlT>, Rhs>::Scalar
          Scalar;

  template <typename Dest>
  static void
  scaleAndAddTo(Dest &dst, const mfhowards::bellman_eq_from_lambdas<CtrlT> &lhs,
                const Rhs &rhs, const Scalar &) {
    for (int i = 0; i < lhs.rows(); ++i) {
      for (int j = 0; j < lhs.cols(); ++j) {
        dst(i) += lhs.A(i, j, lhs.curr_ctrls[i]) * rhs(j);
      }
    }
  }
};
} // namespace internal
} // namespace Eigen

#endif


// Copyright (C) 2006  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/matrix.h>
#include <sstream>
#include <string>
#include <cstdlib>
#include <ctime>
#include <vector>
#include "../stl_checked.h"
#include "../array.h"
#include "../rand.h"
#include "checkerboard.h"
#include <dlib/statistics.h>

#include "tester.h"
#include <dlib/svm_threaded.h>

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.svm");

// ----------------------------------------------------------------------------------------

    void test_clutering (
    )
    {
        dlog << LINFO << "   being test_clutering()";
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef matrix<double,2,1> sample_type;

        // Now we are making a typedef for the kind of kernel we want to use.  I picked the
        // radial basis kernel because it only has one parameter and generally gives good
        // results without much fiddling.
        typedef radial_basis_kernel<sample_type> kernel_type;

        // Here we declare an instance of the kcentroid object.  The first argument to the constructor
        // is the kernel we wish to use.  The second is a parameter that determines the numerical
        // accuracy with which the object will perform part of the learning algorithm.  Generally
        // smaller values give better results but cause the algorithm to run slower.  You just have
        // to play with it to decide what balance of speed and accuracy is right for your problem.
        // Here we have set it to 0.01.
        kcentroid<kernel_type> kc(kernel_type(0.1),0.01);

        // Now we make an instance of the kkmeans object and tell it to use kcentroid objects
        // that are configured with the parameters from the kc object we defined above.
        kkmeans<kernel_type> test(kc);

        std::vector<sample_type> samples;
        std::vector<sample_type> initial_centers;

        sample_type m;

        dlib::rand rnd;

        print_spinner();
        // we will make 50 points from each class
        const long num = 50;

        // make some samples near the origin
        double radius = 0.5;
        for (long i = 0; i < num; ++i)
        {
            double sign = 1;
            if (rnd.get_random_double() < 0.5)
                sign = -1;
            m(0) = 2*radius*rnd.get_random_double()-radius;
            m(1) = sign*sqrt(radius*radius - m(0)*m(0));

            // add this sample to our set of samples we will run k-means
            samples.push_back(m);
        }

        // make some samples in a circle around the origin but far away
        radius = 10.0;
        for (long i = 0; i < num; ++i)
        {
            double sign = 1;
            if (rnd.get_random_double() < 0.5)
                sign = -1;
            m(0) = 2*radius*rnd.get_random_double()-radius;
            m(1) = sign*sqrt(radius*radius - m(0)*m(0));

            // add this sample to our set of samples we will run k-means
            samples.push_back(m);
        }

        // make some samples in a circle around the point (25,25)
        radius = 4.0;
        for (long i = 0; i < num; ++i)
        {
            double sign = 1;
            if (rnd.get_random_double() < 0.5)
                sign = -1;
            m(0) = 2*radius*rnd.get_random_double()-radius;
            m(1) = sign*sqrt(radius*radius - m(0)*m(0));

            // translate this point away from the origin
            m(0) += 25;
            m(1) += 25;

            // add this sample to our set of samples we will run k-means
            samples.push_back(m);
        }
        print_spinner();

        // tell the kkmeans object we made that we want to run k-means with k set to 3.
        // (i.e. we want 3 clusters)
        test.set_number_of_centers(3);

        // You need to pick some initial centers for the k-means algorithm.  So here
        // we will use the dlib::pick_initial_centers() function which tries to find
        // n points that are far apart (basically).
        pick_initial_centers(3, initial_centers, samples, test.get_kernel());

        print_spinner();
        // now run the k-means algorithm on our set of samples.
        test.train(samples,initial_centers);
        print_spinner();

        const unsigned long class1 = test(samples[0]);
        const unsigned long class2 = test(samples[num]);
        const unsigned long class3 = test(samples[2*num]);
        // now loop over all our samples and print out their predicted class.  In this example
        // all points are correctly identified.
        for (unsigned long i = 0; i < samples.size()/3; ++i)
        {
            DLIB_TEST(test(samples[i]) == class1);
            DLIB_TEST(test(samples[i+num]) == class2);
            DLIB_TEST(test(samples[i+2*num]) == class3);
        }

        dlog << LINFO << "   end test_clutering()";
    }

// ----------------------------------------------------------------------------------------

    // Here is the sinc function we will be trying to learn with the krls
    // object.
    double sinc(double x)
    {
        if (x == 0)
            return 1;
        return sin(x)/x;
    }

    void test_regression (
    )
    {
        dlog << LINFO << "   being test_regression()";
        // Here we declare that our samples will be 1 dimensional column vectors.  The reason for
        // using a matrix here is that in general you can use N dimensional vectors as inputs to the
        // krls object.  But here we only have 1 dimension to make the example simple.
        typedef matrix<double,1,1> sample_type;

        // Now we are making a typedef for the kind of kernel we want to use.  I picked the
        // radial basis kernel because it only has one parameter and generally gives good
        // results without much fiddling.
        typedef radial_basis_kernel<sample_type> kernel_type;

        // Here we declare an instance of the krls object.  The first argument to the constructor
        // is the kernel we wish to use.  The second is a parameter that determines the numerical
        // accuracy with which the object will perform part of the regression algorithm.  Generally
        // smaller values give better results but cause the algorithm to run slower.  You just have
        // to play with it to decide what balance of speed and accuracy is right for your problem.
        // Here we have set it to 0.001.
        krls<kernel_type> test(kernel_type(0.1),0.001);
        rvm_regression_trainer<kernel_type> rvm_test;
        rvm_test.set_kernel(test.get_kernel());

        krr_trainer<kernel_type> krr_test;
        krr_test.set_kernel(test.get_kernel());

        svr_trainer<kernel_type> svr_test;
        svr_test.set_kernel(test.get_kernel());
        svr_test.set_epsilon_insensitivity(0.0001);
        svr_test.set_c(10);

        rbf_network_trainer<kernel_type> rbf_test;
        rbf_test.set_kernel(test.get_kernel());
        rbf_test.set_num_centers(13);

        print_spinner();
        std::vector<sample_type> samples;
        std::vector<sample_type> samples2;
        std::vector<double> labels;
        std::vector<double> labels2;
        // now we train our object on a few samples of the sinc function.
        sample_type m;
        for (double x = -10; x <= 5; x += 0.6)
        {
            m(0) = x;
            test.train(m, sinc(x));

            samples.push_back(m);
            samples2.push_back(m);
            labels.push_back(sinc(x));
            labels2.push_back(2);
        }

        print_spinner();
        decision_function<kernel_type> test2 = rvm_test.train(samples, labels);
        print_spinner();
        decision_function<kernel_type> test3 = rbf_test.train(samples, labels);
        print_spinner();
        decision_function<kernel_type> test4 = krr_test.train(samples, labels);
        print_spinner();
        decision_function<kernel_type> test5 = svr_test.train(samples, labels);
        print_spinner();

        // now we output the value of the sinc function for a few test points as well as the
        // value predicted by krls object.
        m(0) = 2.5; dlog << LDEBUG << "krls: " << sinc(m(0)) << "   " << test(m); DLIB_TEST(abs(sinc(m(0)) - test(m)) < 0.01);
        m(0) = 0.1; dlog << LDEBUG << "krls: " << sinc(m(0)) << "   " << test(m); DLIB_TEST(abs(sinc(m(0)) - test(m)) < 0.01);
        m(0) = -4;  dlog << LDEBUG << "krls: " << sinc(m(0)) << "   " << test(m); DLIB_TEST(abs(sinc(m(0)) - test(m)) < 0.01);
        m(0) = 5.0; dlog << LDEBUG << "krls: " << sinc(m(0)) << "   " << test(m); DLIB_TEST(abs(sinc(m(0)) - test(m)) < 0.01);

        m(0) = 2.5; dlog << LDEBUG << "rvm: " << sinc(m(0)) << "   " << test2(m); DLIB_TEST(abs(sinc(m(0)) - test2(m)) < 0.01);
        m(0) = 0.1; dlog << LDEBUG << "rvm: " << sinc(m(0)) << "   " << test2(m); DLIB_TEST(abs(sinc(m(0)) - test2(m)) < 0.01);
        m(0) = -4;  dlog << LDEBUG << "rvm: " << sinc(m(0)) << "   " << test2(m); DLIB_TEST(abs(sinc(m(0)) - test2(m)) < 0.01);
        m(0) = 5.0; dlog << LDEBUG << "rvm: " << sinc(m(0)) << "   " << test2(m); DLIB_TEST(abs(sinc(m(0)) - test2(m)) < 0.01);

        m(0) = 2.5; dlog << LDEBUG << "rbf: " << sinc(m(0)) << "   " << test3(m); DLIB_TEST(abs(sinc(m(0)) - test3(m)) < 0.01);
        m(0) = 0.1; dlog << LDEBUG << "rbf: " << sinc(m(0)) << "   " << test3(m); DLIB_TEST(abs(sinc(m(0)) - test3(m)) < 0.01);
        m(0) = -4;  dlog << LDEBUG << "rbf: " << sinc(m(0)) << "   " << test3(m); DLIB_TEST(abs(sinc(m(0)) - test3(m)) < 0.01);
        m(0) = 5.0; dlog << LDEBUG << "rbf: " << sinc(m(0)) << "   " << test3(m); DLIB_TEST(abs(sinc(m(0)) - test3(m)) < 0.01);

        m(0) = 2.5; dlog << LDEBUG << "krr: " << sinc(m(0)) << "   " << test4(m); DLIB_TEST(abs(sinc(m(0)) - test4(m)) < 0.01);
        m(0) = 0.1; dlog << LDEBUG << "krr: " << sinc(m(0)) << "   " << test4(m); DLIB_TEST(abs(sinc(m(0)) - test4(m)) < 0.01);
        m(0) = -4;  dlog << LDEBUG << "krr: " << sinc(m(0)) << "   " << test4(m); DLIB_TEST(abs(sinc(m(0)) - test4(m)) < 0.01);
        m(0) = 5.0; dlog << LDEBUG << "krr: " << sinc(m(0)) << "   " << test4(m); DLIB_TEST(abs(sinc(m(0)) - test4(m)) < 0.01);

        m(0) = 2.5; dlog << LDEBUG << "svr: " << sinc(m(0)) << "   " << test5(m); DLIB_TEST(abs(sinc(m(0)) - test5(m)) < 0.01);
        m(0) = 0.1; dlog << LDEBUG << "svr: " << sinc(m(0)) << "   " << test5(m); DLIB_TEST(abs(sinc(m(0)) - test5(m)) < 0.01);
        m(0) = -4;  dlog << LDEBUG << "svr: " << sinc(m(0)) << "   " << test5(m); DLIB_TEST(abs(sinc(m(0)) - test5(m)) < 0.01);
        m(0) = 5.0; dlog << LDEBUG << "svr: " << sinc(m(0)) << "   " << test5(m); DLIB_TEST(abs(sinc(m(0)) - test5(m)) < 0.01);

        randomize_samples(samples, labels);
        dlog << LINFO << "KRR MSE and R-squared: "<< cross_validate_regression_trainer(krr_test, samples, labels, 6);
        dlog << LINFO << "SVR MSE and R-squared: "<< cross_validate_regression_trainer(svr_test, samples, labels, 6);
        matrix<double,1,4> cv = cross_validate_regression_trainer(krr_test, samples, labels, 6);
        DLIB_TEST(cv(0) < 1e-4);
        DLIB_TEST(cv(1) > 0.99);
        cv = cross_validate_regression_trainer(svr_test, samples, labels, 6);
        DLIB_TEST(cv(0) < 1e-4);
        DLIB_TEST(cv(1) > 0.99);

        randomize_samples(samples2, labels2);
        dlog << LINFO << "KRR MSE and R-squared: "<< cross_validate_regression_trainer(krr_test, samples2, labels2, 6);
        dlog << LINFO << "SVR MSE and R-squared: "<< cross_validate_regression_trainer(svr_test, samples2, labels2, 6);
        cv = cross_validate_regression_trainer(krr_test, samples2, labels2, 6);
        DLIB_TEST(cv(0) < 1e-4);
        cv = cross_validate_regression_trainer(svr_test, samples2, labels2, 6);
        DLIB_TEST(cv(0) < 1e-4);

        dlog << LINFO << "   end test_regression()";
    }

// ----------------------------------------------------------------------------------------

    void test_anomaly_detection (
    )
    {
        dlog << LINFO << "   begin test_anomaly_detection()";
        // Here we declare that our samples will be 2 dimensional column vectors.
        typedef matrix<double,2,1> sample_type;

        // Now we are making a typedef for the kind of kernel we want to use.  I picked the
        // radial basis kernel because it only has one parameter and generally gives good
        // results without much fiddling.
        typedef radial_basis_kernel<sample_type> kernel_type;

        // Here we declare an instance of the kcentroid object.  The first argument to the constructor
        // is the kernel we wish to use.  The second is a parameter that determines the numerical
        // accuracy with which the object will perform part of the learning algorithm.  Generally
        // smaller values give better results but cause the algorithm to run slower.  You just have
        // to play with it to decide what balance of speed and accuracy is right for your problem.
        // Here we have set it to 0.01.
        kcentroid<kernel_type> test(kernel_type(0.1),0.01);

        svm_one_class_trainer<kernel_type> one_class_trainer;
        one_class_trainer.set_nu(0.4);
        one_class_trainer.set_kernel(kernel_type(0.2));

        std::vector<sample_type> samples;

        // now we train our object on a few samples of the sinc function.
        sample_type m;
        for (double x = -15; x <= 8; x += 1)
        {
            m(0) = x;
            m(1) = sinc(x);
            test.train(m);
            samples.push_back(m);
        }

        decision_function<kernel_type> df = one_class_trainer.train(samples);

        running_stats<double> rs;

        // Now lets output the distance from the centroid to some points that are from the sinc function.
        // These numbers should all be similar.  We will also calculate the statistics of these numbers
        // by accumulating them into the running_stats object called rs.  This will let us easily
        // find the mean and standard deviation of the distances for use below.
        dlog << LDEBUG << "Points that are on the sinc function:\n";
        m(0) = -1.5; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -1.5; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -0;   m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -0.5; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -4.1; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -1.5; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));
        m(0) = -0.5; m(1) = sinc(m(0)); dlog << LDEBUG << "   " << test(m);  rs.add(test(m));

        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -0;   m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -0.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -4.1; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));
        m(0) = -0.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(rs.scale(test(m)) < 2, rs.scale(test(m)));

        const double thresh = 0.01;
        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -0;   m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -0.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -4.1; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -1.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));
        m(0) = -0.5; m(1) = sinc(m(0)); DLIB_TEST_MSG(df(m)+thresh > 0, df(m));

        dlog << LDEBUG;
        // Lets output the distance from the centroid to some points that are NOT from the sinc function.
        // These numbers should all be significantly bigger than previous set of numbers.  We will also
        // use the rs.scale() function to find out how many standard deviations they are away from the
        // mean of the test points from the sinc function.  So in this case our criterion for "significantly bigger"
        // is > 3 or 4 standard deviations away from the above points that actually are on the sinc function.
        dlog << LDEBUG << "Points that are NOT on the sinc function:\n";
        m(0) = -1.5; m(1) = sinc(m(0))+4;
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -1.5; m(1) = sinc(m(0))+3;
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -0;   m(1) = -sinc(m(0));
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -0.5; m(1) = -sinc(m(0));
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -4.1; m(1) = sinc(m(0))+2;
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -1.5; m(1) = sinc(m(0))+0.9;
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        m(0) = -0.5; m(1) = sinc(m(0))+1;
        dlog << LDEBUG << "   " << test(m) << " is " << rs.scale(test(m)) << " standard deviations from sinc.";
        DLIB_TEST_MSG(rs.scale(test(m)) > 6, rs.scale(test(m)));
        DLIB_TEST_MSG(df(m) + thresh < 0, df(m));

        dlog << LINFO << "   end test_anomaly_detection()";
    }

// ----------------------------------------------------------------------------------------

    void unittest_binary_classification (
    )
    /*!
        ensures
            - runs tests on the svm stuff compliance with the specs
    !*/
    {
        dlog << LINFO << "   begin unittest_binary_classification()";
        print_spinner();

        typedef double scalar_type;
        typedef matrix<scalar_type,2,1> sample_type;

        std::vector<sample_type> x;
        std::vector<matrix<double,0,1> > x_linearized;
        std::vector<scalar_type> y;

        get_checkerboard_problem(x,y, 300, 2);
        const scalar_type gamma = 1;

        typedef radial_basis_kernel<sample_type> kernel_type;

        rbf_network_trainer<kernel_type> rbf_trainer;
        rbf_trainer.set_kernel(kernel_type(gamma));
        rbf_trainer.set_num_centers(100);

        rvm_trainer<kernel_type> rvm_trainer;
        rvm_trainer.set_kernel(kernel_type(gamma));

        krr_trainer<kernel_type> krr_trainer;
        krr_trainer.use_classification_loss_for_loo_cv();
        krr_trainer.set_kernel(kernel_type(gamma));

        svm_pegasos<kernel_type> pegasos_trainer;
        pegasos_trainer.set_kernel(kernel_type(gamma));
        pegasos_trainer.set_lambda(0.00001);

        svm_c_ekm_trainer<kernel_type> ocas_ekm_trainer;
        ocas_ekm_trainer.set_kernel(kernel_type(gamma));
        ocas_ekm_trainer.set_c(100000);

        svm_nu_trainer<kernel_type> trainer;
        trainer.set_kernel(kernel_type(gamma));
        trainer.set_nu(0.05);

        svm_c_trainer<kernel_type> c_trainer;
        c_trainer.set_kernel(kernel_type(gamma));
        c_trainer.set_c(100);

        svm_c_linear_trainer<linear_kernel<matrix<double,0,1> > > lin_trainer;
        lin_trainer.set_c(100000);
        // use an ekm to linearize this dataset so we can use it with the lin_trainer
        empirical_kernel_map<kernel_type> ekm;
        ekm.load(kernel_type(gamma), x);
        for (unsigned long i = 0; i < x.size(); ++i)
            x_linearized.push_back(ekm.project(x[i]));

        print_spinner();
        matrix<scalar_type> rvm_cv = cross_validate_trainer_threaded(rvm_trainer, x,y, 4, 2);
        print_spinner();
        matrix<scalar_type> krr_cv = cross_validate_trainer_threaded(krr_trainer, x,y, 4, 2);
        print_spinner();
        matrix<scalar_type> svm_cv = cross_validate_trainer(trainer, x,y, 4);
        print_spinner();
        matrix<scalar_type> svm_c_cv = cross_validate_trainer(c_trainer, x,y, 4);
        print_spinner();
        matrix<scalar_type> rbf_cv = cross_validate_trainer_threaded(rbf_trainer, x,y, 10, 2);
        print_spinner();
        matrix<scalar_type> lin_cv = cross_validate_trainer_threaded(lin_trainer, x_linearized, y, 4, 2);
        print_spinner();
        matrix<scalar_type> ocas_ekm_cv = cross_validate_trainer_threaded(ocas_ekm_trainer, x, y, 4, 2);
        print_spinner();
        ocas_ekm_trainer.set_basis(randomly_subsample(x, 300));
        matrix<scalar_type> ocas_ekm_cv2 = cross_validate_trainer_threaded(ocas_ekm_trainer, x, y, 4, 2);
        print_spinner();
        matrix<scalar_type> peg_cv = cross_validate_trainer_threaded(batch(pegasos_trainer,1.0), x,y, 4, 2);
        print_spinner();
        matrix<scalar_type> peg_c_cv = cross_validate_trainer_threaded(batch_cached(pegasos_trainer,1.0), x,y, 4, 2);
        print_spinner();

        dlog << LDEBUG << "rvm cv:        " << rvm_cv;
        dlog << LDEBUG << "krr cv:        " << krr_cv;
        dlog << LDEBUG << "nu-svm cv:     " << svm_cv;
        dlog << LDEBUG << "C-svm cv:      " << svm_c_cv;
        dlog << LDEBUG << "rbf cv:        " << rbf_cv;
        dlog << LDEBUG << "lin cv:        " << lin_cv;
        dlog << LDEBUG << "ocas_ekm cv:   " << ocas_ekm_cv;
        dlog << LDEBUG << "ocas_ekm cv2:  " << ocas_ekm_cv2;
        dlog << LDEBUG << "peg cv:        " << peg_cv;
        dlog << LDEBUG << "peg cached cv: " << peg_c_cv;

        // make sure the cached version of pegasos computes the same result
        DLIB_TEST_MSG(sum(abs(peg_cv - peg_c_cv)) < std::sqrt(std::numeric_limits<double>::epsilon()),
                      sum(abs(peg_cv - peg_c_cv)) << "   \n" << peg_cv << peg_c_cv  );

        DLIB_TEST_MSG(mean(rvm_cv) > 0.9, rvm_cv);
        DLIB_TEST_MSG(mean(krr_cv) > 0.9, krr_cv);
        DLIB_TEST_MSG(mean(svm_cv) > 0.9, svm_cv);
        DLIB_TEST_MSG(mean(svm_c_cv) > 0.9, svm_c_cv);
        DLIB_TEST_MSG(mean(rbf_cv) > 0.9, rbf_cv);
        DLIB_TEST_MSG(mean(lin_cv) > 0.9, lin_cv);
        DLIB_TEST_MSG(mean(peg_cv) > 0.9, peg_cv);
        DLIB_TEST_MSG(mean(peg_c_cv) > 0.9, peg_c_cv);
        DLIB_TEST_MSG(mean(ocas_ekm_cv) > 0.9, ocas_ekm_cv);
        DLIB_TEST_MSG(mean(ocas_ekm_cv2) > 0.9, ocas_ekm_cv2);

        const long num_sv = trainer.train(x,y).basis_vectors.size();
        print_spinner();
        const long num_rv = rvm_trainer.train(x,y).basis_vectors.size();
        print_spinner();
        dlog << LDEBUG << "num sv: " << num_sv;
        dlog << LDEBUG << "num rv: " << num_rv;
        print_spinner();
        ocas_ekm_trainer.clear_basis();
        const long num_bv = ocas_ekm_trainer.train(x,y).basis_vectors.size();
        dlog << LDEBUG << "num ekm bv: " << num_bv;

        DLIB_TEST(num_rv <= 17);
        DLIB_TEST_MSG(num_sv <= 45, num_sv);
        DLIB_TEST_MSG(num_bv <= 45, num_bv);

        decision_function<kernel_type> df = reduced2(trainer, 19).train(x,y);
        print_spinner();

        matrix<scalar_type> svm_reduced_error = test_binary_decision_function(df, x, y);
        print_spinner();
        dlog << LDEBUG << "svm reduced test error: " << svm_reduced_error;
        dlog << LDEBUG << "svm reduced num sv: " << df.basis_vectors.size();
        DLIB_TEST(mean(svm_reduced_error) > 0.9);

        svm_cv = cross_validate_trainer(reduced(trainer,30), x,y, 4);
        dlog << LDEBUG << "svm reduced cv: " << svm_cv;
        DLIB_TEST_MSG(mean(svm_cv) > 0.9, svm_cv);

        DLIB_TEST(df.basis_vectors.size() <= 19);
        dlog << LINFO << "   end unittest_binary_classification()";
    }

// ----------------------------------------------------------------------------------------

    template <typename kernel_type>
    struct kernel_der_obj
    {
        typename kernel_type::sample_type x;
        kernel_type k;

        double operator()(const typename kernel_type::sample_type& y) const { return k(x,y); }
    };

    template <typename kernel_type>
    void test_kernel_derivative (
        const kernel_type& k,
        const typename kernel_type::sample_type& x,
        const typename kernel_type::sample_type& y
    )
    {
        kernel_der_obj<kernel_type> obj;
        obj.x = x;
        obj.k = k;
        kernel_derivative<kernel_type> der(obj.k);
        DLIB_TEST(dlib::equal(derivative(obj)(y) , der(obj.x,y), 1e-5));
    }

    void test_kernel_derivative (
    )
    {
        typedef matrix<double, 2, 1> sample_type;

        sigmoid_kernel<sample_type> k1;
        radial_basis_kernel<sample_type> k2;
        linear_kernel<sample_type> k3;
        polynomial_kernel<sample_type> k4(2,3,4);

        offset_kernel<sigmoid_kernel<sample_type> > k5;
        offset_kernel<radial_basis_kernel<sample_type> > k6;

        dlib::rand rnd;

        sample_type x, y;
        for (int i = 0; i < 10; ++i)
        {
            x = randm(2,1,rnd);
            y = randm(2,1,rnd);
            test_kernel_derivative(k1, x, y);
            test_kernel_derivative(k2, x, y);
            test_kernel_derivative(k3, x, y);
            test_kernel_derivative(k4, x, y);
            test_kernel_derivative(k5, x, y);
            test_kernel_derivative(k6, x, y);
        }
    }

// ----------------------------------------------------------------------------------------

    void test_svm_trainer2()
    {
        typedef matrix<double, 2, 1> sample_type;
        typedef linear_kernel<sample_type> kernel_type;

        std::vector<sample_type> samples;
        std::vector<double> labels;

        sample_type samp;
        samp(0) = 1;
        samp(1) = 1;
        samples.push_back(samp);
        labels.push_back(+1);

        samp(0) = 1;
        samp(1) = 2;
        samples.push_back(samp);
        labels.push_back(-1);

        svm_c_trainer<kernel_type> trainer;

        decision_function<kernel_type> df = trainer.train(samples, labels);

        samp(0) = 1;
        samp(1) = 1;
        dlog << LINFO << "test +1 : "<< df(samp);
        DLIB_TEST(df(samp) > 0);
        samp(0) = 1;
        samp(1) = 2;
        dlog << LINFO << "test -1 : "<< df(samp);
        DLIB_TEST(df(samp) < 0);

        svm_nu_trainer<kernel_type> trainer2;
        df = trainer2.train(samples, labels);

        samp(0) = 1;
        samp(1) = 1;
        dlog << LINFO << "test +1 : "<< df(samp);
        DLIB_TEST(df(samp) > 0);
        samp(0) = 1;
        samp(1) = 2;
        dlog << LINFO << "test -1 : "<< df(samp);
        DLIB_TEST(df(samp) < 0);

    }

// ----------------------------------------------------------------------------------------

    class svm_tester : public tester
    {
    public:
        svm_tester (
        ) :
            tester ("test_svm",
                    "Runs tests on the svm/kernel algorithm components.")
        {}

        void perform_test (
        )
        {
            test_kernel_derivative();
            unittest_binary_classification();
            test_clutering();
            test_regression();
            test_anomaly_detection();
            test_svm_trainer2();
        }
    } a;

}


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/firstbitset.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/nbmantissabits.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using p_t = bs::pack<T, N>;
  using iT =  bd::as_integer_t<T, unsigned>;
  using pi_t = bs::pack<iT, N>;

  T a1[N];
  iT b[N];
  for(std::size_t i = 0; i < N; ++i)
  {
     a1[i] = T(i+1);
     b[i] = bs::firstbitset(a1[i]);
   }
  p_t aa1(&a1[0], &a1[0]+N);
  pi_t bb(&b[0], &b[0]+N);
  STF_EQUAL(bs::firstbitset(aa1), bb);
}

STF_CASE_TPL("Check firstbitset on pack" ,  STF_NUMERIC_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" firstbitset real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using p_t = bs::pack<T>;
  using r_t = decltype(firstbitset(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<p_t, unsigned>));

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_EQUAL(firstbitset(bs::Inf<p_t>()), r_t(1ull<<bs::Nbmantissabits<T>()));
  STF_EQUAL(firstbitset(bs::Minf<p_t>()), r_t(1ull<<bs::Nbmantissabits<T>()));
  STF_EQUAL(firstbitset(bs::Nan<p_t>()), bs::One<r_t>());
#endif
  STF_EQUAL(firstbitset(bs::Signmask<p_t>()), bs::One<r_t>()+bs::Valmax<r_t>()/2);
  STF_EQUAL(firstbitset(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for real_

STF_CASE_TPL (" firstbitset signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using p_t = bs::pack<T>;
  using r_t = decltype(firstbitset(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<p_t, unsigned>));

  // specific values tests
  STF_EQUAL(firstbitset(bs::One<p_t>()), bs::One<r_t>());
  STF_EQUAL(firstbitset(bs::Signmask<p_t>()), bs::One<r_t>()+bs::Valmax<r_t>()/2);
  STF_EQUAL(firstbitset(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for signed_int_

STF_CASE_TPL (" firstbitset unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::firstbitset;
  using p_t = bs::pack<T>;
  using r_t = decltype(firstbitset(p_t()));

  // return type conformity test
  STF_TYPE_IS(r_t,(bd::as_integer_t<p_t, unsigned>));

  // specific values tests
  STF_EQUAL(firstbitset(bs::One<p_t>()), bs::One<r_t>());
  STF_EQUAL(firstbitset(bs::Zero<p_t>()), bs::Zero<r_t>());
} // end of test for unsigned_int_


//####### Test module for lookup tables ######################################

//Define Module name
 #define BOOST_TEST_MODULE "lookup tables"

//Will automatically define a main for this test
 #define BOOST_TEST_DYN_LINK

#include <cmath>
#include <functional>

//Include Boost unit tests library & library for floating point comparison
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

//Units choice. Not relevant here, but avoids compile-time warning
#define PXRMP_USE_SI_UNITS

#include "lookup_tables.hpp"

using namespace picsar::multi_physics;

// ------------- Tests --------------

//Tolerance for double precision calculations
const double double_tolerance = 1.0e-10;

//Tolerance for single precision calculations
const float float_tolerance = 1.0e-4;

//Templated tolerance
template <typename T>
T tolerance()
{
    if(std::is_same<T,float>::value)
        return float_tolerance;
    else
        return double_tolerance;
}

//________________________1D tables__________________________________

//Test lookup_1d constructor generic
template<typename T>
void lookup1d_constructor()
{
    picsar_vector<T> coords{static_cast<T>(0.),
                            static_cast<T>(1.),
                            static_cast<T>(2.),
                            static_cast<T>(3.),
                            static_cast<T>(4.)};
    picsar_vector<T> vals{coords.size()};

    std::transform(coords.begin(), coords.end(), vals.begin(),
        [](T val){return static_cast<T>(2.0*val);});

    lookup_1d<T> l1d{coords, vals};

    lookup_1d<T> l1dbis{l1d};

    lookup_1d<T> l1dnull;

    lookup_1d<T> l1copy = l1d;

    for(size_t i = 0; i < coords.size(); ++i){
        BOOST_CHECK_EQUAL(l1d.get_coords()[i], static_cast<T>(coords[i]));
        BOOST_CHECK_EQUAL(l1dbis.get_coords()[i], static_cast<T>(coords[i]));
        BOOST_CHECK_EQUAL(l1copy.get_coords()[i], static_cast<T>(coords[i]));
    }

    BOOST_CHECK_EQUAL(l1d.is_init(),true);
    BOOST_CHECK_EQUAL(l1dbis.is_init(),true);
    BOOST_CHECK_EQUAL(l1dnull.is_init(),false);
    BOOST_CHECK_EQUAL(l1copy.is_init(),true);
}

//Test lookup_1d constructor in double precision
BOOST_AUTO_TEST_CASE( lookup1d_constructor_double_1 )
{
    lookup1d_constructor<double>();
}

//Test lookup_1d constructor in single precision
BOOST_AUTO_TEST_CASE( lookup1d_constructor_single_1 )
{
    lookup1d_constructor<float>();
}

//Test lookup_1d linear equispaced interpolator generic
template<typename T>
void lookup1d_linear_equi_interp()
{
    picsar_vector<T> coords{static_cast<T>(0.),
                            static_cast<T>(1.),
                            static_cast<T>(2.),
                            static_cast<T>(3.),
                            static_cast<T>(4.)};
    picsar_vector<T> vals{coords.size()};

    std::transform(coords.begin(), coords.end(), vals.begin(),
        [](T val){return static_cast<T>(2.0*val);});

    lookup_1d<T> l1d{coords, vals};

    picsar_vector<T> where{static_cast<T>(0.001),
                            static_cast<T>(0.5),
                            static_cast<T>(1.0),
                            static_cast<T>(1.5),
                            static_cast<T>(2.0),
                            static_cast<T>(3.5),
                            static_cast<T>(3.999)};

    for(auto ww: where){
        T res = l1d.interp_linear_equispaced(ww);
        T exp = static_cast<T>(2.0*ww);
        BOOST_CHECK_SMALL((res-exp)/(exp), tolerance<T>());
    }

    BOOST_CHECK_SMALL(l1d.interp_linear_equispaced(static_cast<T>(0.0)), tolerance<T>());

}

//Test lookup_1d constructor in double precision
BOOST_AUTO_TEST_CASE( lookup1d_linear_interp_equi_double_1 )
{
    lookup1d_linear_equi_interp<double>();
}

//Test lookup_1d constructor in single precision
BOOST_AUTO_TEST_CASE( lookup1d_linear_interp_equi_single_1 )
{
    lookup1d_linear_equi_interp<float>();
}

//Test lookup_1d linear interpolator generic
template<typename T>
void lookup1d_linear_interp()
{
    picsar_vector<T> coords{static_cast<T>(0.),
                            static_cast<T>(1.),
                            static_cast<T>(2.),
                            static_cast<T>(3.),
                            static_cast<T>(4.)};
    picsar_vector<T> vals{coords.size()};

    std::transform(coords.begin(), coords.end(), vals.begin(),
        [](T val){return static_cast<T>(2.0*val);});

    lookup_1d<T> l1d{coords, vals};

    picsar_vector<T> where{static_cast<T>(0.0001),
                            static_cast<T>(0.5),
                            static_cast<T>(1.0),
                            static_cast<T>(1.5),
                            static_cast<T>(2.0),
                            static_cast<T>(3.5),
                            static_cast<T>(3.999)};

    for(auto ww: where){
        T res = l1d.interp_linear(ww);
        T exp = static_cast<T>(2.0*ww);
        BOOST_CHECK_SMALL((res-exp)/(exp), tolerance<T>());
    }

    BOOST_CHECK_SMALL(l1d.interp_linear(static_cast<T>(0.0)), tolerance<T>());

}

//Test lookup_1d constructor in double precision
BOOST_AUTO_TEST_CASE( lookup1d_linear_interp_double_1 )
{
    lookup1d_linear_interp<double>();
}

//Test lookup_1d constructor in single precision
BOOST_AUTO_TEST_CASE( lookup1d_linear_interp_single_1 )
{
    lookup1d_linear_interp<float>();
}

//________________________2D tables__________________________________

//Test lookup_2d constructor generic
template<typename T>
void lookup2d_constructor()
{
    picsar_vector<T> c1{static_cast<T>(-3),
                        static_cast<T>(-2),
                        static_cast<T>(-1),
                        static_cast<T>(0),
                        static_cast<T>(1),
                        static_cast<T>(2),
                        static_cast<T>(3)};

    picsar_vector<T> c2{static_cast<T>(-3),
                        static_cast<T>(-2),
                        static_cast<T>(-1),
                        static_cast<T>(0),
                        static_cast<T>(1),
                        static_cast<T>(2),
                        static_cast<T>(3)};

    picsar_vector<T> vals(c1.size()*c2.size());

    size_t pos = 0;

    for (auto cc1: c1){
        for(auto cc2: c2){
            vals[pos++]=(cc1+cc2);
        }
    }

    lookup_2d<T> l2d{picsar_array<picsar_vector<T>,2>{c1,c2}, vals};

    lookup_2d<T> l2dbis{l2d};

    lookup_2d<T> l2dnull;

    lookup_2d<T> l2copy = l2d;

    for(size_t i = 0; i < c1.size(); ++i){
        BOOST_CHECK_EQUAL(l2d.get_coords()[0][i], static_cast<T>(c1[i]));
        BOOST_CHECK_EQUAL(l2dbis.get_coords()[0][i], static_cast<T>(c1[i]));
        BOOST_CHECK_EQUAL(l2copy.get_coords()[0][i], static_cast<T>(c1[i]));
    }

    for(size_t i = 0; i < c2.size(); ++i){
        BOOST_CHECK_EQUAL(l2d.get_coords()[1][i], static_cast<T>(c2[i]));
        BOOST_CHECK_EQUAL(l2dbis.get_coords()[1][i], static_cast<T>(c2[i]));
        BOOST_CHECK_EQUAL(l2copy.get_coords()[1][i], static_cast<T>(c2[i]));
    }

    BOOST_CHECK_EQUAL(l2d.is_init(),true);
    BOOST_CHECK_EQUAL(l2dbis.is_init(),true);
    BOOST_CHECK_EQUAL(l2dnull.is_init(),false);
    BOOST_CHECK_EQUAL(l2copy.is_init(),true);
}

//Test lookup_1d constructor in double precision
BOOST_AUTO_TEST_CASE( lookup2d_constructor_double_1 )
{
    lookup2d_constructor<double>();
}

//Test lookup_1d constructor in single precision
BOOST_AUTO_TEST_CASE( lookup2d_constructor_single_1 )
{
    lookup2d_constructor<float>();
}

//Test lookup_2d linear interpolator generic
template<typename T>
void lookup2d_linear_interpolator()
{
    picsar_vector<T> c1{static_cast<T>(-3),
                        static_cast<T>(-2),
                        static_cast<T>(-1),
                        static_cast<T>(0),
                        static_cast<T>(1),
                        static_cast<T>(2),
                        static_cast<T>(3)};

    picsar_vector<T> c2{static_cast<T>(-3),
                        static_cast<T>(-2),
                        static_cast<T>(-1),
                        static_cast<T>(0),
                        static_cast<T>(1),
                        static_cast<T>(2),
                        static_cast<T>(3)};
    picsar_vector<T> vals(c1.size()*c2.size());

    size_t pos = 0;

    for (auto cc1: c1){
        for(auto cc2: c2){
            vals[pos++]=(cc1+cc2);
        }
    }

    lookup_2d<T> l2d{picsar_array<picsar_vector<T>,2>{c1,c2}, vals};

    picsar_vector<T> c1_test{static_cast<T>(-2.9),
                             static_cast<T>(-2.1),
                             static_cast<T>(1.2),
                             static_cast<T>(0.001),
                             static_cast<T>(2.99)};

    picsar_vector<T> c2_test{static_cast<T>(-1.9),
                             static_cast<T>(-2.7),
                             static_cast<T>(2.1),
                             static_cast<T>(1.1),
                             static_cast<T>(0.7)};

    for(auto cc1: c1_test){
        for(auto cc2: c1_test){
            T res = l2d.interp_linear(cc1, cc2);
            T exp = static_cast<T>(cc1+cc2);
            BOOST_CHECK_SMALL((res-exp)/(exp), tolerance<T>());
        }
    }
    BOOST_CHECK_EQUAL(1,1);
}

//Test lookup_2d linear_interpolator in double precision
BOOST_AUTO_TEST_CASE( lookup2d_linear_interpolator_double_1 )
{
    lookup2d_linear_interpolator<double>();
}

//Test lookup_2d linear_interpolator in single precision
BOOST_AUTO_TEST_CASE( lookup2d_linear_interpolator_single_1 )
{
    lookup2d_linear_interpolator<float>();
}


#ifndef AFFINE_INVARIANT_FEATURES_AFFINE_INVARIANT_FEATURE
#define AFFINE_INVARIANT_FEATURES_AFFINE_INVARIANT_FEATURE

#include <algorithm>
#include <vector>

#include <affine_invariant_features/affine_invariant_feature_base.hpp>
#include <affine_invariant_features/parallel_tasks.hpp>

#include <boost/bind.hpp>
#include <boost/ref.hpp>

#include <opencv2/core.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgproc.hpp>

namespace affine_invariant_features {

//
// AffineInvariantFeature that samples features in various affine transformation space
//

class AffineInvariantFeature : public AffineInvariantFeatureBase {
protected:
  // the private constructor. users must use create() to instantiate an AffineInvariantFeature
  AffineInvariantFeature(const cv::Ptr< cv::Feature2D > detector,
                         const cv::Ptr< cv::Feature2D > extractor, const double nstripes)
      : AffineInvariantFeatureBase(detector, extractor), nstripes_(nstripes) {
    // generate parameters for affine invariant sampling
    phi_params_.push_back(0.);
    tilt_params_.push_back(1.);
    for (int i = 1; i < 6; ++i) {
      const double tilt(std::pow(2., 0.5 * i));
      for (double phi = 0.; phi < 180.; phi += 72. / tilt) {
        phi_params_.push_back(phi);
        tilt_params_.push_back(tilt);
      }
    }
    ntasks_ = phi_params_.size();
  }

public:
  virtual ~AffineInvariantFeature() {}

  //
  // unique interfaces to instantiate an AffineInvariantFeature
  //

  static cv::Ptr< AffineInvariantFeature > create(const cv::Ptr< cv::Feature2D > feature,
                                                  const double nstripes = -1.) {
    return new AffineInvariantFeature(feature, feature, nstripes);
  }

  static cv::Ptr< AffineInvariantFeature > create(const cv::Ptr< cv::Feature2D > detector,
                                                  const cv::Ptr< cv::Feature2D > extractor,
                                                  const double nstripes = -1.) {
    return new AffineInvariantFeature(detector, extractor, nstripes);
  }

  //
  // overloaded functions from AffineInvariantFeatureBase or its base class
  //

  virtual void compute(cv::InputArray image, std::vector< cv::KeyPoint > &keypoints,
                       cv::OutputArray descriptors) {
    // extract the input
    const cv::Mat image_mat(image.getMat());

    // prepare outputs of following parallel processing
    std::vector< std::vector< cv::KeyPoint > > keypoints_array(ntasks_, keypoints);
    std::vector< cv::Mat > descriptors_array(ntasks_);

    // bind each parallel task and arguments
    ParallelTasks tasks(ntasks_);
    for (std::size_t i = 0; i < ntasks_; ++i) {
      tasks[i] = boost::bind(&AffineInvariantFeature::computeTask, this, boost::ref(image_mat),
                             boost::ref(keypoints_array[i]), boost::ref(descriptors_array[i]),
                             phi_params_[i], tilt_params_[i]);
    }

    // do parallel tasks
    cv::parallel_for_(cv::Range(0, ntasks_), tasks, nstripes_);

    // fill the final outputs
    extendKeypoints(keypoints_array, keypoints);
    extendDescriptors(descriptors_array, descriptors);
  }

  virtual void detect(cv::InputArray image, std::vector< cv::KeyPoint > &keypoints,
                      cv::InputArray mask = cv::noArray()) {
    // extract inputs
    const cv::Mat image_mat(image.getMat());
    const cv::Mat mask_mat(mask.getMat());

    // prepare an output of following parallel processing
    std::vector< std::vector< cv::KeyPoint > > keypoints_array(ntasks_);

    // bind each parallel task and arguments
    ParallelTasks tasks(ntasks_);
    for (std::size_t i = 0; i < ntasks_; ++i) {
      tasks[i] = boost::bind(&AffineInvariantFeature::detectTask, this, boost::ref(image_mat),
                             boost::ref(mask_mat), boost::ref(keypoints_array[i]), phi_params_[i],
                             tilt_params_[i]);
    }

    // do parallel tasks
    cv::parallel_for_(cv::Range(0, ntasks_), tasks, nstripes_);

    // fill the final output
    extendKeypoints(keypoints_array, keypoints);
  }

  virtual void detectAndCompute(cv::InputArray image, cv::InputArray mask,
                                std::vector< cv::KeyPoint > &keypoints, cv::OutputArray descriptors,
                                bool useProvidedKeypoints = false) {
    // just compute descriptors if the keypoints are provided
    if (useProvidedKeypoints) {
      compute(image, keypoints, descriptors);
      return;
    }

    // extract inputs
    const cv::Mat image_mat(image.getMat());
    const cv::Mat mask_mat(mask.getMat());

    // prepare outputs of following parallel processing
    std::vector< std::vector< cv::KeyPoint > > keypoints_array(ntasks_);
    std::vector< cv::Mat > descriptors_array(ntasks_);

    // bind each parallel task and arguments
    ParallelTasks tasks(ntasks_);
    for (std::size_t i = 0; i < ntasks_; ++i) {
      tasks[i] =
          boost::bind(&AffineInvariantFeature::detectAndComputeTask, this, boost::ref(image_mat),
                      boost::ref(mask_mat), boost::ref(keypoints_array[i]),
                      boost::ref(descriptors_array[i]), phi_params_[i], tilt_params_[i]);
    }

    // do parallel tasks
    cv::parallel_for_(cv::Range(0, ntasks_), tasks, nstripes_);

    // fill the final outputs
    extendKeypoints(keypoints_array, keypoints);
    extendDescriptors(descriptors_array, descriptors);
  }

  virtual cv::String getDefaultName() const { return "AffineInvariantFeature"; }

protected:
  void computeTask(const cv::Mat &src_image, std::vector< cv::KeyPoint > &keypoints,
                   cv::Mat &descriptors, const double phi, const double tilt) const {
    // apply the affine transformation to the image on the basis of the given parameters
    cv::Mat image(src_image.clone());
    cv::Matx23f affine;
    warpImage(image, affine, phi, tilt);

    // apply the affine transformation to keypoints
    transformKeypoints(keypoints, affine);

    // extract descriptors on the skewed image and keypoints
    CV_Assert(extractor_);
    extractor_->compute(image, keypoints, descriptors);

    // invert keypoints
    invertKeypoints(keypoints, affine);
  }

  void detectTask(const cv::Mat &src_image, const cv::Mat &src_mask,
                  std::vector< cv::KeyPoint > &keypoints, const double phi,
                  const double tilt) const {
    // apply the affine transformation to the image on the basis of the given parameters
    cv::Mat image(src_image.clone());
    cv::Matx23f affine;
    warpImage(image, affine, phi, tilt);

    // apply the affine transformation to the mask
    cv::Mat mask(src_mask.empty() ? cv::Mat(src_image.size(), CV_8UC1, 255) : src_mask.clone());
    warpMask(mask, affine, image.size());

    // detect keypoints on the skewed image and mask
    CV_Assert(detector_);
    detector_->detect(image, keypoints, mask);

    // invert keypoints
    invertKeypoints(keypoints, affine);
  }

  void detectAndComputeTask(const cv::Mat &src_image, const cv::Mat &src_mask,
                            std::vector< cv::KeyPoint > &keypoints, cv::Mat &descriptors,
                            const double phi, const double tilt) const {
    // apply the affine transformation to the image on the basis of the given parameters
    cv::Mat image(src_image.clone());
    cv::Matx23f affine;
    warpImage(image, affine, phi, tilt);

    // if keypoints are not provided, first apply the affine transformation to the mask
    cv::Mat mask(src_mask.empty() ? cv::Mat(src_image.size(), CV_8UC1, 255) : src_mask.clone());
    warpMask(mask, affine, image.size());

    // detect keypoints on the skewed image and mask
    // and extract descriptors on the image and keypoints
    if (detector_ == extractor_) {
      CV_Assert(detector_);
      detector_->detectAndCompute(image, mask, keypoints, descriptors, false);
    } else {
      CV_Assert(detector_);
      CV_Assert(extractor_);
      detector_->detect(image, keypoints, mask);
      extractor_->compute(image, keypoints, descriptors);
    }

    // invert the positions of the detected keypoints
    invertKeypoints(keypoints, affine);
  }

  static void warpImage(cv::Mat &image, cv::Matx23f &affine, const double phi, const double tilt) {
    // initiate output
    affine = cv::Matx23f::eye();

    if (phi != 0.) {
      // rotate the source frame
      affine = cv::getRotationMatrix2D(cv::Point2f(0., 0.), phi, 1.);
      cv::Rect tmp_rect;
      {
        std::vector< cv::Point2f > corners(4);
        corners[0] = cv::Point2f(0., 0.);
        corners[1] = cv::Point2f(image.cols, 0.);
        corners[2] = cv::Point2f(image.cols, image.rows);
        corners[3] = cv::Point2f(0., image.rows);
        std::vector< cv::Point2f > tmp_corners;
        cv::transform(corners, tmp_corners, affine);
        tmp_rect = cv::boundingRect(tmp_corners);
      }

      // cancel the offset of the rotated frame
      affine(0, 2) = -tmp_rect.x;
      affine(1, 2) = -tmp_rect.y;

      // apply the final transformation to the image
      cv::warpAffine(image, image, affine, tmp_rect.size(), cv::INTER_LINEAR, cv::BORDER_REPLICATE);
    }
    if (tilt != 1.) {
      // shrink the image in width
      cv::GaussianBlur(image, image, cv::Size(0, 0), 0.8 * std::sqrt(tilt * tilt - 1.), 0.01);
      cv::resize(image, image, cv::Size(0, 0), 1. / tilt, 1., cv::INTER_NEAREST);
      affine(0, 0) /= tilt;
      affine(0, 1) /= tilt;
      affine(0, 2) /= tilt;
    }
  }

  static void warpMask(cv::Mat &mask, const cv::Matx23f &affine, const cv::Size size) {
    if (affine == cv::Matx23f::eye()) {
      return;
    }
    cv::warpAffine(mask, mask, affine, size, cv::INTER_NEAREST);
  }

  static void transformKeypoints(std::vector< cv::KeyPoint > &keypoints,
                                 const cv::Matx23f &affine) {
    if (affine == cv::Matx23f::eye()) {
      return;
    }
    for (std::vector< cv::KeyPoint >::iterator keypoint = keypoints.begin();
         keypoint != keypoints.end(); ++keypoint) {
      // convert cv::Point2f to cv::Mat (1x1,2ch) without copying data.
      // this is required because cv::transform does not accept cv::Point2f.
      cv::Mat pt(cv::Mat(keypoint->pt, false).reshape(2));
      cv::transform(pt, pt, affine);
    }
  }

  static void invertKeypoints(std::vector< cv::KeyPoint > &keypoints, const cv::Matx23f &affine) {
    if (affine == cv::Matx23f::eye()) {
      return;
    }
    cv::Matx23f invert_affine;
    cv::invertAffineTransform(affine, invert_affine);
    transformKeypoints(keypoints, invert_affine);
  }

  static void extendKeypoints(const std::vector< std::vector< cv::KeyPoint > > &src,
                              std::vector< cv::KeyPoint > &dst) {
    dst.clear();
    for (std::size_t i = 0; i < src.size(); ++i) {
      dst.insert(dst.end(), src[i].begin(), src[i].end());
    }
  }

  void extendDescriptors(const std::vector< cv::Mat > &src, cv::OutputArray dst) const {
    // create the output array
    int nrows(0);
    for (std::size_t i = 0; i < src.size(); ++i) {
      nrows += src[i].rows;
    }
    dst.create(nrows, descriptorSize(), descriptorType());

    // fill the output array
    cv::Mat dst_mat(dst.getMat());
    int rows(0);
    for (std::size_t i = 0; i < src.size(); ++i) {
      if (src[i].rows > 0) {
        src[i].copyTo(dst_mat.rowRange(rows, rows + src[i].rows));
        rows += src[i].rows;
      }
    }
  }

protected:
  std::vector< double > phi_params_;
  std::vector< double > tilt_params_;
  std::size_t ntasks_;
  const double nstripes_;
};

} // namespace affine_invariant_features

#endif