/* Copyright (C) 2017 IBM Corp.
 *  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.
 */
/*****************************************************************************
 * GGH15.h - an implementation of the graph-based multilinear maps from
 *           [Gentry-Gorbunov-Halevi, TCC 2015]
 *****************************************************************************/
#include <stdexcept>
#include <unistd.h>
#include <sys/stat.h>

#if defined(__unix__) || defined(__unix) || defined(unix)
#include <sys/time.h>
#include <sys/resource.h>
#endif

#include <NTL/BasicThreadPool.h>

#include "mat_l.h"
#include "GGH15.h"
#include "utils/tools.h"

bool bGGH15threading=1;

//#define DEBUGPRINT
//#define PRINTDOT
#ifdef PRINTDOT
#define printDot cerr << "." << std::flush
#else
#define printDot
#endif

// A lower bound on |q|/2^10 for an L-edge chain. We need to bound the size
// of a product of L matrices of dim m-by-m, where L-1 have entries drawn
// with parameter sqrt{sigmaX) and one has entries with parameter sec.
// The formula we use is q/2^10 > largest-singular-val(product), which
// translates to
//    q/2^10 > (2*sqrt(m)*sqrt(sigmaX))^{L-1} * lambda*sqrt(m+n)
//      \approx lambda * sigmaX^{(L-1)/2} * m^{L/2} * 2^L
long qBound(long sigmaX, long m, long L)
{
  // cerr << "@ L="<<L<<", sigmaX="<<sigmaX
  //      <<",log_2(sqrt(sigmaX))="<<(log(sigmaX)/(log(2)*2))
  //      <<", log_2(m)="<<(log(m)/log(2)) << endl;

  double logQ = ((L-1)*log(sigmaX) + L*log(m))/2.0;
  return ceil(logQ/log(2.0)) + L + 16;
}

// Computing basic parameters for length-'diam' MMAPs. In particular,
// assuming that "fresh" sampled encodings use "varince" sigmaX, we need
// to ensure that
//  (a) log q > qBound(sigmaX, m, L, sec)      (qBound from above)
//  (b)     m > mBound(log q, errBits=7, sec)  (mBound from TDMatrixParams)
//
// Since the size of q is determined by the number of small factors, i.e.,
// q = \prod_{i=0}^{k-1} p_i^e, we just try to find the smallest k for which
// the above constraints are satisfied.
//
// In addition, we also need to ensure that m >= n*k*e + mBar, where
// n = nStates + ceil(sqrt(sec)/2) and mBar = mbarBound(log q, n, sec)
// (from TDMatrixParams).

GGH15basicParams::GGH15basicParams(COMP_PARAMS_TYPE comp, //=COMP_GGH15PRMS
                                   long nStates, long L, long sec, long ee)
{
    FHE_TIMER_START;

    e = ee;
    n = nStates + ceil(sqrt(sec)/2);

    // k will never be smaller than L
    for (k=L; k<TDMATRIX_NUM_SMALL_FACTORS; k++) {
      long qBits = get_qBits(k, e);
      m = mBound(qBits, /*errBits=*/7, sec);

      long mBar = mbarBound(qBits, n, sec);
      long wLen = n*k*e;
      if (m < mBar+wLen) m = mBar+wLen;
      else               mBar = m-wLen;

      long sigmaX = getsigmaXVal(wLen, mBar, /*maxFactor=*/181);
      long qbound = qBound(sigmaX,m,L);

      if (qBits >= qbound) // all constraints are satisfied
	break;

      // otherwise we try a larger value of k
    }
    if (k >= TDMATRIX_NUM_SMALL_FACTORS) { // could not find a solution
      throw std::logic_error("Cannot set GGH15 parameters");
    }
}

// Initialize a single node, choose A w/ trapdoor and T, Tinv
void GGH15node::initGGH15node(TDMatrixParams& prms, long idx)
{
    #ifdef DEBUGPRINT
        fprintf(stderr,"initGGH15node %s %d\n", __FILE__, __LINE__);
    #endif // DEBUGPRINT

    FHE_TIMER_START;
    index = idx;
    params = &prms;

    A.initTDmatrix(prms); // Choose matrix A with trapdoor

    #ifdef DEBUGPRINT
        fprintf(stderr,"initGGH15node %s %d\n", __FILE__, __LINE__);
    #endif // DEBUGPRINT

    // Choose a small n-by-n matrix P, set Pinv as its inverse mod q
    Pinv.params = &prms;
    do {
      setSmall(P, prms.n, prms.n, /*howSmall=*/prms.maxFactor);
    #ifdef DEBUGPRINT
        fprintf(stderr,"initGGH15node %s %d\n", __FILE__, __LINE__);
    #endif // DEBUGPRINT
    }
    while (!Pinv.invert(P)); // Repeat until you get an invertible P mod q

    #ifdef DEBUGPRINT
        fprintf(stderr,"initGGH15node %s %d\n", __FILE__, __LINE__);
    #endif // DEBUGPRINT

    // choose T, Tinv as random inverses of each other mod q
    T.params = Tinv.params = &prms;
    generateMatrixPair(T, Tinv, prms.m);
}

// Initilize an instance with n nodes
GGH15mmaps::GGH15mmaps(TDMatrixParams& p, long n)
{
    FHE_TIMER_START;
    assert (n>0);

    params = &p;

    nodes.SetLength(n-2);

    //the flag allows parallelizing when it is set to false
    NTL_GEXEC_RANGE(!bGGH15threading, n-2, first, last)

    for (long i=first; i< last; i++)
    {
#ifdef DEBUGPRINT
        cout << "initializing node:" << i << endl;
#if (defined(__unix__) || defined(__unix) || defined(unix))
        struct rusage rusage;
        getrusage( RUSAGE_SELF, &rusage );
        cout << "  rusage.ru_maxrss="<<rusage.ru_maxrss << endl;
#endif
#endif // DEBUGPRINT

        nodes[i].initGGH15node(p, i+1);

#ifdef DEBUGPRINT
        cout << "after initializing node:" << i << endl;
#endif // DEBUGPRINT
    }
    NTL_GEXEC_RANGE_END

#ifdef DEBUGPRINT
    cout << "before randomfill:"  << endl;
#endif // DEBUGPRINT

    // The right bracket is a random non-small n-by-1 matrix
    randomFill(An, /*rows=*/p.n, /*cols=*/1, p);

#ifdef DEBUGPRINT
    cout << "after randomfill:"  << endl;
#endif // DEBUGPRINT
}

long GGH15mmaps::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = params->writeToFile(handle);
    count += An.writeToFile(handle);
    long nNodes = numNodes()-2;
    count += fwrite(&nNodes,sizeof(long),1,handle);
    for (long i = 0; i < nNodes; i++)
    {
        count+= nodes[i].writeToFile(handle);
    }
    return count;
}

long GGH15mmaps::readFromFile(FILE* handle, TDMatrixParams* prmBuf)
{
    FHE_TIMER_START;
    assert(params != NULL || prmBuf != NULL); // some pointer must be provided

    long count=0;

    if (prmBuf != NULL)
    {
        TDMatrixParams p;
        count = p.readFromFile(handle);
        assert(p == *prmBuf); // sanity check
        params = prmBuf;      // point to given params
    }
    else
        count = params->readFromFile(handle); // overwrite params from input

    count += An.readFromFile(handle,params);

    // Read the actual nodes from disk, one at a time

    long nNodes;
    count += fread(&nNodes,sizeof(nNodes),1,handle);

    nodes.SetLength(nNodes);
    for (long i = 0; i < (nNodes); i++)
    {
        count+= nodes[i].readFromFile(handle, params);
    }
    return count;
}

// Write one GGH15 node to output
long GGH15node::writeToActualFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = params->writeToFile(handle);

    count += fwrite(&index, sizeof(index),1, handle);
    count += A.writeToFile(handle);

    count += ::writeToFile(P,handle);
    count += T.writeToFile(handle);
    count += Tinv.writeToFile(handle);
    count += Pinv.writeToFile(handle);

    return count;
}

long GGH15node::readFromFile(FILE* handle, TDMatrixParams* prmBuf)
{
    FHE_TIMER_START;
    assert(params != NULL || prmBuf != NULL); // some pointer must be provided

    long count = 0;
    if (prmBuf != NULL)
    {
        TDMatrixParams p;
        count = p.readFromFile(handle);
        assert(p == *prmBuf); // sanity check
        params = prmBuf;     // point to given params
    }
    else
        count = params->readFromFile(handle); // overwrite params from input

    count += fread(&index, sizeof(index),1, handle);
    count += A.readFromFile(handle, params);

    count += ::readFromFile(P,handle);
    count += T.readFromFile(handle, params);

    count += Tinv.readFromFile(handle, params);
    count += Pinv.readFromFile(handle, params);

    return count;
}

long GGH15encoding::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fwrite(&from, sizeof(from),1, handle);
    count += fwrite(&to, sizeof(to),1, handle);

    count += data.writeToFile(handle);

    return count;
}

long GGH15encoding::readFromFile(FILE* handle, TDMatrixParams& prms)
{
    FHE_TIMER_START;
    long count = fread(&from, sizeof(from),1, handle);
    count += fread(&to, sizeof(to),1, handle);
    return count + data.readFromFile(handle, &prms);
}

/*************************************************************/
/** Encding routines: there are four such routines,         **/
/** depending on whether or not i=0 and whether or not j=n. **/
/*************************************************************/

/* GGH15 encoding for 0<i,j<n. Set C = Tinv_i * CC * T_j, where
 * CC satisfies A_i*CC = (Pinv_i*S*P_j)*A_j+E (mod q). We assume
 * that toA was already multiplied by P_j.
 */
static void encodeGGH15(CRTmatrix& C, const mat_l& S,
                        const GGH15node& from, const GGH15node& to,
                        const CRTmatrix& toA)
{
    FHE_TIMER_START;
    const TDMatrixParams* prms = from.getPrms();

    // Set B = (Pinv_i x S x P_j) x A + E

    CRTmatrix B = from.getPinv(); // Pinv_i
    B *= S;                       // Pinv_i x S
    B *= toA;                     // (Pinv_i x S x P_j) x A

    mat_l E;  // Choose a small n-by-m noise matrix E
    setSmall(E, S.NumRows(), toA.NumCols(), /*sigma=*/prms-> r);

    B += E;  // (Pinv_i x S x P_j) x A  + E

    // Sample small C s.t. fromA x CC = B
    mat_l CC(INIT_SIZE, prms->m, B.NumCols());

    #ifdef DEBUGPRINT
        cout << "encoding" << endl;
    #endif
    //NTL_GEXEC_RANGE(!bGGH15threading, B.NumCols(), first, last)
    GEXEC_RANGE(!bGGH15threading, B.NumCols(), first, last)
    for (long j=first; j< last; j++)
    {
        // Sample vector x_j s.t. A*x_j = j'th column of B
        Vec<vec_zz_p> colMod;
        B.getColumn(colMod, j); // The j'th column of B in CRT format

        vec_l sampledColumn;    // this is where we put the vector x_j

        from.getA().sampleWithTrapdoor(sampledColumn, colMod);// sample a column

        for (long i=0; i<CC.NumRows(); i++)                    // store it in CC
            CC[i][j] = sampledColumn[i];
    }
    //NTL_GEXEC_RANGE_END
    GEXEC_RANGE_END(!bGGH15threading)

    // Compute C = Tinv_i * CC * T_j
    C = from.getTinv();
    C *= CC;
    C *= to.getT();
}
//------------------------------------------------------------------//

/* GGH15 encoding for i=0<j<n. Set C =(S*P_j*A_j +E) *T_j (mod q).
 * We assume that toA it was already multiplied by P_j.
 */
static void encodeGGH15first(CRTmatrix& C, const mat_l& S,
                             const GGH15node& to, const CRTmatrix& toA)
{
    FHE_TIMER_START;
    const TDMatrixParams* prms = to.getPrms();

    // Set C = S x P_j x A + E
    const mat_l& tmpS = S;

    mat_l E;  // Choose a small 1-by-m noise matrix E
    setSmall(E, tmpS.NumRows(), toA.NumCols(), /*sigma=*/prms->r);

    C = toA;            // P_j x A
    C.leftMultBy(tmpS); // S x P_j x A
    C += E;             // S x P_j x A + E

    C *= to.getT();
}
//------------------------------------------------------------------//

/* GGH15 encoding for 0<i<j=n. Set C = Tinv_i * CC, where CC
 * satisfies A_i*CC = (Pinv_i*S)*A_n + E (mod q).
 */
static void encodeGGH15last(CRTmatrix& C, const mat_l& S,
                            const GGH15node& from, const CRTmatrix& toA)
{
    FHE_TIMER_START;
    const TDMatrixParams* prms = from.getPrms();

    // Set B = (Pinv_i x S) x A_n + E

    CRTmatrix B = from.getPinv(); // Pinv_i
    B *= S;                       // Pinv_i x S
    B *= toA;                     // Pinv_i x S x A_n

    mat_l E;  // Choose a small n-by-m noise matrix E
    setSmall(E, B.NumRows(), B.NumCols(), /*sigma=*/prms->r);

    B += E;                       // Pinv_i x S x A_n + E

    // Sample small CC s.t. fromA x CC = B
    mat_l CC(INIT_SIZE, prms->m, B.NumCols());
    for (long j=0; j<B.NumCols(); j++)
    {
        // Sample vector x_j s.t. A*x_j = j'th column of B

        Vec<vec_zz_p> colMod;
        B.getColumn(colMod, j); // The j'th column of B in CRT format

        vec_l sampledColumn;    // this is where we put the vector x_j

        from.getA().sampleWithTrapdoor(sampledColumn, colMod);// sample a column
        for (long i=0; i<CC.NumRows(); i++)                    // store it in CC
            CC[i][j] = sampledColumn[i];
    }

    // Compute C = Tinv_i * CC
    C = from.getTinv();
    C *= CC;
}
//------------------------------------------------------------------//

/* GGH15 encoding for i=0<j=n, set C =(S*A_n +E) (mod q).
 */
static void encodeGGH15_0_n(CRTmatrix& C, const mat_l& S, const CRTmatrix& toA)
{
    FHE_TIMER_START;
    const mat_l& tmpS = S;

    C = toA;            // A_n
    C.leftMultBy(tmpS); // u x S x A

    mat_l E;  // Choose a small 1-by-m noise matrix E
    setSmall(E, tmpS.NumRows(), toA.NumCols(), /*sigma=*/toA.params->r);

    C += E;             // u x S x A + E
}

bool encodeMatrix(const std::string& dirName, GGH15encoding& C, const mat_l& S,
		  long i, long j, GGH15node* iNode, GGH15node* jNode,
		  CRTmatrix& An, TaggedCRTmatrix* toAptr,
		  TDMatrixParams *params, int nNodes)

{
#ifdef DEBUGPRINT
    cout << "begin encode matrix" << endl;
#endif // DEBUGPRINT

    FHE_TIMER_START;

    #ifdef DEBUGPRINT
        cout << "j=" << ToString(j) << endl;
    #endif

    // FIXME: should we enforce i<j ??
    assert(i>=0 && i<nNodes-1);
    assert(j>0  && j<nNodes);

    // Get the destination matrix A in explicit CRT form
    TaggedCRTmatrix toA(params);
    if (toAptr==NULL) toAptr = &toA;

    bool isLast = (nNodes-1 == j);

#ifdef DEBUGPRINT
    cout << "after reading jNode" << endl;
    //getcwd(cwd, sizeof(cwd));
    //cout << "read node info, current directory = " << cwd << endl;
#endif

    // If the caller did not supply a good toA, then compute it now
    if (toAptr->tag != j && !isLast)   // If target is NOT the last node
    {
        jNode->getCRTA(toAptr->data);
        toAptr->data.leftMultBy( jNode->getP() ); // set toA = P_j x A_j
        toAptr->tag = j;
    }

    // Do the actual encoding
    C.setFrom(i);
    C.setTo(j);
    printDot;
    if (i == 0)    // encoding wrt 0 -> ...
    {
      if (isLast)  // encoding wrt 0 -> n
      {
#ifdef DEBUGPRINT
	cout << "encoding encodeGGH15_0_n" << endl;
#endif
	encodeGGH15_0_n(C.getData(), S, An);
      }
      else         // encoding wrt 0 -> j<n
      {
#ifdef DEBUGPRINT
	cout << "encoding first" << endl;
#endif
	encodeGGH15first(C.getData(), S, *jNode, toAptr->data);
      }
    }
    else           // encoding wrt 0<i -> ...
    {

      if (isLast)  // encoding wrt 0<i -> n
      {
#ifdef DEBUGPRINT
	cout << "Encoding last" << endl;
#endif
	encodeGGH15last(C.getData(), S, *iNode, An);
      }
      else         // encoding wrt 0<i -> j<n
      {
	encodeGGH15(C.getData(), S, *iNode, *jNode, toAptr->data);
      }
    }
    return true;
}

bool readEncodeMatrix(const std::string& dirName, GGH15encoding& C, const mat_l& S, long i, long j,
                  TaggedCRTmatrix* toAptr, TDMatrixParams *params, int nNodes)
{
#ifdef DEBUGPRINT
    cout << "begin encode matrix" << endl;
#endif // DEBUGPRINT

    FHE_TIMER_START;
    // FIXME: should we enforce i<j ??
    assert(i>=0 && i<nNodes-1);
    assert(j>0  && j<nNodes);

    // Get the destination matrix A in explicit CRT form
    TaggedCRTmatrix toA(params);
    if (toAptr==NULL) toAptr = &toA;

    bool isLast = (nNodes-1 == j);

    GGH15node jNode, iNode;

#ifdef DEBUGPRINT
    char cwd[1024];
    getcwd(cwd, sizeof(cwd));
    cout << "read node info, current directory = " << cwd << endl;
    cout << "reading jNode" << endl;
#endif

//  load the correct node
    std::string fileName = "node"+ToString(j)+ ".dat";
    FILE* handle = fopen(fileName.c_str(), "rb");
    if (handle != 0)
    {
        jNode.readFromFile(handle, params);
        fclose(handle);
    }

#ifdef DEBUGPRINT
    cout << "after reading jNode" << endl;
    getcwd(cwd, sizeof(cwd));
    cout << "read node info, current directory = " << cwd << endl;
#endif
    //load An
    CRTmatrix An;// The An matrix (actually n-by-1 "matrix")
    fileName = "nAn.dat";
    handle = fopen(fileName.c_str(), "rb");
    if (handle != 0)
    {
        long n;
        long count = fread(&n, sizeof(n),1, handle);
        count+=An.readFromFile(handle, params);
        fclose(handle);
    }
    else NTL::Error("Cannot read An");

    // If the caller did not supply a good toA, then compute it now
    if (toAptr->tag != j && !isLast)   // If target is NOT the last node
    {
        jNode.getCRTA(toAptr->data);
        toAptr->data.leftMultBy( jNode.getP() ); // set toA = P_j x A_j
        toAptr->tag = j;
    }

    // Do the actual encoding
    C.setFrom(i);
    C.setTo(j);
    printDot;
    if (i == 0)    // encoding wrt 0 -> ...
    {
      if (isLast)  // encoding wrt 0 -> n
      {
#ifdef DEBUGPRINT
	cout << "encoding encodeGGH15_0_n" << endl;
#endif
	encodeGGH15_0_n(C.getData(), S, An);
      }
      else         // encoding wrt 0 -> j<n
      {
#ifdef DEBUGPRINT
	cout << "encoding first" << endl;
#endif
	encodeGGH15first(C.getData(), S, jNode, toAptr->data);
      }
    }
    else           // encoding wrt 0<i -> ...
    {
      //get iNode - we are already in the correct directory
      fileName = "node"+ToString(i)+ ".dat";
      handle = fopen(fileName.c_str(), "rb");
      if (handle != 0)
      {
	iNode.readFromFile(handle , params);
	fclose(handle);
      }
      if (isLast)  // encoding wrt 0<i -> n
      {
#ifdef DEBUGPRINT
	cout << "Encoding last" << endl;
#endif
	encodeGGH15last(C.getData(), S, iNode, An);
      }
      else         // encoding wrt 0<i -> j<n
      {
	encodeGGH15(C.getData(), S, iNode, jNode, toAptr->data);
      }
    }
    return true;
}

// Encode plaintext matrix wrt the path i -> j. The toAptr argument is
// an optimization:
//  + If toAptr is non-NULL and toAptr->tag==j, then it is assumed
//    that to toAptr->data is the matrx P_j x A_j.
//  + If toAptr is non-NULL and toAptr->tag!=j, then toA->data is set
//    P_j x A_j and toAptr->tag is set to j.
bool GGH15mmaps::encode(GGH15encoding& C, const mat_l& S, long i, long j,
                        TaggedCRTmatrix* toAptr)
{
    FHE_TIMER_START;
    // FIXME: should we enforce i<j ??
    assert(i>=0 && i<numNodes()-1);
    assert(j>0  && j<numNodes());

    // Get the destination matrix A in explicit CRT form
    TaggedCRTmatrix toA(params);
    if (toAptr==NULL) toAptr = &toA;

    bool isLast = (numNodes()-1 == j);

    // If the caller did not supply a good toA, then compute it now
    if (toAptr->tag != j && !isLast)   // If target is NOT the last node
    {
        nodes[j-1].getCRTA(toAptr->data);
        toAptr->data.leftMultBy( nodes[j-1].getP() ); // set toA = P_j x A_j
        toAptr->tag = j;
    }

    // Do the actual encoding
    C.from = i;
    C.to = j;

    printDot;
    if (i == 0)    // encoding wrt 0 -> ...
    {
        if (isLast)  // encoding wrt 0 -> n
            encodeGGH15_0_n(C.data, S, An);

        else         // encoding wrt 0 -> j<n
            encodeGGH15first(C.data, S, nodes[j-1], toAptr->data);
    }
    else           // encoding wrt 0<i -> ...
    {

        if (isLast)  // encoding wrt 0<i -> n
            encodeGGH15last(C.data, S, nodes[i-1], An);

        else         // encoding wrt 0<i -> j<n
            encodeGGH15(C.data, S, nodes[i-1], nodes[j-1], toAptr->data);

    }
    return true;
}

// A debugging method to check that an encoding was done right
bool GGH15mmaps::verifyEncoding(const GGH15encoding& C,	const mat_l& S) const
{
    FHE_TIMER_START;
    long i = C.fromNode();
    long j = C.toNode();

    CRTmatrix CC(params), SS(params), toA(params), fromA(params);
    if (i!=0)
    {
        nodes[i-1].getCRTA(fromA);

        if (j!=(numNodes()-1))   // 0<i -> j<n
        {
            nodes[j-1].getCRTA(toA);

            CC = nodes[i-1].getT();
            CC *= C.data;
            CC *= nodes[j-1].getTinv();

            SS = nodes[i-1].getPinv();
            SS *= S;
            SS *= nodes[j-1].getP();
        }
        else                 // 0<i -> j=n
        {
            toA = An;

            CC = nodes[i-1].getT();
            CC *= C.data;

            SS = nodes[i-1].getPinv();
            SS *= S;
        }
    }
    else // i==0
    {
        if (j!=(numNodes()-1))   // 0=i -> j<n
        {
            nodes[j-1].getCRTA(toA);

            CC = C.data;
            CC *= nodes[j-1].getTinv();

            SS = nodes[j-1].getP();
            SS.leftMultBy(S);
        }
        else                 // 0=i -> j=n
        {
            toA = An;
            CC = C.data;
            SS = S;
        }
    }

    if (i>0)   // Check that CC and EE = fromA*CC - SS*toA are small
    {
        fromA *= CC;
        SS *= toA;
        fromA -= SS;
        return (CC.isSmall() && fromA.isSmall());
    }
    else   // Check that EE = CC - SS*toA is small
    {
        SS *= toA;
        CC -= SS;
        return CC.isSmall();
    }
}

bool operator==(const GGH15mmaps& A, const GGH15mmaps& B)
{
    FHE_TIMER_START;

    if (A.getParams()!=B.getParams())
        return false;

    if (A.numNodes()!=B.numNodes())
        return false;

    for (long iNode = 0; iNode < (A.numNodes()-2); iNode++)
    {
        if (A.getNode(iNode)!=B.getNode(iNode))
            return false;
    }

    if (A.getAn()!=B.getAn())
        return false;

    return true;
}

// checks whether two nodes are identical, i.e., have the exact same
// variable values in them. if so, returns true.

bool operator==(const GGH15node& A, const GGH15node& B)
{
    if (A.getIndex()!=B.getIndex())
        return false;
    if (A.getPrms()!=B.getPrms())
        return false;
    if (A.getA()!=B.getA())
        return false;
    if (A.getP()!=B.getP())
        return false;
    if (A.getPinv()!=B.getPinv())
        return false;
    if (A.getT()!=B.getT())
        return false;
    if (A.getTinv()!=B.getTinv())
        return false;

    return true;
}


#ifndef BOOST_NUMERIC_AUTOMATIC_HPP
#define BOOST_NUMERIC_AUTOMATIC_HPP

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

//  Copyright (c) 2012 Robert Ramey
//
// 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)

// policy which creates expanded results types designed
// to avoid overflows.

#include <limits>
#include <cstdint>     // (u)intmax_t,
#include <type_traits> // conditional
#include <boost/integer.hpp>

#include "safe_common.hpp"
#include "checked_result.hpp"
#include "checked_default.hpp"
#include "checked_integer.hpp"
#include "checked_result_operations.hpp"
#include "interval.hpp"
#include "utility.hpp"

namespace boost {
namespace safe_numerics {

struct automatic {
private:
    // the following returns the "true" type.  After calculating the new max and min
    // these return the minimum size type which can hold the expected result.
    struct defer_stored_signed_lazily {
        template<std::intmax_t Min, std::intmax_t Max>
        using type = utility::signed_stored_type<Min, Max>;
    };

    struct defer_stored_unsigned_lazily {
        template<std::uintmax_t Min, std::uintmax_t Max>
        using type = utility::unsigned_stored_type<Min, Max>;
    };

    template<typename T, T Min, T Max>
    struct result_type {
        using type = typename std::conditional<
            std::numeric_limits<T>::is_signed,
            defer_stored_signed_lazily,
            defer_stored_unsigned_lazily
        >::type::template type<Min, Max>;
    };

public:
    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct addition_result {
        using temp_base_type = typename std::conditional<
            // if both arguments are unsigned
            ! std::numeric_limits<T>::is_signed
            && ! std::numeric_limits<U>::is_signed,
            // result is unsigned
            std::uintmax_t,
            // otherwise result is signed
            std::intmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type r_interval = t_interval + u_interval;

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct subtraction_result {
        // result of subtraction are always signed.
        using temp_base_type = intmax_t;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type r_interval = t_interval - u_interval;

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct multiplication_result {
        using temp_base_type = typename std::conditional<
            // if both arguments are unsigned
            ! std::numeric_limits<T>::is_signed
            && ! std::numeric_limits<U>::is_signed,
            // result is unsigned
            std::uintmax_t,
            // otherwise result is signed
            std::intmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type r_interval = t_interval * u_interval;

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct division_result {
        using temp_base_type = typename std::conditional<
            // if both arguments are unsigned
            ! std::numeric_limits<T>::is_signed
            && ! std::numeric_limits<U>::is_signed,
            // result is unsigned
            std::uintmax_t,
            // otherwise result is signed
            std::intmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type rx(){
        if(u_interval.u < r_type(0)
        || u_interval.l > r_type(0))
            return t_interval / u_interval;
        return utility::minmax(
            std::initializer_list<r_type> {
                t_interval.l / u_interval.l,
                t_interval.l / r_type(-1),
                t_interval.l / r_type(1),
                t_interval.l / u_interval.u,
                t_interval.u / u_interval.l,
                t_interval.u / r_type(-1),
                t_interval.u / r_type(1),
                t_interval.u / u_interval.u,
            }
        );
        }

        constexpr static const r_interval_type r_interval = rx();

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct modulus_result {
        using temp_base_type = typename std::conditional<
            // if both arguments are unsigned
            ! std::numeric_limits<T>::is_signed
            && ! std::numeric_limits<U>::is_signed,
            // result is unsigned
            std::uintmax_t,
            // otherwise result is signed
            std::intmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type rx(){
            if(u_interval.u < r_type(0)
            || u_interval.l > r_type(0))
                return t_interval / u_interval;
            return utility::minmax(
                std::initializer_list<r_type> {
                    t_interval.l % u_interval.l,
                    t_interval.l % r_type(-1),
                    t_interval.l % r_type(1),
                    t_interval.l % u_interval.u,
                    t_interval.u % u_interval.l,
                    t_interval.u % r_type(-1),
                    t_interval.u % r_type(1),
                    t_interval.u % u_interval.u,
                }
            );
        }

        constexpr static const r_interval_type r_interval = rx();

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    // note: comparison_result (<, >, ...) is special.
    // The return value is always a bool.  The type returned here is
    // the intermediate type applied to make the values comparable.
    template<typename T, typename U>
    struct comparison_result {
        using temp_base_type = typename std::conditional<
            // if both arguments are unsigned
            ! std::numeric_limits<T>::is_signed
            && ! std::numeric_limits<U>::is_signed,
            // result is unsigned
            std::uintmax_t,
            // otherwise result is signed
            std::intmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static r_type min(const r_type & t, const r_type & u){
            // assert(! u.exception());
            // assert(! t.exception());
            return static_cast<bool>(t < u) ? t : u;
        }

       constexpr static r_type max(const r_type & t, const r_type & u){
            // assert(! u.exception());
            // assert(! t.exception());
            return static_cast<bool>(t < u) ? u : t;
        }

        // union of two intervals
        // note: we can't use t_interval | u_interval because it
        // depends on max and min which in turn depend on < which in turn
        // depends on implicit conversion of tribool to bool
        constexpr static r_interval_type union_interval(
            const r_interval_type & t,
            const r_interval_type & u
        ){
            const r_type & rl = min(t.l, u.l);
            const r_type & ru = max(t.u, u.u);
            return r_interval_type(rl, ru);
        }

        constexpr static const r_interval_type r_interval =
            union_interval(t_interval, u_interval);

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    // shift operations
    template<typename T, typename U>
    struct left_shift_result {
        using temp_base_type = typename std::conditional<
            std::numeric_limits<T>::is_signed,
            std::intmax_t,
            std::uintmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_interval_type u_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type r_interval =
            t_interval << u_interval;

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;
    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct right_shift_result {
        using temp_base_type = typename std::conditional<
            std::numeric_limits<T>::is_signed,
            std::intmax_t,
            std::uintmax_t
        >::type;

        using r_type = checked_result<temp_base_type>;
        using r_interval_type = interval<r_type>;

        constexpr static const r_interval_type t_interval{
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::min())),
            checked::cast<temp_base_type>(base_value(std::numeric_limits<T>::max()))
        };

        constexpr static const r_type u_min
            = checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::min()));

        constexpr static const r_interval_type u_interval{
            u_min.exception()
            ? r_type(0)
            : u_min,
            checked::cast<temp_base_type>(base_value(std::numeric_limits<U>::max()))
        };

        constexpr static const r_interval_type r_interval = t_interval >> u_interval;

        using type = typename result_type<
            temp_base_type,
            r_interval.l.exception()
                ? std::numeric_limits<temp_base_type>::min()
                : static_cast<temp_base_type>(r_interval.l),
            r_interval.u.exception()
                ? std::numeric_limits<temp_base_type>::max()
                : static_cast<temp_base_type>(r_interval.u)
        >::type;

    };

    ///////////////////////////////////////////////////////////////////////
    template<typename T, typename U>
    struct bitwise_and_result {
        using type = decltype(
            typename base_type<T>::type()
            & typename base_type<U>::type()
        );
    };
    template<typename T, typename U>
    struct bitwise_or_result {
        using type = decltype(
            typename base_type<T>::type()
            | typename base_type<U>::type()
        );
    };
    template<typename T, typename U>
    struct bitwise_xor_result {
        using type = decltype(
            typename base_type<T>::type()
            ^ typename base_type<U>::type()
        );
    };
};

} // safe_numerics
} // boost

#endif // BOOST_NUMERIC_AUTOMATIC_HPP


//==============================================================================
//         Copyright 2015          J.T. Lapreste
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/round.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( round_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::round;
  using nt2::tag::round_;
  typedef typename std::complex<T> cT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round(nt2::Inf<cT>()), nt2::Inf<cT>());
  NT2_TEST_EQUAL(round(nt2::Minf<cT>()), nt2::Minf<cT>());
  NT2_TEST_EQUAL(round(nt2::Nan<cT>()), nt2::Nan<cT>());
#endif
  NT2_TEST_EQUAL(round(cT(T(-1.1))), cT(-1));
  NT2_TEST_EQUAL(round(cT(T(1.1))), cT(1));
  NT2_TEST_EQUAL(round(cT(T(-1.1), T(1.1))), cT(-1, 1));
  NT2_TEST_EQUAL(round(cT(T(1.4))), cT(1));
  NT2_TEST_EQUAL(round(cT(T(1.5))), cT(2));
  NT2_TEST_EQUAL(round(cT(T(1.6))), cT(2));
  NT2_TEST_EQUAL(round(cT(T(2.5))), cT(3));
  NT2_TEST_EQUAL(round(nt2::Half<cT>()), nt2::One<cT>());
  NT2_TEST_EQUAL(round(nt2::Mhalf<cT>()), nt2::Mone<cT>());
  NT2_TEST_EQUAL(round(nt2::Mone<cT>()), nt2::Mone<cT>());
  NT2_TEST_EQUAL(round(nt2::One<cT>()), nt2::One<cT>());
  NT2_TEST_EQUAL(round(nt2::Zero<cT>()), nt2::Zero<cT>());
}

NT2_TEST_CASE_TPL ( round_real2,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::round;
  using boost::simd::tag::round_;
 typedef typename std::complex<T> cT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round(boost::simd::Inf<cT>(), 2), boost::simd::Inf<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Minf<cT>(), 2), boost::simd::Minf<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Nan<cT>(), 2), boost::simd::Nan<cT>());
#endif
  NT2_TEST_EQUAL(round(boost::simd::Mhalf<cT>(), 2), boost::simd::Mhalf<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Mone<cT>(), 2), boost::simd::Mone<cT>());
  NT2_TEST_EQUAL(round(boost::simd::One<cT>(), 2), boost::simd::One<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Zero<cT>(), 2), boost::simd::Zero<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<cT>()-boost::simd::Half<cT>(), 2),boost::simd::Maxflint<cT>());
  NT2_TEST_EQUAL(round(boost::simd::Maxflint<cT>(), 2),boost::simd::Maxflint<cT>());
  NT2_TEST_EQUAL(round(cT(1.44, 1.44), 1), cT(1.4,1.4 ));
  NT2_TEST_EQUAL(round(cT(1.45, 1.45), 1), cT(1.5,1.5 ));
  NT2_TEST_EQUAL(round(cT(1.46, 1.46), 1), cT(1.5,1.5 ));
  NT2_TEST_EQUAL(round(cT(2.45, 2.45), 1), cT(2.5,2.5 ));
  NT2_TEST_EQUAL(round(cT(-1.44,-1.44), 1), cT(-1.4,-1.4));
  NT2_TEST_EQUAL(round(cT(-1.45,-1.45), 1), cT(-1.5,-1.5));
  NT2_TEST_EQUAL(round(cT(-1.46,-1.46), 1), cT(-1.5,-1.5));
  NT2_TEST_EQUAL(round(cT(-2.45,-2.45), 1), cT(-2.5,-2.5));
  NT2_TEST_ULP_EQUAL(round(cT(145,145), -2), cT(100,100), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(150,150), -2), cT(200,200), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(156,156), -2), cT(200,200), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(250,250), -2), cT(300,300), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(-145,-145), -2), cT(-100,-100), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(-155,-155), -2), cT(-200,-200), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(-156,-156), -2), cT(-200,-200), 0.5);
  NT2_TEST_ULP_EQUAL(round(cT(-255,-255), -2), cT(-300,-300), 0.5);
} // end of test for floating_


#ifndef STRATUM_IO_SIZE_PRINTER
#define STRATUM_IO_SIZE_PRINTER

#include "stratum/macro/namespaces.hpp"
#include "stratum/common/constants.h"
#include <iostream>
#include <iomanip>
#include <boost/io/ios_state.hpp>

NAMESPACE_BEGIN(stratum, io)

struct size_printer
{
    double size_;
    int prec_;

    explicit size_printer(double size, size_t prec = 2)
    : size_(size), prec_(prec)
    { assert(size >= 0.0); }
}; // struct size_printer

inline std::ostream& operator<<(std::ostream& out, const size_printer& bsize)
{
    namespace constant = NAMESPACE(stratum, common, constant);

    boost::io::ios_all_saver  ias(out);

    out << std::setfill('0') << std::fixed << std::setprecision(bsize.prec_) << std::left;
    double bytes = bsize.size_;

    if ( bytes > constant::GIGABYTES )
    {
        out << ( bytes / double(constant::GIGABYTES) ) << " Gb";
    }
    else if ( bytes > constant::MEGABYTES )
    {
        out << ( bytes / double(constant::MEGABYTES) ) << " Mb";
    }
    else if ( bytes > constant::KILOBYTES )
    {
        out << ( bytes / double(constant::KILOBYTES) ) << " Kb";
    }
    else
    {
        out << bytes << " bytes";
    }

    return out;
} // operator<<()

NAMESPACE_END(stratum, io)

#endif // STRATUM_IO_SIZE_PRINTER


/*  =========================================================================
    Author: Leonardo Citraro
    Company:
    Filename: main.cpp
    Last modifed:   29.12.2016 by Leonardo Citraro
    Description:    Basic test of the HOG feature

    =========================================================================
    https://lear.inrialpes.fr/people/triggs/pubs/Dalal-cvpr05.pdf
    =========================================================================
*/
#include "HOG.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include <iostream>
#include <algorithm>
#include <memory>
#include <functional>
#include <math.h>
#include <chrono>
#include <boost/filesystem.hpp>
#include "csv.hpp"

namespace fs = boost::filesystem;
using namespace std;

int verbose = 1;

int main(int argc, char* argv[])
{
    // IO variables
    fs::path input_path (argv[1]);
    fs::path output_file (argv[2]);

    // Retrieve the HOG from the image
    size_t crop_height = 256; //atoi(argv[3]);
    size_t crop_width = 128; //atoi(argv[4]);
    size_t blocksize = 32; //atoi(argv[5]);
    size_t cellsize = 16; //atoi(argv[6]);
    size_t stride = 16; //atoi(argv[7]);
    size_t binning = 9; //atoi(argv[8]);

    // Auxiliary variables for memory allocation of HOG features
    size_t cells_x_grid = blocksize / cellsize;
    size_t hog_bins = binning * (cells_x_grid * cells_x_grid);
    size_t w = (crop_width - blocksize) / stride + 1;
    size_t h = (crop_height - blocksize) / stride + 1;
    size_t hog_size = hog_bins * (w * h);

    // Instantiate HOG feature extractor
    HOG hog(blocksize, cellsize, stride, binning, HOG::GRADIENT_UNSIGNED);

    // List image files to be described
    std::vector<std::string> filenames;
    for (fs::directory_iterator itr(input_path); itr != fs::directory_iterator(); ++itr)
    {
        if (fs::is_regular_file(itr->status()))
            filenames.push_back(itr->path().filename().string());
    }

    // Init HOG features
    cv::Mat hog_features (filenames.size(), hog_size, CV_32FC1);

    // Loop over images (measure time)
    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    int n = filenames.size();
    for (int i = 0; i < n; i++)
    {
        string filepath = (input_path / fs::path(filenames[i])).string();
        cv::Mat image = cv::imread(filepath, CV_LOAD_IMAGE_UNCHANGED);
        cv::Mat image_crop;
        cv::resize( image, image_crop, cv::Size(crop_width, crop_height) );

        if (verbose > 0)
            cout << '(' << i << '/' << n-1 << ')' << ' ' << filenames[i] << ' ';

        hog.process(image);
        auto hist = hog.retrieve(cv::Rect(0,0,image_crop.cols, image_crop.rows));

        assert(hist.size() == hog_size);

        for (int k = 0; k < hist.size(); k++)
            hog_features.at<float>(i,k) = hist[k];

        if (verbose > 0)
            cout << " -> DONE" << '\n';
    }

    cv::FileStorage fs;
    fs.open(output_file.string(), cv::FileStorage::APPEND);
    fs << "filenames"<< filenames;
    fs << "hog_features" << hog_features;
    fs.release();

    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

    std::cout << "Total elapsed time = " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() <<std::endl;

    return 0;
}


#include <armadillo>
#include <cassert>
#include <cstdio>

int main() {

    size_t zero = 0;

    arma::vec Av(zero);
    arma::mat Am(zero, zero);
    arma::cube Ac(zero, zero, zero);

    Av.print("Av");
    printf("%p\n", &Av);
    Am.print("Am");
    printf("%p\n", &Am);
    Ac.print("Ac");
    printf("%p\n", &Ac);

    arma::cube Bc = 2.0 * Ac;

    Bc.print("Bc");
    printf("%p\n", &Bc);

    arma::cube Cc = Ac + 1.2;

    Cc.print("Cc");
    printf("%p\n", &Cc);

    arma::cube Dc(zero, 2, zero);
    arma::cube Ec(2, 2, zero);

    Dc.print("Dc");
    printf("%p\n", &Dc);
    Ec.print("Ec");
    printf("%p\n", &Ec);

    int d_Ec_Dc = &Dc - &Ec;
    printf("d_Ec_Dc: %d\n", d_Ec_Dc);
    assert(d_Ec_Dc == 1);
    int d_Ec_Bc = &Bc - &Ec;
    printf("d_Ec_Bc: %d\n", d_Ec_Bc);
    // int d_Av_Dc = &Dc(0, 0, 0) - &Av(0);
    // printf("d_Av_Dc: %d\n", d_Av_Dc);

    Av.zeros();
    Am.zeros();
    Ac.zeros();
    Ec.zeros();

    return 0;

}


#include <string>
#include <fstream>
#include <dirent.h>
#include <iostream>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <opencv2/opencv.hpp>
#include <boost/lexical_cast.hpp>

#include "conf.h"
#include "../third_party/cmdline.h"
#include "cmdlineutils.h"

cv::Rect getSubFrameRect(YAML::Node &conf, cv::Mat &frame, int oriWidth, int oriHeight) {
	cv::Rect rect(0, 0, 0, 0);

	if (!conf["originalVideo"] ||
		!conf["originalVideo"]["flagArea"]) {
        return rect;
    }

	// int oriWidth    = conf["originalVideo"]["resolution"]["width"].as<int>();
	// int oriHeight   = conf["originalVideo"]["resolution"]["height"].as<int>();

	int oriFlagX    = conf["originalVideo"]["flagArea"]["x"].as<int>();
	int oriFlagY    = conf["originalVideo"]["flagArea"]["y"].as<int>();
	int oriFlagW    = conf["originalVideo"]["flagArea"]["width"].as<int>();
	int oriFlagH    = conf["originalVideo"]["flagArea"]["height"].as<int>();

	int frameWidth  = frame.cols;
	int frameHeight = frame.rows;

	float ratioX      = oriWidth * 1.0 / frameWidth;
	float ratioY      = oriHeight * 1.0 / frameHeight;

	std::cout << "ratioX: " << ratioX << ", ratioY:" << ratioY << std::endl;

	rect = cv::Rect(int(oriFlagX / ratioX),
		            int(oriFlagY / ratioY),
		            int(oriFlagW / ratioX),
		            int(oriFlagH / ratioY));

	return rect;
}

int main(int argc, char *argv[]) {
	cv::VideoCapture cap;
	std::string wndTitle;

	cmdline::parser cmdPara = initCmdLine(argc, argv);
	cmdPara.parse_check(argc, argv);

	std::string file = cmdPara.get<std::string>("input");
	int oriWidth     = cmdPara.get<int>("originWidth");
	int oriHeight    = cmdPara.get<int>("originHeight");
	std::string dir  = cmdPara.get<std::string>("dir");

	std::cout << "dir: " << dir << std::endl;

	if (!isDirExist(dir)) {
		mkdir(dir.c_str(), S_IRWXU);
	}

	wndTitle = file;
	cap.open(file);

	YAML::Node conf = initConfigure();

    cv::Mat frame;
    cap >> frame;
    int i = 0;

    cv::Rect subFrameRect = getSubFrameRect(conf, frame, oriWidth, oriHeight);

	while(!frame.empty()) {
		char c = (char)cv::waitKey(1);
    	if (c == 27) {
    		break;
    	}

    	cv::Mat subFrame;
		subFrame = frame(subFrameRect);

    	cv::Mat greyFrame;
    	if (frame.channels() == 1) {
    		greyFrame = subFrame;
    	} else {
    		cv::cvtColor(subFrame, greyFrame, cv::COLOR_RGB2GRAY);
    	}

		cv::imwrite(dir + "/" + boost::lexical_cast<std::string>(i) + ".png", subFrame);
		//cv::imshow(wndTitle, subFrame);
		++i;
		cap >> frame;
  	}

	return 0;
}


//==================================================================================================
/*
  Copyright 2017 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)
*/
//==================================================================================================
//! [segmented_aligned_range]
#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>
#include <vector>

int main()
{
  std::vector<float> x{1,2,3,4,5,6,7,8,9,10,11,12,13};

  float sr{0};
  boost::simd::pack<float> vr{0};
  auto segments = boost::simd::segmented_aligned_range(x.begin(),x.end());

  // Scalar head
  for(float e : segments.head) sr += e;

  // SIMD body
  for(boost::simd::pack<float> e : segments.body) vr += e;

  // Scalar tail
  for(float e : segments.tail) sr += e;

  std::cout << "Sum of [" << x.front() << " ... " << x.back() << "] is " << boost::simd::sum(vr) + sr << std::endl;

  return 0;
}
//! [segmented_aligned_range]


//=======================================================================
// Copyright 2013 University of Warsaw.
// Authors: Piotr Wygocki
//
// 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)
//=======================================================================

#define BOOST_TEST_MODULE cycle_canceling_test

#include <boost/test/unit_test.hpp>

#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>

#include "min_cost_max_flow_utils.hpp"

BOOST_AUTO_TEST_CASE(cycle_canceling_def_test) {
    boost::SampleGraph::vertex_descriptor s,t;
    boost::SampleGraph::Graph g;
    boost::SampleGraph::getSampleGraph(g, s, t);

    boost::edmonds_karp_max_flow(g, s, t);
    boost::cycle_canceling(g);

    int cost = boost::find_flow_cost(g);
    BOOST_CHECK_EQUAL(cost, 29);
}

BOOST_AUTO_TEST_CASE(path_augmentation_def_test2) {
    boost::SampleGraph::vertex_descriptor s,t;
    boost::SampleGraph::Graph g;
    boost::SampleGraph::getSampleGraph2(g, s, t);

    boost::edmonds_karp_max_flow(g, s, t);
    boost::cycle_canceling(g);

    int cost =  boost::find_flow_cost(g);
    BOOST_CHECK_EQUAL(cost, 7);
}

BOOST_AUTO_TEST_CASE(cycle_canceling_test) {
    boost::SampleGraph::vertex_descriptor s,t;
    typedef boost::SampleGraph::Graph Graph;
    boost::SampleGraph::Graph g;
    boost::SampleGraph::getSampleGraph(g, s, t);

    int N = num_vertices(g);
    std::vector<int> dist(N);
    typedef boost::graph_traits<Graph>::edge_descriptor edge_descriptor;
    std::vector<edge_descriptor> pred(N);

    boost::property_map<Graph, boost::vertex_index_t>::const_type idx = get(boost::vertex_index, g);

    boost::edmonds_karp_max_flow(g, s, t);
    boost::cycle_canceling(g, boost::distance_map(boost::make_iterator_property_map(dist.begin(), idx)).predecessor_map(boost::make_iterator_property_map(pred.begin(), idx)).vertex_index_map(idx));

    int cost = boost::find_flow_cost(g);
    BOOST_CHECK_EQUAL(cost, 29);
}


#ifndef ARMA_GETTER_H
#define ARMA_GETTER_H
#include <armadillo>
/**
This is some templated classes for getting an element from an armadillo matrix.
They are written to both be able to efficiently filter both rows and
columns of an Armadillo matrix using the lowPassFilter class.
*/

namespace visa
{
  enum class ArmaMatrix_t { ROW, COL, VECTOR };

  /** Default return element in column fixedIndx */
  template <class elemType, ArmaMatrix_t dir>
  class ArmaGetter
  {
  public:
    const elemType& operator()( arma::Mat<elemType> &mat, unsigned int indx ) const { return mat(indx, fixedIndx); };
    elemType& operator()( arma::Mat<elemType> &mat, unsigned int indx ){ return mat(indx, fixedIndx); };
    unsigned int fixedIndx{0};
  };

  /** Return element in row fixedIndx */
  template<class elemType>
  class ArmaGetter<elemType, ArmaMatrix_t::ROW>
  {
  public:
    const elemType& operator()( arma::Mat<elemType> &mat, unsigned int indx ) const { return mat(fixedIndx, indx); };
    elemType& operator()( arma::Mat<elemType> &mat, unsigned int indx ){ return mat(fixedIndx, indx); };
    unsigned int fixedIndx{0};
  };

  /** Use this to run a low pass filter on a armadillo column vector */
  template<class elemType>
  class ArmaGetter<elemType, ArmaMatrix_t::VECTOR>
  {
  public:
    const elemType& operator()( arma::Mat<elemType> &vec, unsigned int indx ) const { return vec(indx); };
    elemType& operator()( arma::Mat<elemType> &vec, unsigned int indx ){ return vec(indx); };
    unsigned int fixedIndx{0}; // Unused, but needs to be declared for compatibility with the low pass filter
  };
}
#endif


#include <boost/test/unit_test.hpp>

#include "tools/mapper.h" // header to test
#include <map>

using namespace biosim;

BOOST_AUTO_TEST_SUITE(suite_mapper)

BOOST_AUTO_TEST_CASE(mapper_ctor) {
  tools::mapper<std::string> m({});
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 0);
  BOOST_CHECK(m.encode(0) == std::string());
  BOOST_CHECK(m.encode(1) == std::string());

  BOOST_REQUIRE_THROW(tools::mapper<std::string>(std::vector<std::vector<char>>{{}}), std::invalid_argument);

  m = tools::mapper<std::string>({{'0', '1'}});
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 1);

  m = tools::mapper<std::string>({{'0', '1'}, {'0', '1'}});
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 3);

  m = tools::mapper<std::string>({{'0', '1'}, {'0', '1'}, {'0', '1'}});
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 7);
}

BOOST_AUTO_TEST_CASE(mapper_string) {
  std::map<size_t, std::string> expected_big_endian{{0, "000"},
                                                    {1, "001"},
                                                    {2, "002"},
                                                    {3, "010"},
                                                    {4, "011"},
                                                    {5, "012"},
                                                    {6, "100"},
                                                    {7, "101"},
                                                    {8, "102"},
                                                    {9, "110"},
                                                    {10, "111"},
                                                    {11, "112"},
                                                    {12, "112"}};
  tools::mapper<std::string> m({{'0', '1'}, {'0', '1'}, {'0', '1', '2'}}, true);
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 11);
  for(size_t index(m.get_min()); index <= m.get_max(); ++index) {
    BOOST_CHECK(m.encode(index) == expected_big_endian[index]);
    BOOST_CHECK(m.decode(m.encode(index)) == index);
  } // for

  std::map<size_t, std::string> expected_little_endian{{0, "000"},
                                                       {1, "100"},
                                                       {2, "010"},
                                                       {3, "110"},
                                                       {4, "001"},
                                                       {5, "101"},
                                                       {6, "011"},
                                                       {7, "111"},
                                                       {8, "002"},
                                                       {9, "102"},
                                                       {10, "012"},
                                                       {11, "112"},
                                                       {12, "112"}};
  m = tools::mapper<std::string>({{'0', '1'}, {'0', '1'}, {'0', '1', '2'}}, false);
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 11);
  for(size_t index(m.get_min()); index <= m.get_max(); ++index) {
    BOOST_CHECK(m.encode(index) == expected_little_endian[index]);
    BOOST_CHECK(m.decode(m.encode(index)) == index);
  } // for
}

BOOST_AUTO_TEST_CASE(mapper_vector) {
  std::map<size_t, std::vector<size_t>> expected_big_endian{{0, {0, 0, 0}},
                                                            {1, {0, 0, 1}},
                                                            {2, {0, 0, 2}},
                                                            {3, {0, 1, 0}},
                                                            {4, {0, 1, 1}},
                                                            {5, {0, 1, 2}},
                                                            {6, {1, 0, 0}},
                                                            {7, {1, 0, 1}},
                                                            {8, {1, 0, 2}},
                                                            {9, {1, 1, 0}},
                                                            {10, {1, 1, 1}},
                                                            {11, {1, 1, 2}},
                                                            {12, {1, 1, 2}}};
  tools::mapper<std::vector<size_t>> m({{0, 1}, {0, 1}, {0, 1, 2}}, true);
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 11);
  for(size_t index(m.get_min()); index <= m.get_max(); ++index) {
    BOOST_CHECK(m.encode(index) == expected_big_endian[index]);
    BOOST_CHECK(m.decode(m.encode(index)) == index);
  } // for

  std::map<size_t, std::vector<size_t>> expected_little_endian{{0, {0, 0, 0}},
                                                               {1, {1, 0, 0}},
                                                               {2, {0, 1, 0}},
                                                               {3, {1, 1, 0}},
                                                               {4, {0, 0, 1}},
                                                               {5, {1, 0, 1}},
                                                               {6, {0, 1, 1}},
                                                               {7, {1, 1, 1}},
                                                               {8, {0, 0, 2}},
                                                               {9, {1, 0, 2}},
                                                               {10, {0, 1, 2}},
                                                               {11, {1, 1, 2}},
                                                               {12, {1, 1, 2}}};
  m = tools::mapper<std::vector<size_t>>({{0, 1}, {0, 1}, {0, 1, 2}}, false);
  BOOST_CHECK(m.get_min() == 0);
  BOOST_CHECK(m.get_max() == 11);
  for(size_t index(m.get_min()); index <= m.get_max(); ++index) {
    BOOST_CHECK(m.encode(index) == expected_little_endian[index]);
    BOOST_CHECK(m.decode(m.encode(index)) == index);
  } // for
}

BOOST_AUTO_TEST_SUITE_END()


/*
 *  The MIT License (MIT)
 *
 *  Copyright (c) 2015 MRL-SPL RoboCup Team
 *
 *  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.
 *
 */

/*
 * File:   DHFrame.hpp
 * Author: Mohammad Ali Sharpasand
 *
 * Created on February 29, 2016, 9:21 PM
 */

#pragma once

#include <vector>
#include <armadillo>

#include "Angle.hpp"

namespace BipedLibrary
{
    using namespace arma;

    class DHFrame
    {
    public:
        DHFrame(Angle const alpha = 0,
                Angle const theta = 0,
                float const r = 0,
                float const d = 0);

        unsigned id() const;
        Angle theta() const;
        Angle deltaTheta() const;
        Angle alpha() const;
        float r() const;
        float d() const;

        DHFrame& setTheta(Angle const theta);
        DHFrame& setDeltaTheta(Angle const deltaTheta);
        DHFrame& setAlpha(Angle const alpha);
        DHFrame& setR(float const r);
        DHFrame& setD(float const d);

        bool operator==(DHFrame const& other);

    private:
        unsigned id_;
        static unsigned lastGenaratedId_;
        Angle theta_;
        Angle deltaTheta_;
        Angle alpha_;
        float r_;
        float d_;
    };

}


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-boolean
    Function object implementing if_plus capabilities

    The function returns either the second parameter or the sum of the second
    and third parameters, depending on whether the first parameter is
	@ref False or @ref True .

    @par Semantic:

    For every parameter @c c of type @c C and @c x, @c y of type @c T :

    @code
    T r = if_plus(c, x, y);
    @endcode

    is similar to:

    @code
    T r = c ? x + y : x;
    @endcode
  **/
  Value if_plus(Value const& c, Value const& x, Value const& y);

  //@overload
  Value if_plus(LogicalValue const& c, Value const& x, Value const& y);
} }
#endif

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

#endif


/**
 * \file      miscellaneous-algorithms.hpp
 * \author    Mehdi Benallegue
 * \date       2013
 * \brief      Gathers many kinds of algorithms
 *
 *
 *
 */

#ifndef STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS
#define STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS

#include <boost/utility.hpp>

#include <state-observation/api.h>
#include <state-observation/tools/definitions.hpp>

namespace stateObservation
{
namespace tools
{

/// computes the square of a value of any type
template<class T>
inline T square(const T & x)
{
  return T(x * x);
}

/// derivates any type with finite differences
template<class T>
inline T derivate(const T & o1, const T & o2, double dt)
{
  T o(o2 - o1);
  return o * (1 / dt);
}

/// gives the sign of a variable (1, 0 or -1)
template<typename T>
inline int signum(T x)
{
  return (T(0) < x) - (x < T(0));
}

template<typename T>
std::string toString(T val)
{
  std::stringstream ss("");
  ss << val;
  return ss.str();
}

/// provides an acceleration giving a finite time convergence to zero
/// the state is the position x and the derivative xd and the output is the
/// acceleration. The gains kp, kv must be negative
inline double STATE_OBSERVATION_DLLAPI finiteTimeAccControl(double x, double xd, double kp = -1, double kv = -1)
{
  double sax = sqrt(fabs(x));
  double xdr = kp * signum(x) * sax;
  double y = xd - xdr;
  double ydr = -kv * signum(y) * sqrt(fabs(y));
  return ydr - kp * xd / (2 * sax);
}

/// sqme as the scalar version but for every member of the vector
inline Vector STATE_OBSERVATION_DLLAPI finiteTimeAccControl(const Vector & x,
                                                            const Vector & xd,
                                                            double kp = -1,
                                                            double kv = -1)
{
  Vector xdd(x.size());
  for(Index i = 1; i < x.size(); ++i)
  {
    xdd(i) = finiteTimeAccControl(x(i), xd(i), kp, kv);
  }
  return xdd;
}
} // namespace tools

} // namespace stateObservation

#endif // STATEOBSERVATIONTOOLSMISCELANEOUSALGORITHMS


//
// Copyright 2013 Krzysztof Czainski
// Copyright 2020 Mateusz Loskot <mateusz at loskot dot net>
//
// 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
//
#include <boost/gil.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>

#include <boost/core/lightweight_test.hpp>

namespace gil = boost::gil;

// FIXME: Remove when https://github.com/boostorg/core/issues/38 happens
#define BOOST_GIL_TEST_IS_CLOSE(a, b, epsilon) BOOST_TEST(std::abs((a) - (b)) < (epsilon))

void test_plus()
{
    gil::rgb8_pixel_t a(10, 20, 30);
    gil::bgr8_pixel_t b(30, 20, 10);

    gil::pixel_plus_t<gil::rgb8_pixel_t, gil::bgr8_pixel_t, gil::rgb8_pixel_t> op;
    gil::rgb8_pixel_t c = op(a, b);

    BOOST_TEST(get_color(c, gil::red_t()) == 20);
    BOOST_TEST(get_color(c, gil::green_t()) == 40);
    BOOST_TEST(get_color(c, gil::blue_t()) == 60);

    gil::pixel_plus_t<gil::rgb8_pixel_t, gil::bgr8_pixel_t, gil::bgr8_pixel_t> op2;
    gil::bgr8_pixel_t d = op2(a, b);

    BOOST_TEST(get_color(d, gil::red_t()) == 20);
    BOOST_TEST(get_color(d, gil::green_t()) == 40);
    BOOST_TEST(get_color(d, gil::blue_t()) == 60);
}

void test_multiply()
{
    gil::rgb32f_pixel_t a(1.f, 2.f, 3.f);
    gil::bgr32f_pixel_t b(2.f, 2.f, 2.f);

    gil::pixel_multiply_t<
        gil::rgb32f_pixel_t, gil::bgr32f_pixel_t, gil::rgb32f_pixel_t>
        op;
    gil::rgb32f_pixel_t c = op(a, b);

    float epsilon = 1e-6f;
    BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::red_t()), 2.f, epsilon);
    BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::green_t()), 4.f, epsilon);
    BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::blue_t()), 6.f, epsilon);
}

void test_divide()
{
    // integer
    {
        gil::rgb8_pixel_t a(10, 20, 30);
        gil::bgr8_pixel_t b(2, 2, 2);

        gil::pixel_divide_t<gil::rgb8_pixel_t, gil::bgr8_pixel_t, gil::rgb8_pixel_t> op;
        gil::rgb32f_pixel_t c = op(a, b);

        BOOST_TEST(get_color(c, gil::red_t()) == 5);
        BOOST_TEST(get_color(c, gil::green_t()) == 10);
        BOOST_TEST(get_color(c, gil::blue_t()) == 15);
    }

    // float
    {
        gil::rgb32f_pixel_t a(1.f, 2.f, 3.f);
        gil::bgr32f_pixel_t b(2.f, 2.f, 2.f);

        gil::pixel_divide_t<gil::rgb32f_pixel_t, gil::bgr32f_pixel_t, gil::rgb32f_pixel_t> op;
        gil::rgb32f_pixel_t c = op(a, b);

        float epsilon = 1e-6f;
        BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::red_t()), 0.5f, epsilon);
        BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::green_t()), 1.f, epsilon);
        BOOST_GIL_TEST_IS_CLOSE(get_color(c, gil::blue_t()), 1.5f, epsilon);
    }
}

int main()
{
    test_plus();
    test_multiply();
    test_divide();

    return ::boost::report_errors();
}


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

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

 * See LICENSE for the license information

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

/**
 * @file    testCombinedImuFactor.cpp
 * @brief   Unit test for Lupton-style combined IMU factor
 * @author  Luca Carlone
 * @author  Frank Dellaert
 * @author  Richard Roberts
 * @author  Stephen Williams
 */

#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>

#include <CppUnitLite/TestHarness.h>

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

#include "imuFactorTesting.h"

namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegratedCombinedMeasurements::Params> Params() {
  auto p = PreintegratedCombinedMeasurements::Params::MakeSharedD(kGravity);
  p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
  p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
  p->integrationCovariance = 0.0001 * I_3x3;
  return p;
}
}

/* ************************************************************************* */
TEST( CombinedImuFactor, PreintegratedMeasurements ) {
  // Linearization point
  Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases

  // Measurements
  Vector3 measuredAcc(0.1, 0.0, 0.0);
  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
  double deltaT = 0.5;
  double tol = 1e-6;

  auto p = testing::Params();

  // Actual preintegrated values
  PreintegratedImuMeasurements expected1(p, bias);
  expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  PreintegratedCombinedMeasurements actual1(p, bias);

  actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
  EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
  EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
  DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}

/* ************************************************************************* */
TEST( CombinedImuFactor, ErrorWithBiases ) {
  Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
  Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
  Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0));
  Vector3 v1(0.5, 0.0, 0.0);
  Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)),
      Point3(5.5, 1.0, -50.0));
  Vector3 v2(0.5, 0.0, 0.0);

  auto p = testing::Params();
  p->omegaCoriolis = Vector3(0,0.1,0.1);
  PreintegratedImuMeasurements pim(
      p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));

  // Measurements
  Vector3 measuredOmega;
  measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
  Vector3 measuredAcc =
      x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0);
  double deltaT = 1.0;
  double tol = 1e-6;

  pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  PreintegratedCombinedMeasurements combined_pim(p,
      Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)));

  combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim);

  noiseModel::Gaussian::shared_ptr Combinedmodel =
      noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
  CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
                                   combined_pim);

  Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
  Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
      bias2);
  EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));

  // Expected Jacobians
  Matrix H1e, H2e, H3e, H4e, H5e;
  (void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);

  // Actual Jacobians
  Matrix H1a, H2a, H3a, H4a, H5a, H6a;
  (void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
      H3a, H4a, H5a, H6a);

  EXPECT(assert_equal(H1e, H1a.topRows(9)));
  EXPECT(assert_equal(H2e, H2a.topRows(9)));
  EXPECT(assert_equal(H3e, H3a.topRows(9)));
  EXPECT(assert_equal(H4e, H4a.topRows(9)));
  EXPECT(assert_equal(H5e, H5a.topRows(9)));
}

/* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
  auto p = testing::Params();
  testing::SomeMeasurements measurements;

  auto preintegrated = [=](const Vector3& a, const Vector3& w) {
    PreintegratedImuMeasurements pim(p, Bias(a, w));
    testing::integrateMeasurements(measurements, &pim);
    return pim.preintegrated();
  };

  // Actual pre-integrated values
  PreintegratedCombinedMeasurements pim(p);
  testing::integrateMeasurements(measurements, &pim);

  EXPECT(assert_equal(numericalDerivative21<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
                      pim.preintegrated_H_biasAcc()));
  EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
                      pim.preintegrated_H_biasOmega(), 1e-3));
}
#endif

/* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity) {
  const Bias bias(Vector3(0, 0.1, 0), Vector3(0, 0.1, 0));  // Biases (acc, rot)

  auto p = testing::Params();

  // Measurements
  const Vector3 measuredOmega(0, 0.1, 0);  // M_PI/10.0+0.3;
  const Vector3 measuredAcc(0, 1.1, -kGravity);
  const double deltaT = 0.01;

  PreintegratedCombinedMeasurements pim(p, bias);

  for (int i = 0; i < 100; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);

  // Create factor
  const noiseModel::Gaussian::shared_ptr combinedmodel =
      noiseModel::Gaussian::Covariance(pim.preintMeasCov());
  const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);

  // Predict
  const NavState actual = pim.predict(NavState(), bias);
  const Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
  const Vector3 expectedVelocity(0, 1, 0);
  EXPECT(assert_equal(expectedPose, actual.pose()));
  EXPECT(assert_equal(Vector(expectedVelocity), Vector(actual.velocity())));
}

/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
  const Bias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
  auto p = testing::Params();
  PreintegratedCombinedMeasurements pim(p, bias);
  const Vector3 measuredAcc = - kGravityAlongNavZDown;
  const Vector3 measuredOmega(0, 0, M_PI / 10.0);
  const double deltaT = 0.01;
  const double tol = 1e-4;
  for (int i = 0; i < 100; ++i)
    pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
  const CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim);

  // Predict
  const Pose3 x(Rot3::Ypr(0, 0, 0), Point3(0, 0, 0)), x2;
  const Vector3 v(0, 0, 0), v2;
  const NavState actual = pim.predict(NavState(x, v), bias);
  const Pose3 expectedPose(Rot3::Ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
  EXPECT(assert_equal(expectedPose, actual.pose(), tol));
}

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


#pragma once

//#include <ADEKF/ceres/jet.h>
#include "ADEKFUtils.h"
#include "ADEKF.h"

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

namespace adekf
{

    //using namespace Eigen;
    //using namespace std::placeholders;

    /**
     * An RTS Smoother Implementation based on the ADEKF
     * @tparam Filter The Filter to be used for estimation
     */
    template <typename State>
    class RTS_Smoother : public ADEKF<State>
    {

    public:
        using Covariance = typename ADEKF<State>::Covariance;
        using ScalarType = typename ADEKF<State>::ScalarType;
        static constexpr size_t DOF = DOFOf<State>;
        static constexpr double weighted_mean_epsilon = 1e-8;
        aligned_vector<State> old_mus, smoothed_mus, predicted_mus;
        aligned_vector<Covariance> old_sigmas, smoothed_sigmas, predicted_sigmas;

        RTS_Smoother(const State &_mu, const Covariance &_sigma) : ADEKF<State>(_mu, _sigma)
        {
            storeEstimation();
            storePredictedEstimation();//called only to have something in 0. Is never used.
        }
        RTS_Smoother(const ADEKF<State> &filter) : ADEKF<State>(filter)
        {
            storeEstimation();
            storePredictedEstimation();//called only to have something in 0. Is never used.
        }
        /**
         * Stores the current mean and covariance of the filter
         */
        void storeEstimation()
        {
            old_mus.push_back(this->mu);
            smoothed_mus.push_back(this->mu);
            old_sigmas.push_back(this->sigma);
            smoothed_sigmas.push_back(this->sigma);
        }

        void storePredictedEstimation()
        {
            predicted_mus.push_back(this->mu);
            predicted_sigmas.push_back(this->sigma);
        }
        template <typename Head, typename... Tail>
        std::tuple<Tail...> tuple_tail(const std::tuple<Head, Tail...> &tuple)
        {
            return apply([](auto head, auto... tail) {
                return std::make_tuple(tail...);
            },
                         tuple);
        }

        template <typename DynamicModel, typename tstate, typename NoiseVector, typename... Controls, typename... ExpandedControls>
        void applyDynamicModel(DynamicModel dynamicModel, tstate &input, const NoiseVector &noise, const std::tuple<Controls...> &controls, const ExpandedControls &...expanded_controls)
        {
            if constexpr (std::tuple_size<std::tuple<Controls...>>::value > 0)
            {
                applyDynamicModel(dynamicModel, input, noise, tuple_tail(controls), expanded_controls..., std::get<0>(controls));
            }
            else
            {
                dynamicModel(input, noise, expanded_controls...);
            }
        }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        Covariance getNonAdditiveDynamicNoise(const State &smoothed, DynamicModel dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::tuple<Controls...> &controls)
        {
            auto derivator = adekf::getDerivator<DOF + NoiseDim>();
            auto input = eval(smoothed + derivator.template head<DOF>());
            //Todo u vernünftig machen
            applyDynamicModel(dynamicModel, input, derivator.template tail<NoiseDim>(), controls);
            auto result = (input - smoothed);
            //std::cout << result << std::endl;
            auto F=extractJacobi(result);
            return eval(F.template rightCols<NoiseDim>() * Q * F.template rightCols<NoiseDim>().transpose());
        }

static inline size_t indefinite_counter=0;
        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothSingleStep(size_t k, DynamicModel &dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::tuple<Controls...> &controls, const State &smoothed_mu_kplus, const Covariance &smoothed_sigma_kplus)
        {

            auto delta = adekf::getDerivator<DOF>();
            auto input = eval(old_mus[k] + delta);
            applyDynamicModel(dynamicModel, input, MatrixType<ScalarType,NoiseDim, 1>::Zero(), controls);

            Covariance Fk=extractJacobi(input - predicted_mus[k+1]);

            Covariance SmootherGain = old_sigmas[k] * Fk.transpose() * predicted_sigmas[k+1].inverse();
            typename State::template DeltaType<double> smoother_innovation=SmootherGain * (smoothed_mu_kplus-predicted_mus[k + 1]);
            smoothed_mus[k] = old_mus[k] + smoother_innovation;

            Covariance Jk=transformReferenceJacobian(old_mus[k],smoothed_mus[k],smoother_innovation);

            Covariance Bk=transformReferenceJacobian(smoothed_mu_kplus,predicted_mus[k+1]);
            smoothed_sigmas[k] = Jk * (old_sigmas[k] + SmootherGain * (Bk*smoothed_sigma_kplus*Bk.transpose()-predicted_sigmas[k+1]) * SmootherGain.transpose())*Jk.transpose();
            if(!isPositiveDefinite(smoothed_sigmas[k]))
                indefinite_counter++;
            assurePositiveDefinite(smoothed_sigmas[k]);
            }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothIntervalWithNonAdditiveNoise(size_t steps, int start, DynamicModel &dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::vector<std::tuple<Controls...>> &all_controls)
        {
            if (start < 0)
            {
                start = old_mus.size() - 1 + start;
            }
            assert(start - steps >= 0 && "Can not smooth more estimates than available");
            assert(start < old_mus.size() - 1 && "Can not smooth a state which belongs to future");
            assert(all_controls.size() == old_mus.size() - 1 && "Requires all control inputs for the dynamic model.");
            for (size_t k = start; k >= old_mus.size() - steps - 1; k--)
            {
                //std::cout << "At iteration: " << k << std::endl;
                smoothSingleStep(k, dynamicModel, Q, all_controls[k], smoothed_mus[k + 1], smoothed_sigmas[k + 1]);
            }
        }

        template <int NoiseDim, typename DynamicModel, typename... Controls>
        void smoothAllWithNonAdditiveNoise(DynamicModel &dynamicModel, const Eigen::Matrix<double,NoiseDim,NoiseDim> &Q, const std::vector<std::tuple<Controls...>> &all_controls)
        {
            smoothIntervalWithNonAdditiveNoise(old_mus.size() - 2, -1, dynamicModel, Q, all_controls);
        }
    };

    /**
     * General Deduction Template for the RTS_Smoother based on StateRetriever.
     * This is needed so you can type RTS_Smoother smoother(Filter{...}) without template arguments
     */
    /*template <typename State>
    RTS_Smoother(ADEKF<State> &&) -> RTS_Smoother<State>;
    template <typename State>
    RTS_Smoother(ADEKF<State> ) -> RTS_Smoother<State>;
*/

} // namespace adekf


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_ELLIPTIC_FUNCTIONS_SIMD_COMMON_AM_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_SIMD_COMMON_AM_HPP_INCLUDED

#include <nt2/elliptic/functions/am.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/elliptic/functions/generic/details/am_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/atan.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>

namespace nt2 { namespace ext
{

   BOOST_DISPATCH_IMPLEMENT  (am_, tag::cpu_,
                              (A0)(A1)(A2)(X),
                              ((simd_<floating_<A0>,X >))
                              ((simd_<floating_<A1>,X >))
                              (scalar_<floating_<A2> >)
                             )
   {
     typedef A0 result_type;

     BOOST_FORCEINLINE result_type operator()(const A0 & u, const A1 & x,
                                             const A2 & tol) const
    {
      result_type xx = nt2::abs(x);
      result_type k = if_else(eq(xx, One<A0>()), Half<A0>(), xx);
      return  if_else(eq(xx, One<A0>()),
                      Two<A0>()*atan(exp(u))-Pio_2<A0>(),
                      details::am_kernel(u, k, tol));
    }
  };

} }
#endif


/**
 * @Author: Maxime Agor (4rzael)
 * @Date:   Sat Jun 23 2018
 * @Email:  maxime.agor23@gmail.com
 * @Project: CUDA-Based Simulator of Quantum Systems
 * @Filename: FloatExprAst.hpp
 * @Last modified by:   vial-dj
 * @Last modified time: Wed Nov 14 2018, 12:00:20
 * @License: MIT License
 */

/**
 * The float expression evaluator is highly inspired from theses codes:
 * https://github.com/djowel/spirit_x3/tree/master/example/x3
 */

#pragma once

#include <list>
#include <boost/fusion/include/adapt_struct.hpp>
#include <boost/spirit/home/x3/support/ast/variant.hpp>
#include <boost/variant/static_visitor.hpp>

/* Float expr AST */
namespace Parser {
    namespace AST {
        struct t_float_expr_unaried_operand;
        struct t_float_expr_operand;
        struct t_float_expression;
        class OperandPrinterVisitor;

        typedef ::boost::variant<double, std::string> t_float;

        struct t_float_expr_nil {
        };

        struct t_float_expr_operand : ::boost::spirit::x3::variant<
                t_float_expr_nil
                , t_float
                , ::boost::spirit::x3::forward_ast<t_float_expr_unaried_operand>
                , ::boost::spirit::x3::forward_ast<t_float_expression>
            >
        {
            using base_type::base_type;
            using base_type::operator=;
        };
        std::ostream& operator<< (std::ostream& stream, const t_float_expr_operand& operand);

        struct t_float_expr_operation
        {
            char operator_;
            t_float_expr_operand operand_;
        };
        std::ostream& operator<< (std::ostream& stream, const t_float_expr_operation& operation);

        /**
         * @brief The AST node representing a floating-point expression
         *
         * Example: (3/4) * pi + (-a ^ 2)
         */
        struct t_float_expression
        {
            t_float_expr_operand first;
            std::list<t_float_expr_operation> rest;
            t_float_expression(double op) : first(op) {}
            t_float_expression() {}
        };
        std::ostream& operator<< (std::ostream& stream, const t_float_expression& expression);

        struct t_float_expr_unaried_operand
        {
            std::string unary_;
            t_float_expression operand_;
        };
        std::ostream& operator<< (std::ostream& stream, const t_float_expr_unaried_operand& unop);

        class OperandPrinterVisitor : public ::boost::static_visitor<>
        {
        private:
            std::ostream & m_out;
        public:
            explicit OperandPrinterVisitor(std::ostream & out) : m_out(out) {}
            void operator()(const t_float_expr_nil &nil) const;
            void operator()(const t_float &v) const;
            void operator()(const ::boost::spirit::x3::forward_ast<t_float_expr_unaried_operand> &ast) const;
            void operator()(const ::boost::spirit::x3::forward_ast<t_float_expression> &ast) const;
        };

        class TFloatPrinterVisitor : public ::boost::static_visitor<>
        {
        private:
            std::ostream & m_out;
        public:
            explicit TFloatPrinterVisitor(std::ostream & out) : m_out(out) {}
            void operator()(const double &f) const;
            void operator()(const std::string &s) const;
        };
    }
}

BOOST_FUSION_ADAPT_STRUCT(
    Parser::AST::t_float_expr_unaried_operand,
    (std::string, unary_)
    (Parser::AST::t_float_expression, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    Parser::AST::t_float_expr_operation,
    (char, operator_)
    (Parser::AST::t_float_expr_operand, operand_)
)

BOOST_FUSION_ADAPT_STRUCT(
    Parser::AST::t_float_expression,
    (Parser::AST::t_float_expr_operand, first)
    (std::list<Parser::AST::t_float_expr_operation>, rest)
)


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

  @copyright 2016 NumScale SAS

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

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

 /*!

    @ingroup group-reduction
    Function object implementing cummin capabilities

    Computes the cumulate minimum of the vector elements

    @par Semantic:

    For every pack parameter of type @c T

    @code
    auto r = cummin(x);
    @endcode

    is similar to:

    @code
    auto  r = x;
    for(int i=1;i < T::static_size; ++i)
      r[i] = min(r[i-1], r[i]);
    @endcode

  **/
  Value cummin(Value const & v0);
} }
#endif

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

#endif