<commit_before>#include <Rcpp11>
#include <iostream>
#include <math.h>
#include <numeric>

using namespace Rcpp;
using namespace std;

double loglik(NumericVector eta, NumericVector y, NumericVector weights, Function linkinv, Function devfun)
{
    try {
        // Compute some values we need for the logLik computation:
        NumericVector mu = linkinv(eta);

        // Calculate the actual log likelihood:
        NumericVector dev_resids = devfun(y, mu, weights);
        double ll = sum(dev_resids);
        return(ll);
    }
    catch (...) {
        cout << "Error in loglik!" << endl;

        cout << "eta: ";
        for( auto c : eta)
            cout << c << ' ';
        cout << endl;

        cout << "y: ";
        for( auto c : y)
            cout << c << ' ';
        cout << endl;

        cout << "weights: ";
        for( auto c : weights)
            cout << c << ' ';
        cout << endl;
    }
    return 0.;
}

void gradCalc(NumericVector eta, NumericVector y, NumericVector weights, Function linkinv, NumericVector ldot)
{
    // Compute some values we need for the gradient computation:
    NumericVector mu = linkinv(eta);
    double sumw = sum(weights);

    // Calculate the actual log likelihood:
    ldot = weights * (mu-y) / sumw;
}

void rcppLinSolver(NumericMatrix X, NumericVector y, NumericVector w, NumericVector adaweights, int nrow, int ncol, int numGroup, NumericMatrix beta, Function linkinv, Function devfun, IntegerVector rangeGroupInd, IntegerVector groupLen, NumericVector lambda, int step, int innerIter, double thresh, NumericVector ldot, NumericVector nullBeta, double gamma, NumericVector eta, IntegerVector betaIsZero, int& groupChange, IntegerVector isActive, IntegerVector useGroup, int reset)
{
    NumericVector theta(ncol);
    int startInd = 0;
    double zeroCheck = 0;
    double check = 0;
    int count = 0;
    double diff = 1;
    double norm = 0;
    double uOp = 0;
    double Lnew = 0;
    double Lold = 0;
    double sqNormDelta = 0;
    double iProd = 0;
    NumericVector etaNew(nrow);
    NumericVector etaNull(nrow);
    NumericVector var(nrow);

    for(int i = 0; i < numGroup; i++)
    {
        if(useGroup[i] == 1)
        {
            startInd = rangeGroupInd[i];

            // Setting up null residuals calc to check if group is 0
            // Null residuals means the coefficients of group i are set to zero before computing residuals.
            for(int k = 0; k < nrow; k++)
            {
                etaNull[k] = eta[k];
                for(int j = startInd; j < rangeGroupInd[i] + groupLen[i]; j++)
                {
                    etaNull[k] = etaNull[k] - X[k + nrow * j] * beta[step*ncol + j];
                }
            }
            gradCalc(etaNull, y, w, linkinv, ldot);

            // Now compute the correlation of this group with the null residuals.
            NumericVector grad(groupLen[i]);
            for(int j = 0; j < groupLen[i]; j++)
            {
                grad[j] = 0;
                for(int k = 0; k < nrow; k++)
                {
                    grad[j] = grad[j] + X[k + nrow * (j + rangeGroupInd[i])] * ldot[k];
                }
            }
            zeroCheck = sum(pow(grad,2));

            // Is this group's correlation with the null residuals smaller than the threshold?
            // If so, set the coefficients of the group to zero.
            if(zeroCheck <= pow(adaweights[i],2)*pow(lambda[step],2)*groupLen[i])
            {
                if(betaIsZero[i] == 0)
                {
                    for(int k = 0; k < nrow; k++)
                    {
                        for(int j = rangeGroupInd[i]; j < rangeGroupInd[i] + groupLen[i]; j++)
                        {
                            eta[k] = eta[k] - X[k + nrow * j] * beta[step*ncol + j];
                        }
                    }
                }
                betaIsZero[i] = 1;
                for(int j = 0; j < groupLen[i]; j++)
                {
                    beta[step*ncol + j + rangeGroupInd[i]] = 0;
                }
            }
            else // The group's coefficients are nonzero
            {
                if(isActive[i] == 0)
                {
                    groupChange = 1;
                }
                isActive[i] = 1;

                // Hold the previous values of the coefficients
                for(int k = 0; k < ncol; k++)
                {
                    theta[k] = beta[step*ncol + k];
                }

                betaIsZero[i] = 0;
                NumericVector z(groupLen[i]);
                NumericVector U(groupLen[i]);
                NumericVector delta(groupLen[i]);
                NumericVector betaNew(ncol);

                count = 0;
                check = 100000;
                double t = 1;

                // Until convergence, iteratively recompute the group's coefficients:
                while(count <= innerIter && check > thresh)
                {
                    count++;

                    // Compute the working residuals (i.e. residuals at the current coefficient values)
                    gradCalc(eta, y, w, linkinv, ldot);

                    // Compute the group's correlation with the working residuals
                    for(int j = 0; j < groupLen[i]; j++)
                    {
                        grad[j] = 0;
                        for(int k = 0; k < nrow; k++)
                        {
                            grad[j] = grad[j] + X[k + nrow * (j + rangeGroupInd[i])] * ldot[k];
                        }
                    }

                    // Compute the log likelihood for the previous coefficient iteration
                    Lold = loglik(eta, y, w, linkinv, devfun);

                    // Back-tracking to optimize the step size for gradient descent:
                    diff = -1;
                    int optim_steps = 0;
                    while(diff < 0)
                    {
                        optim_steps++;

                        for(int j = 0; j < groupLen[i]; j++)
                        {
                            z[j] = beta[step*ncol + j + rangeGroupInd[i]] - t * grad[j];
                        }

                        norm = sum(pow(z, 2));
                        norm = sqrt(norm);

                        if(norm != 0){
                            uOp = (1 - adaweights[i]*lambda[step]*sqrt(double(groupLen[i]))*t/norm);   //Or not?
                        }
                        else{uOp = 0;}

                        if(uOp < 0)
                        {
                            uOp = 0;
                        }

                        for(int j = 0; j < groupLen[i]; j++)
                        {
                            U[j] = uOp*z[j];
                            delta[j] = U[j] - beta[step*ncol + j + rangeGroupInd[i]];
                        }

                        // Setting up betaNew and etaNew in direction of Grad for descent momentum
                        for(int k = 0; k < nrow; k++)
                        {
                            etaNew[k] = eta[k];
                            for(int j = 0; j < groupLen[i]; j++)
                            {
                                etaNew[k] = etaNew[k] + delta[j] * X[k + nrow*(rangeGroupInd[i] + j)];
                            }
                        }

                        // Compute log likelihood for the working coefficients
                        Lnew = loglik(etaNew, y, w, linkinv, devfun);

                        sqNormDelta = sum(pow(delta, 2));
                        iProd = sum(grad * delta);

                        // Check whether the working coefficients reduce the log likelihood
                        diff = Lold - Lnew + iProd + 0.5/t * sqNormDelta;

                        // Reduce the step size for the next iteration
                        t = t * gamma;
                    }
                    t = t / gamma;

                    check = 0;

                    for(int j = 0; j < groupLen[i]; j++)
                    {
                        // Check the difference between iterations of the coefficients
                        check = check + fabs(theta[j + rangeGroupInd[i]] - U[j]);

                        // Calculate the null etas (i.e. the etas when group i's coefficients are zero)
                        for(int k = 0; k < nrow; k++)
                        {
                            eta[k] = eta[k] - X[k + nrow * (j + rangeGroupInd[i])]*beta[step*ncol + j + rangeGroupInd[i]];
                        }

                        //Apply Nesterov acceleration to calculate the new coefficient values:
                        beta[step*ncol + j + rangeGroupInd[i]] = U[j] + count/(count+3) * (U[j] - theta[j + rangeGroupInd[i]]);
                        theta[j + rangeGroupInd[i]] = U[j];

                        // Compute the new values of eta after iterating group i's coefficients.
                        for(int k = 0; k < nrow; k++)
                        {
                            eta[k] = eta[k] + X[k + nrow * (j + rangeGroupInd[i])]*beta[step*ncol + j + rangeGroupInd[i]];
                        }
                    }
                }
            }
        }
    }
}

// [[Rcpp::export]]
double killTest(Function ll, double x)
{
    NumericVector y = ll(x);
    return y[0];
}

// [[Rcpp::export]]
int rcppLinNest(NumericMatrix X, NumericVector y, NumericVector w, NumericVector adaweights, Function linkinv, Function devfun, int nrow, int ncol, int numGroup, IntegerVector rangeGroupInd, IntegerVector groupLen, NumericVector lambda, NumericMatrix beta, int innerIter, int outerIter, double thresh, double outerThresh, NumericVector eta, double gamma, IntegerVector betaIsZero, int reset)
{
    NumericVector prob(nrow);
    NumericVector nullBeta(ncol);
    int n = nrow;
    int p = ncol;
    NumericVector ldot(n);
    IntegerVector isActive(numGroup);
    IntegerVector useGroup(numGroup);
    IntegerVector tempIsActive(numGroup);
    int nlam = lambda.size();

    for (int step=0; step<nlam; step++)
    {
        for(int i=0; i<numGroup; i++)
        {
            isActive[i] = 0;
            useGroup[i] = 1;
        }

        //Copy the most recent betas into position
        if (step>0)
        {
            int l = step - 1;

            for (int i=0; i<ncol; i++)
            {
                beta[step*ncol + i] = beta[l*ncol + i];
            }
        }

        // outer most loop creating response etc...
        int outermostCounter = 0;
        double outermostCheck = 100000;
        NumericVector outerOldBeta(p);
        int groupChange = 1;

        while(groupChange == 1)
        {
            groupChange = 0;

            rcppLinSolver(X, y, w, adaweights, nrow, ncol, numGroup, beta, linkinv, devfun, rangeGroupInd, groupLen, lambda, step, innerIter, thresh, ldot, nullBeta, gamma, eta, betaIsZero, groupChange, isActive, useGroup, reset);

            while(outermostCounter < outerIter && outermostCheck > outerThresh)
            {
                outermostCounter ++;
                for(int i=0; i<p; i++)
                {
                    outerOldBeta[i] = beta[step*ncol + i];
                }

                for(int i=0; i<numGroup; i++)
                {
                    tempIsActive[i] = isActive[i];
                }

                rcppLinSolver(X, y, w, adaweights, nrow, ncol, numGroup, beta, linkinv, devfun, rangeGroupInd, groupLen, lambda, step, innerIter, thresh, ldot, nullBeta, gamma, eta, betaIsZero, groupChange, isActive, tempIsActive, reset);

                outermostCheck = 0;
                for(int i=0; i<p; i++)
                {
                    outermostCheck = outermostCheck + fabs(outerOldBeta[i] - beta[step*ncol + i]);
                }
                outermostCheck = outermostCheck / abs(sum(outerOldBeta));
            }
        }
    }
    return 1;
}
<commit_msg>added a catch block in gradCalc<commit_after>#include <Rcpp11>
#include <iostream>
#include <math.h>
#include <numeric>

using namespace Rcpp;
using namespace std;

double loglik(NumericVector eta, NumericVector y, NumericVector weights, Function linkinv, Function devfun)
{
    try {
        // Compute some values we need for the logLik computation:
        NumericVector mu = linkinv(eta);

        // Calculate the actual log likelihood:
        NumericVector dev_resids = devfun(y, mu, weights);
        double ll = sum(dev_resids);
        return(ll);
    }
    catch (...) {
        cout << "Error in loglik!" << endl;

        cout << "eta: ";
        for( auto c : eta)
            cout << c << ' ';
        cout << endl;

        cout << "y: ";
        for( auto c : y)
            cout << c << ' ';
        cout << endl;

        cout << "weights: ";
        for( auto c : weights)
            cout << c << ' ';
        cout << endl;
    }
    return 0.;
}

void gradCalc(NumericVector eta, NumericVector y, NumericVector weights, Function linkinv, NumericVector ldot)
{
    try {
        // Compute some values we need for the gradient computation:
        NumericVector mu = linkinv(eta);
        double sumw = sum(weights);

        // Calculate the actual log likelihood:
        ldot = weights * (mu-y) / sumw;
    }
    catch (...) {
        cout << "Error in gradCalc!" << endl;

        cout << "eta: ";
        for( auto c : eta)
            cout << c << ' ';
        cout << endl;

        cout << "y: ";
        for( auto c : y)
            cout << c << ' ';
        cout << endl;

        cout << "weights: ";
        for( auto c : weights)
            cout << c << ' ';
        cout << endl;

        cout << "ldot: ";
        for( auto c : weights)
            cout << c << ' ';
        cout << endl;
    }

}

void rcppLinSolver(NumericMatrix X, NumericVector y, NumericVector w, NumericVector adaweights, int nrow, int ncol, int numGroup, NumericMatrix beta, Function linkinv, Function devfun, IntegerVector rangeGroupInd, IntegerVector groupLen, NumericVector lambda, int step, int innerIter, double thresh, NumericVector ldot, NumericVector nullBeta, double gamma, NumericVector eta, IntegerVector betaIsZero, int& groupChange, IntegerVector isActive, IntegerVector useGroup, int reset)
{
    NumericVector theta(ncol);
    int startInd = 0;
    double zeroCheck = 0;
    double check = 0;
    int count = 0;
    double diff = 1;
    double norm = 0;
    double uOp = 0;
    double Lnew = 0;
    double Lold = 0;
    double sqNormDelta = 0;
    double iProd = 0;
    NumericVector etaNew(nrow);
    NumericVector etaNull(nrow);
    NumericVector var(nrow);

    for(int i = 0; i < numGroup; i++)
    {
        if(useGroup[i] == 1)
        {
            startInd = rangeGroupInd[i];

            // Setting up null residuals calc to check if group is 0
            // Null residuals means the coefficients of group i are set to zero before computing residuals.
            for(int k = 0; k < nrow; k++)
            {
                etaNull[k] = eta[k];
                for(int j = startInd; j < rangeGroupInd[i] + groupLen[i]; j++)
                {
                    etaNull[k] = etaNull[k] - X[k + nrow * j] * beta[step*ncol + j];
                }
            }
            gradCalc(etaNull, y, w, linkinv, ldot);

            // Now compute the correlation of this group with the null residuals.
            NumericVector grad(groupLen[i]);
            for(int j = 0; j < groupLen[i]; j++)
            {
                grad[j] = 0;
                for(int k = 0; k < nrow; k++)
                {
                    grad[j] = grad[j] + X[k + nrow * (j + rangeGroupInd[i])] * ldot[k];
                }
            }
            zeroCheck = sum(pow(grad,2));

            // Is this group's correlation with the null residuals smaller than the threshold?
            // If so, set the coefficients of the group to zero.
            if(zeroCheck <= pow(adaweights[i],2)*pow(lambda[step],2)*groupLen[i])
            {
                if(betaIsZero[i] == 0)
                {
                    for(int k = 0; k < nrow; k++)
                    {
                        for(int j = rangeGroupInd[i]; j < rangeGroupInd[i] + groupLen[i]; j++)
                        {
                            eta[k] = eta[k] - X[k + nrow * j] * beta[step*ncol + j];
                        }
                    }
                }
                betaIsZero[i] = 1;
                for(int j = 0; j < groupLen[i]; j++)
                {
                    beta[step*ncol + j + rangeGroupInd[i]] = 0;
                }
            }
            else // The group's coefficients are nonzero
            {
                if(isActive[i] == 0)
                {
                    groupChange = 1;
                }
                isActive[i] = 1;

                // Hold the previous values of the coefficients
                for(int k = 0; k < ncol; k++)
                {
                    theta[k] = beta[step*ncol + k];
                }

                betaIsZero[i] = 0;
                NumericVector z(groupLen[i]);
                NumericVector U(groupLen[i]);
                NumericVector delta(groupLen[i]);
                NumericVector betaNew(ncol);

                count = 0;
                check = 100000;
                double t = 1;

                // Until convergence, iteratively recompute the group's coefficients:
                while(count <= innerIter && check > thresh)
                {
                    count++;

                    // Compute the working residuals (i.e. residuals at the current coefficient values)
                    gradCalc(eta, y, w, linkinv, ldot);

                    // Compute the group's correlation with the working residuals
                    for(int j = 0; j < groupLen[i]; j++)
                    {
                        grad[j] = 0;
                        for(int k = 0; k < nrow; k++)
                        {
                            grad[j] = grad[j] + X[k + nrow * (j + rangeGroupInd[i])] * ldot[k];
                        }
                    }

                    // Compute the log likelihood for the previous coefficient iteration
                    Lold = loglik(eta, y, w, linkinv, devfun);

                    // Back-tracking to optimize the step size for gradient descent:
                    diff = -1;
                    int optim_steps = 0;
                    while(diff < 0)
                    {
                        optim_steps++;

                        for(int j = 0; j < groupLen[i]; j++)
                        {
                            z[j] = beta[step*ncol + j + rangeGroupInd[i]] - t * grad[j];
                        }

                        norm = sum(pow(z, 2));
                        norm = sqrt(norm);

                        if(norm != 0){
                            uOp = (1 - adaweights[i]*lambda[step]*sqrt(double(groupLen[i]))*t/norm);   //Or not?
                        }
                        else{uOp = 0;}

                        if(uOp < 0)
                        {
                            uOp = 0;
                        }

                        for(int j = 0; j < groupLen[i]; j++)
                        {
                            U[j] = uOp*z[j];
                            delta[j] = U[j] - beta[step*ncol + j + rangeGroupInd[i]];
                        }

                        // Setting up betaNew and etaNew in direction of Grad for descent momentum
                        for(int k = 0; k < nrow; k++)
                        {
                            etaNew[k] = eta[k];
                            for(int j = 0; j < groupLen[i]; j++)
                            {
                                etaNew[k] = etaNew[k] + delta[j] * X[k + nrow*(rangeGroupInd[i] + j)];
                            }
                        }

                        // Compute log likelihood for the working coefficients
                        Lnew = loglik(etaNew, y, w, linkinv, devfun);

                        sqNormDelta = sum(pow(delta, 2));
                        iProd = sum(grad * delta);

                        // Check whether the working coefficients reduce the log likelihood
                        diff = Lold - Lnew + iProd + 0.5/t * sqNormDelta;

                        // Reduce the step size for the next iteration
                        t = t * gamma;
                    }
                    t = t / gamma;

                    check = 0;

                    for(int j = 0; j < groupLen[i]; j++)
                    {
                        // Check the difference between iterations of the coefficients
                        check = check + fabs(theta[j + rangeGroupInd[i]] - U[j]);

                        // Calculate the null etas (i.e. the etas when group i's coefficients are zero)
                        for(int k = 0; k < nrow; k++)
                        {
                            eta[k] = eta[k] - X[k + nrow * (j + rangeGroupInd[i])]*beta[step*ncol + j + rangeGroupInd[i]];
                        }

                        //Apply Nesterov acceleration to calculate the new coefficient values:
                        beta[step*ncol + j + rangeGroupInd[i]] = U[j] + count/(count+3) * (U[j] - theta[j + rangeGroupInd[i]]);
                        theta[j + rangeGroupInd[i]] = U[j];

                        // Compute the new values of eta after iterating group i's coefficients.
                        for(int k = 0; k < nrow; k++)
                        {
                            eta[k] = eta[k] + X[k + nrow * (j + rangeGroupInd[i])]*beta[step*ncol + j + rangeGroupInd[i]];
                        }
                    }
                }
            }
        }
    }
}

// [[Rcpp::export]]
double killTest(Function ll, double x)
{
    NumericVector y = ll(x);
    return y[0];
}

// [[Rcpp::export]]
int rcppLinNest(NumericMatrix X, NumericVector y, NumericVector w, NumericVector adaweights, Function linkinv, Function devfun, int nrow, int ncol, int numGroup, IntegerVector rangeGroupInd, IntegerVector groupLen, NumericVector lambda, NumericMatrix beta, int innerIter, int outerIter, double thresh, double outerThresh, NumericVector eta, double gamma, IntegerVector betaIsZero, int reset)
{
    NumericVector prob(nrow);
    NumericVector nullBeta(ncol);
    int n = nrow;
    int p = ncol;
    NumericVector ldot(n);
    IntegerVector isActive(numGroup);
    IntegerVector useGroup(numGroup);
    IntegerVector tempIsActive(numGroup);
    int nlam = lambda.size();

    for (int step=0; step<nlam; step++)
    {
        for(int i=0; i<numGroup; i++)
        {
            isActive[i] = 0;
            useGroup[i] = 1;
        }

        //Copy the most recent betas into position
        if (step>0)
        {
            int l = step - 1;

            for (int i=0; i<ncol; i++)
            {
                beta[step*ncol + i] = beta[l*ncol + i];
            }
        }

        // outer most loop creating response etc...
        int outermostCounter = 0;
        double outermostCheck = 100000;
        NumericVector outerOldBeta(p);
        int groupChange = 1;

        while(groupChange == 1)
        {
            groupChange = 0;

            rcppLinSolver(X, y, w, adaweights, nrow, ncol, numGroup, beta, linkinv, devfun, rangeGroupInd, groupLen, lambda, step, innerIter, thresh, ldot, nullBeta, gamma, eta, betaIsZero, groupChange, isActive, useGroup, reset);

            while(outermostCounter < outerIter && outermostCheck > outerThresh)
            {
                outermostCounter ++;
                for(int i=0; i<p; i++)
                {
                    outerOldBeta[i] = beta[step*ncol + i];
                }

                for(int i=0; i<numGroup; i++)
                {
                    tempIsActive[i] = isActive[i];
                }

                rcppLinSolver(X, y, w, adaweights, nrow, ncol, numGroup, beta, linkinv, devfun, rangeGroupInd, groupLen, lambda, step, innerIter, thresh, ldot, nullBeta, gamma, eta, betaIsZero, groupChange, isActive, tempIsActive, reset);

                outermostCheck = 0;
                for(int i=0; i<p; i++)
                {
                    outermostCheck = outermostCheck + fabs(outerOldBeta[i] - beta[step*ncol + i]);
                }
                outermostCheck = outermostCheck / abs(sum(outerOldBeta));
            }
        }
    }
    return 1;
}
<|endoftext|>


<commit_before>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2012.  All rights reserved
 */
#ifndef _Stroika_Foundation_Execution_CriticalSection_inl_
#define _Stroika_Foundation_Execution_CriticalSection_inl_  1

/*
 ********************************************************************************
 ***************************** Implementation Details ***************************
 ********************************************************************************
 */
#include    "Exceptions.h"

namespace   Stroika {
    namespace   Foundation {
        namespace   Execution {

            //  class   CriticalSection
            inline  CriticalSection::CriticalSection ()
            {
#if qUseThreads_WindowsNative
                memset (&fCritSec, 0, sizeof(CRITICAL_SECTION));
                ::InitializeCriticalSection (&fCritSec);
#endif
            }
            inline  CriticalSection::~CriticalSection()
            {
#if qUseThreads_WindowsNative
                IgnoreExceptionsForCall (::DeleteCriticalSection (&fCritSec));
#endif
            }
            inline  void    CriticalSection::Lock ()
            {
#if     qUseThreads_WindowsNative
                ::EnterCriticalSection (&fCritSec);
#elif       qUseThreads_StdCPlusPlus
                fMutex_.lock ();
#endif
            }
            inline  void CriticalSection::Unlock()
            {
#if     qUseThreads_WindowsNative
                ::LeaveCriticalSection (&fCritSec);
#elif       qUseThreads_StdCPlusPlus
                fMutex_.unlock ();
#endif
            }

            //  class   AutoCriticalSection
            template    <typename LOCKTYPE>
            inline  AutoCriticalSectionT<LOCKTYPE>::AutoCriticalSectionT (LOCKTYPE& critSec)
                : fCritSec (critSec)
            {
                fCritSec.Lock ();
            }
            template    <typename LOCKTYPE>
            inline  AutoCriticalSectionT<LOCKTYPE>::~AutoCriticalSectionT ()
            {
                fCritSec.Unlock ();
            }

        }
    }
}
#endif  /*_Stroika_Foundation_Execution_CriticalSection_inl_*/
<commit_msg>minor code cleanup<commit_after>/*
 * Copyright(c) Sophist Solutions, Inc. 1990-2012.  All rights reserved
 */
#ifndef _Stroika_Foundation_Execution_CriticalSection_inl_
#define _Stroika_Foundation_Execution_CriticalSection_inl_  1

/*
 ********************************************************************************
 ***************************** Implementation Details ***************************
 ********************************************************************************
 */
#include    "Exceptions.h"

namespace   Stroika {
    namespace   Foundation {
        namespace   Execution {

            //  class   CriticalSection
            inline  CriticalSection::CriticalSection ()
            {
#if qUseThreads_WindowsNative
                memset (&fCritSec, 0, sizeof(fCritSec));
                ::InitializeCriticalSection (&fCritSec);
#endif
            }
            inline  CriticalSection::~CriticalSection()
            {
#if qUseThreads_WindowsNative
                IgnoreExceptionsForCall (::DeleteCriticalSection (&fCritSec));
#endif
            }
            inline  void    CriticalSection::Lock ()
            {
#if     qUseThreads_WindowsNative
                ::EnterCriticalSection (&fCritSec);
#elif       qUseThreads_StdCPlusPlus
                fMutex_.lock ();
#endif
            }
            inline  void CriticalSection::Unlock()
            {
#if     qUseThreads_WindowsNative
                ::LeaveCriticalSection (&fCritSec);
#elif       qUseThreads_StdCPlusPlus
                fMutex_.unlock ();
#endif
            }

            //  class   AutoCriticalSection
            template    <typename LOCKTYPE>
            inline  AutoCriticalSectionT<LOCKTYPE>::AutoCriticalSectionT (LOCKTYPE& critSec)
                : fCritSec (critSec)
            {
                fCritSec.Lock ();
            }
            template    <typename LOCKTYPE>
            inline  AutoCriticalSectionT<LOCKTYPE>::~AutoCriticalSectionT ()
            {
                fCritSec.Unlock ();
            }

        }
    }
}
#endif  /*_Stroika_Foundation_Execution_CriticalSection_inl_*/
<|endoftext|>


<commit_before>// Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved.
// Copyright (c) 2021 - 2022 by Apex.AI Inc. All rights reserved.
//
// 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.
//
// SPDX-License-Identifier: Apache-2.0
#ifndef IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP
#define IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP

#include "iceoryx_hoofs/cxx/requires.hpp"

#include <cstdint>
#include <type_traits>
#include <utility>

namespace iox
{
namespace cxx
{
template <uint64_t N>
struct in_place_index;

template <typename T>
struct in_place_type;
namespace internal
{
template <typename N>
struct is_in_place_index : std::false_type
{
};

template <uint64_t N>
struct is_in_place_index<in_place_index<N>> : std::true_type
{
};

template <typename T>
struct is_in_place_type : std::false_type
{
};

template <typename T>
struct is_in_place_type<in_place_type<T>> : std::true_type
{
};

using byte_t = uint8_t;
template <typename TypeToCheck, typename T, typename... Targs>
struct does_contain_type
{
    static constexpr bool value =
        std::is_same<TypeToCheck, T>::value || does_contain_type<TypeToCheck, Targs...>::value;
};

template <typename TypeToCheck, typename T>
struct does_contain_type<TypeToCheck, T>
{
    static constexpr bool value = std::is_same<TypeToCheck, T>::value;
};

template <uint64_t N, typename Type, typename T, typename... Targs>
struct get_index_of_type
{
    static constexpr uint64_t index = get_index_of_type<N + 1, Type, Targs...>::index;
};

template <uint64_t N, typename Type, typename... Targs>
struct get_index_of_type<N, Type, Type, Targs...>
{
    static constexpr uint64_t index = N;
};

template <uint64_t N, uint64_t Index, typename T, typename... Targs>
struct get_type_at_index
{
    using type = typename get_type_at_index<N + 1, Index, Targs...>::type;
};

template <uint64_t N, typename T, typename... Targs>
struct get_type_at_index<N, N, T, Targs...>
{
    using type = T;
};

template <uint64_t N, typename T, typename... Targs>
struct call_at_index
{
    static void destructor(const uint64_t index, void* ptr) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            reinterpret_cast<T*>(ptr)->~T();
        }
        else
        {
            call_at_index<N + 1, Targs...>::destructor(index, ptr);
        }
    }

    static void move(const uint64_t index, void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T*>(destination) = std::move(*reinterpret_cast<T*>(source));
        }
        else
        {
            call_at_index<N + 1, Targs...>::move(index, source, destination);
        }
    }

    static void moveConstructor(const uint64_t index, void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(std::move(*reinterpret_cast<T*>(source)));
        }
        else
        {
            call_at_index<N + 1, Targs...>::moveConstructor(index, source, destination);
        }
    }

    static void copy(const uint64_t index, const void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T*>(destination) = *reinterpret_cast<const T*>(source);
        }
        else
        {
            call_at_index<N + 1, Targs...>::copy(index, source, destination);
        }
    }

    static void copyConstructor(const uint64_t index, const void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(*reinterpret_cast<const T*>(source));
        }
        else
        {
            call_at_index<N + 1, Targs...>::copyConstructor(index, source, destination);
        }
    }

    static bool equality(const uint64_t index, const void* lhs, const void* rhs)
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter and encapsulated in this class
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            return *reinterpret_cast<const T*>(lhs) == *reinterpret_cast<const T*>(rhs);
        }
        return call_at_index<N + 1, Targs...>::equality(index, lhs, rhs);
    }
};

template <uint64_t N, typename T>
struct call_at_index<N, T>
{
    // NOLINTJUSTIFICATION d'tor changes the data to which source is pointing to
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void destructor(const uint64_t index, void* ptr) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            reinterpret_cast<T*>(ptr)->~T();
        }
        else
        {
            ExpectsWithMsg(false, "Could not call destructor for variant element");
        }
    }

    // NOLINTJUSTIFICATION move c'tor changes the data to which source is pointing to
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void move(const uint64_t index, void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T*>(destination) = std::move(*reinterpret_cast<T*>(source));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call move assignment for variant element");
        }
    }

    // NOLINTJUSTIFICATION Both 'source' and 'destination' will be changed and can't be const
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void moveConstructor(const uint64_t index, void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(std::move(*reinterpret_cast<T*>(source)));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call move constructor for variant element");
        }
    }

    static void copy(const uint64_t index, const void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T*>(destination) = *reinterpret_cast<const T*>(source);
        }
        else
        {
            ExpectsWithMsg(false, "Could not call copy assignment for variant element");
        }
    }

    // NOLINTJUSTIFICATION 'operator new()' needs non-const 'destination'
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void copyConstructor(const uint64_t index, const void* source, void* destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(*reinterpret_cast<const T*>(source));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call copy constructor for variant element");
        }
    }

    static bool equality(const uint64_t index, const void* lhs, const void* rhs) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter and encapsulated in this class
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            return *reinterpret_cast<const T*>(lhs) == *reinterpret_cast<const T*>(rhs);
        }
        ExpectsWithMsg(false, "Could not call equality operator for variant element");
        return false;
    }
};

} // namespace internal
} // namespace cxx
} // namespace iox

#endif // IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP
<commit_msg>iox-#1614 Const correctness in variant_internal.hpp<commit_after>// Copyright (c) 2019 by Robert Bosch GmbH. All rights reserved.
// Copyright (c) 2021 - 2022 by Apex.AI Inc. All rights reserved.
//
// 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.
//
// SPDX-License-Identifier: Apache-2.0
#ifndef IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP
#define IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP

#include "iceoryx_hoofs/cxx/requires.hpp"

#include <cstdint>
#include <type_traits>
#include <utility>

namespace iox
{
namespace cxx
{
template <uint64_t N>
struct in_place_index;

template <typename T>
struct in_place_type;
namespace internal
{
template <typename N>
struct is_in_place_index : std::false_type
{
};

template <uint64_t N>
struct is_in_place_index<in_place_index<N>> : std::true_type
{
};

template <typename T>
struct is_in_place_type : std::false_type
{
};

template <typename T>
struct is_in_place_type<in_place_type<T>> : std::true_type
{
};

using byte_t = uint8_t;
template <typename TypeToCheck, typename T, typename... Targs>
struct does_contain_type
{
    static constexpr bool value =
        std::is_same<TypeToCheck, T>::value || does_contain_type<TypeToCheck, Targs...>::value;
};

template <typename TypeToCheck, typename T>
struct does_contain_type<TypeToCheck, T>
{
    static constexpr bool value = std::is_same<TypeToCheck, T>::value;
};

template <uint64_t N, typename Type, typename T, typename... Targs>
struct get_index_of_type
{
    static constexpr uint64_t index = get_index_of_type<N + 1, Type, Targs...>::index;
};

template <uint64_t N, typename Type, typename... Targs>
struct get_index_of_type<N, Type, Type, Targs...>
{
    static constexpr uint64_t index = N;
};

template <uint64_t N, uint64_t Index, typename T, typename... Targs>
struct get_type_at_index
{
    using type = typename get_type_at_index<N + 1, Index, Targs...>::type;
};

template <uint64_t N, typename T, typename... Targs>
struct get_type_at_index<N, N, T, Targs...>
{
    using type = T;
};

template <uint64_t N, typename T, typename... Targs>
struct call_at_index
{
    static void destructor(const uint64_t index, void* ptr) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            reinterpret_cast<T*>(ptr)->~T();
        }
        else
        {
            call_at_index<N + 1, Targs...>::destructor(index, ptr);
        }
    }

    static void move(const uint64_t index, void* source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T* const>(destination) = std::move(*reinterpret_cast<T*>(source));
        }
        else
        {
            call_at_index<N + 1, Targs...>::move(index, source, destination);
        }
    }

    static void moveConstructor(const uint64_t index, void* source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(std::move(*reinterpret_cast<T*>(source)));
        }
        else
        {
            call_at_index<N + 1, Targs...>::moveConstructor(index, source, destination);
        }
    }

    static void copy(const uint64_t index, const void* const source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T* const>(destination) = *reinterpret_cast<const T* const>(source);
        }
        else
        {
            call_at_index<N + 1, Targs...>::copy(index, source, destination);
        }
    }

    static void copyConstructor(const uint64_t index, const void* const source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(*reinterpret_cast<const T* const>(source));
        }
        else
        {
            call_at_index<N + 1, Targs...>::copyConstructor(index, source, destination);
        }
    }

    static bool equality(const uint64_t index, const void* const lhs, const void* const rhs)
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter and encapsulated in this class
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            return *reinterpret_cast<const T* const>(lhs) == *reinterpret_cast<const T* const>(rhs);
        }
        return call_at_index<N + 1, Targs...>::equality(index, lhs, rhs);
    }
};

template <uint64_t N, typename T>
struct call_at_index<N, T>
{
    // NOLINTJUSTIFICATION d'tor changes the data to which ptr is pointing to
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void destructor(const uint64_t index, void* ptr) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            reinterpret_cast<T*>(ptr)->~T();
        }
        else
        {
            ExpectsWithMsg(false, "Could not call destructor for variant element");
        }
    }

    // NOLINTJUSTIFICATION move c'tor changes the data to which source is pointing to
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void move(const uint64_t index, void* source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T* const>(destination) = std::move(*reinterpret_cast<T*>(source));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call move assignment for variant element");
        }
    }

    // NOLINTJUSTIFICATION Both 'source' and 'destination' will be changed and can't be const
    // NOLINTNEXTLINE(readability-non-const-parameter)
    static void moveConstructor(const uint64_t index, void* source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(std::move(*reinterpret_cast<T*>(source)));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call move constructor for variant element");
        }
    }

    static void copy(const uint64_t index, const void* const source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            *reinterpret_cast<T* const>(destination) = *reinterpret_cast<const T* const>(source);
        }
        else
        {
            ExpectsWithMsg(false, "Could not call copy assignment for variant element");
        }
    }

    static void copyConstructor(const uint64_t index, const void* const source, void* const destination) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            new (destination) T(*reinterpret_cast<const T* const>(source));
        }
        else
        {
            ExpectsWithMsg(false, "Could not call copy constructor for variant element");
        }
    }

    static bool equality(const uint64_t index, const void* const lhs, const void* const rhs) noexcept
    {
        if (N == index)
        {
            // AXIVION Next Construct AutosarC++19_03-A5.2.4 : Type safety ensured through template parameter and encapsulated in this class
            // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast)
            return *reinterpret_cast<const T* const>(lhs) == *reinterpret_cast<const T* const>(rhs);
        }
        ExpectsWithMsg(false, "Could not call equality operator for variant element");
        return false;
    }
};

} // namespace internal
} // namespace cxx
} // namespace iox

#endif // IOX_HOOFS_CXX_VARIANT_INTERNAL_HPP
<|endoftext|>


<commit_before>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <util/timer.h>
#include <httpclient/httpclient.h>
#include <util/filereader.h>
#include "client.h"

Client::Client(ClientArguments *args)
    : _args(args),
      _status(new ClientStatus()),
      _reqTimer(new Timer()),
      _cycleTimer(new Timer()),
      _masterTimer(new Timer()),
      _http(new HTTPClient(_args->_hostname, _args->_port,
                           _args->_keepAlive, _args->_headerBenchmarkdataCoverage,
                           _args->_extraHeaders, _args->_authority)),
      _reader(new FileReader()),
      _output(),
      _linebufsize(args->_maxLineSize),
      _linebuf(new char[_linebufsize]),
      _stop(false),
      _done(false),
      _thread()
{
    assert(args != NULL);
    _cycleTimer->SetMax(_args->_cycle);
}

Client::~Client()
{
    delete [] _linebuf;
}

void Client::runMe(Client * me) {
    me->run();
}

void
Client::run()
{
    char filename[1024];
    char timestr[64];
    int  linelen;
    ///   int  reslen;

    std::this_thread::sleep_for(std::chrono::milliseconds(_args->_delay));

    // open query file
    snprintf(filename, 1024, _args->_filenamePattern, _args->_myNum);
    if (!_reader->Open(filename)) {
        printf("Client %d: ERROR: could not open file '%s' [read mode]\n",
               _args->_myNum, filename);
        _status->SetError("Could not open query file.");
        return;
    }
    if (_args->_outputPattern != NULL) {
        snprintf(filename, 1024, _args->_outputPattern, _args->_myNum);
        _output = std::make_unique<std::ofstream>(filename, std::ofstream::out | std::ofstream::binary);
        if (_output->fail()) {
            printf("Client %d: ERROR: could not open file '%s' [write mode]\n",
                   _args->_myNum, filename);
            _status->SetError("Could not open output file.");
            return;
        }
    }
    if (_output)
        _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);

    if (_args->_ignoreCount == 0)
        _masterTimer->Start();

    // Start reading from offset
    if ( _args->_singleQueryFile )
        _reader->SetFilePos(_args->_queryfileOffset);

    // run queries
    while (!_stop) {

        _cycleTimer->Start();

        linelen = _reader->ReadLine(_linebuf, _linebufsize);

        // Read maximum to _queryfileOffsetEnd
        if ( _args->_singleQueryFile && _reader->GetBufPos() >= _args->_queryfileBytes ) {
            _reader->SetFilePos(_args->_queryfileOffset);
        }

        if (linelen < 0) {
            _reader->Reset();
            // Start reading from offset
            if ( _args->_singleQueryFile ) {
                _reader->SetFilePos(_args->_queryfileOffset);
            }

            linelen = _reader->ReadLine(_linebuf, _linebufsize);
            if (linelen < 0) {
                fprintf(stderr, "Client %d: ERROR: could not read any lines from '%s'\n",
                        _args->_myNum, filename);
                _status->SetError("Could not read any lines from query file.");
                break;
            }
            if (_args->_restartLimit == 0) {
                break;
            } else if (_args->_restartLimit > 0) {
                _args->_restartLimit--;
            }
        }
        if (linelen < _linebufsize) {
            if (_output) {
                _output->write("URL: ", strlen("URL: "));
                _output->write(_linebuf, linelen);
                _output->write("\n\n", 2);
            }
            if (linelen + (int)_args->_queryStringToAppend.length() < _linebufsize) {
                strcat(_linebuf, _args->_queryStringToAppend.c_str());
            }
            _reqTimer->Start();
            auto fetch_status = _http->Fetch(_linebuf, _output.get());
            _reqTimer->Stop();
            _status->AddRequestStatus(fetch_status.RequestStatus());
            if (fetch_status.Ok() && fetch_status.TotalHitCount() == 0)
                ++_status->_zeroHitQueries;
            if (_output) {
                if (!fetch_status.Ok()) {
                    _output->write("\nFBENCH: URL FETCH FAILED!\n",
                                          strlen("\nFBENCH: URL FETCH FAILED!\n"));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                } else {
                    sprintf(timestr, "\nTIME USED: %0.4f s\n",
                            _reqTimer->GetTimespan() / 1000.0);
                    _output->write(timestr, strlen(timestr));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                }
            }
            if (fetch_status.ResultSize() >= _args->_byteLimit) {
                if (_args->_ignoreCount == 0)
                    _status->ResponseTime(_reqTimer->GetTimespan());
            } else {
                if (_args->_ignoreCount == 0)
                    _status->RequestFailed();
            }
        } else {
            if (_args->_ignoreCount == 0)
                _status->SkippedRequest();
        }
        _cycleTimer->Stop();
        if (_args->_cycle < 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(int(_reqTimer->GetTimespan())));
        } else {
            if (_cycleTimer->GetRemaining() > 0) {
                std::this_thread::sleep_for(std::chrono::milliseconds(int(_cycleTimer->GetRemaining())));
            } else {
                if (_args->_ignoreCount == 0)
                    _status->OverTime();
            }
        }
        if (_args->_ignoreCount > 0) {
            _args->_ignoreCount--;
            if (_args->_ignoreCount == 0)
                _masterTimer->Start();
        }
        // Update current time span to calculate Q/s
        _status->SetRealTime(_masterTimer->GetCurrent());
    }
    _masterTimer->Stop();
    _status->SetRealTime(_masterTimer->GetTimespan());
    _status->SetReuseCount(_http->GetReuseCount());
    printf(".");
    fflush(stdout);
    _done = true;
}

void Client::stop() {
    _stop = true;
}

bool Client::done() {
    return _done;
}

void Client::start() {
    _thread = std::thread(Client::runMe, this);
}

void Client::join() {
    _thread.join();
}
<commit_msg>Ensure that you report the correct filename when producing error message.<commit_after>// Copyright 2016 Yahoo Inc. Licensed under the terms of the Apache 2.0 license. See LICENSE in the project root.
#include <util/timer.h>
#include <httpclient/httpclient.h>
#include <util/filereader.h>
#include "client.h"

Client::Client(ClientArguments *args)
    : _args(args),
      _status(new ClientStatus()),
      _reqTimer(new Timer()),
      _cycleTimer(new Timer()),
      _masterTimer(new Timer()),
      _http(new HTTPClient(_args->_hostname, _args->_port,
                           _args->_keepAlive, _args->_headerBenchmarkdataCoverage,
                           _args->_extraHeaders, _args->_authority)),
      _reader(new FileReader()),
      _output(),
      _linebufsize(args->_maxLineSize),
      _linebuf(new char[_linebufsize]),
      _stop(false),
      _done(false),
      _thread()
{
    assert(args != NULL);
    _cycleTimer->SetMax(_args->_cycle);
}

Client::~Client()
{
    delete [] _linebuf;
}

void Client::runMe(Client * me) {
    me->run();
}

void
Client::run()
{
    char inputFilename[1024];
    char outputFilename[1024];
    char timestr[64];
    int  linelen;
    ///   int  reslen;

    std::this_thread::sleep_for(std::chrono::milliseconds(_args->_delay));

    // open query file
    snprintf(inputFilename, 1024, _args->_filenamePattern, _args->_myNum);
    if (!_reader->Open(inputFilename)) {
        printf("Client %d: ERROR: could not open file '%s' [read mode]\n",
               _args->_myNum, inputFilename);
        _status->SetError("Could not open query file.");
        return;
    }
    if (_args->_outputPattern != NULL) {
        snprintf(outputFilename, 1024, _args->_outputPattern, _args->_myNum);
        _output = std::make_unique<std::ofstream>(outputFilename, std::ofstream::out | std::ofstream::binary);
        if (_output->fail()) {
            printf("Client %d: ERROR: could not open file '%s' [write mode]\n",
                   _args->_myNum, outputFilename);
            _status->SetError("Could not open output file.");
            return;
        }
    }
    if (_output)
        _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);

    if (_args->_ignoreCount == 0)
        _masterTimer->Start();

    // Start reading from offset
    if ( _args->_singleQueryFile )
        _reader->SetFilePos(_args->_queryfileOffset);

    // run queries
    while (!_stop) {

        _cycleTimer->Start();

        linelen = _reader->ReadLine(_linebuf, _linebufsize);

        // Read maximum to _queryfileOffsetEnd
        if ( _args->_singleQueryFile && _reader->GetBufPos() >= _args->_queryfileBytes ) {
            _reader->SetFilePos(_args->_queryfileOffset);
        }

        if (linelen < 0) {
            _reader->Reset();
            // Start reading from offset
            if ( _args->_singleQueryFile ) {
                _reader->SetFilePos(_args->_queryfileOffset);
            }

            linelen = _reader->ReadLine(_linebuf, _linebufsize);
            if (linelen < 0) {
                fprintf(stderr, "Client %d: ERROR: could not read any lines from '%s'\n",
                        _args->_myNum, inputFilename);
                _status->SetError("Could not read any lines from query file.");
                break;
            }
            if (_args->_restartLimit == 0) {
                break;
            } else if (_args->_restartLimit > 0) {
                _args->_restartLimit--;
            }
        }
        if (linelen < _linebufsize) {
            if (_output) {
                _output->write("URL: ", strlen("URL: "));
                _output->write(_linebuf, linelen);
                _output->write("\n\n", 2);
            }
            if (linelen + (int)_args->_queryStringToAppend.length() < _linebufsize) {
                strcat(_linebuf, _args->_queryStringToAppend.c_str());
            }
            _reqTimer->Start();
            auto fetch_status = _http->Fetch(_linebuf, _output.get());
            _reqTimer->Stop();
            _status->AddRequestStatus(fetch_status.RequestStatus());
            if (fetch_status.Ok() && fetch_status.TotalHitCount() == 0)
                ++_status->_zeroHitQueries;
            if (_output) {
                if (!fetch_status.Ok()) {
                    _output->write("\nFBENCH: URL FETCH FAILED!\n",
                                          strlen("\nFBENCH: URL FETCH FAILED!\n"));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                } else {
                    sprintf(timestr, "\nTIME USED: %0.4f s\n",
                            _reqTimer->GetTimespan() / 1000.0);
                    _output->write(timestr, strlen(timestr));
                    _output->write(FBENCH_DELIMITER + 1, strlen(FBENCH_DELIMITER) - 1);
                }
            }
            if (fetch_status.ResultSize() >= _args->_byteLimit) {
                if (_args->_ignoreCount == 0)
                    _status->ResponseTime(_reqTimer->GetTimespan());
            } else {
                if (_args->_ignoreCount == 0)
                    _status->RequestFailed();
            }
        } else {
            if (_args->_ignoreCount == 0)
                _status->SkippedRequest();
        }
        _cycleTimer->Stop();
        if (_args->_cycle < 0) {
            std::this_thread::sleep_for(std::chrono::milliseconds(int(_reqTimer->GetTimespan())));
        } else {
            if (_cycleTimer->GetRemaining() > 0) {
                std::this_thread::sleep_for(std::chrono::milliseconds(int(_cycleTimer->GetRemaining())));
            } else {
                if (_args->_ignoreCount == 0)
                    _status->OverTime();
            }
        }
        if (_args->_ignoreCount > 0) {
            _args->_ignoreCount--;
            if (_args->_ignoreCount == 0)
                _masterTimer->Start();
        }
        // Update current time span to calculate Q/s
        _status->SetRealTime(_masterTimer->GetCurrent());
    }
    _masterTimer->Stop();
    _status->SetRealTime(_masterTimer->GetTimespan());
    _status->SetReuseCount(_http->GetReuseCount());
    printf(".");
    fflush(stdout);
    _done = true;
}

void Client::stop() {
    _stop = true;
}

bool Client::done() {
    return _done;
}

void Client::start() {
    _thread = std::thread(Client::runMe, this);
}

void Client::join() {
    _thread.join();
}
<|endoftext|>


<commit_before>#include <sstream>
#include <julia.h>
#include "debug.h"

using namespace std;

string nj::getTypeName(jl_value_t *v)
{
   string res = "none";
   stringstream ss;

   if(v)
   {
     if(jl_is_structtype(v)) ss << "value is struct_type";
     if(jl_is_datatype(v)) ss << "value is type datatype";
     if(jl_is_expr(v)) ss << "value is expr";
     if(jl_is_tuple(v)) ss << "value is tuple";
     if(jl_is_vararg_type(v)) ss << "value is vararg";
     if(jl_is_func(v)) ss << "value is func";
     if(jl_is_byte_string(v)) ss << "value is string";
     if(jl_is_uniontype(v)) ss << "value is union";
     if(jl_is_typector(v)) ss << "value is ctor";
     if(jl_is_symbol(v)) ss << "value is jl_is_symbol";
     if(jl_is_lambda_info(v)) ss << "value is jl_is_lambda_info";
     if(jl_is_vararg_type(v)) ss << "value is jl_is_vararg_type";
     if(jl_is_getfieldnode(v)) ss << "value is jl_is_getfieldnode";
     if(jl_is_datatype(jl_typeof(v))) ss << "value is a data type";

     string tmp = ss.str();

     if(tmp != "") res = tmp;
   }

   return res;
}
<commit_msg>Commented unneeded jl_is_getfieldnode<commit_after>#include <sstream>
#include <julia.h>
#include "debug.h"

using namespace std;

string nj::getTypeName(jl_value_t *v)
{
   string res = "none";
   stringstream ss;

   if(v)
   {
     if(jl_is_structtype(v)) ss << "value is struct_type";
     if(jl_is_datatype(v)) ss << "value is type datatype";
     if(jl_is_expr(v)) ss << "value is expr";
     if(jl_is_tuple(v)) ss << "value is tuple";
     if(jl_is_vararg_type(v)) ss << "value is vararg";
     if(jl_is_func(v)) ss << "value is func";
     if(jl_is_byte_string(v)) ss << "value is string";
     if(jl_is_uniontype(v)) ss << "value is union";
     if(jl_is_typector(v)) ss << "value is ctor";
     if(jl_is_symbol(v)) ss << "value is jl_is_symbol";
     if(jl_is_lambda_info(v)) ss << "value is jl_is_lambda_info";
     if(jl_is_vararg_type(v)) ss << "value is jl_is_vararg_type";
     //if(jl_is_getfieldnode(v)) ss << "value is jl_is_getfieldnode";
     if(jl_is_datatype(jl_typeof(v))) ss << "value is a data type";

     string tmp = ss.str();

     if(tmp != "") res = tmp;
   }

   return res;
}
<|endoftext|>


<commit_before>#include "model.hpp"

namespace Model {
}
<commit_msg>Added destructor<commit_after>#include "model.hpp"

namespace Model {
	model::~model(void) {}
}
<|endoftext|>


<commit_before>#include "stdafx.h"
#include "CppUnitTest.h"

#include "../dummy_values.h"
#include "internal/operations/erase_operation.h"
#include "internal/operations/post_operation.h"
#include "internal/operations/put_operation.h"
#include "internal/internal_datastore.h"

using Assert = Microsoft::VisualStudio::CppUnitTestFramework::Assert;

namespace You {
namespace DataStore {
namespace UnitTests {

using DataStore = You::DataStore::Internal::DataStore;

/// Unit Test Class for DataStore class
TEST_CLASS(DataStoreTest) {
public:
	TEST_METHOD(beginTransaction) {
		DataStore& sut = DataStore::get();
		Assert::IsTrue(sut.transactionStack.empty());
		Transaction t(sut.begin());
		Assert::AreEqual(1U, sut.transactionStack.size());
	}

	TEST_METHOD(pushedOperationsAddedToTransactionOperationsQueue) {
		DataStore& sut = DataStore::get();
		Transaction t(sut.begin());
		sut.post(10, task1);
		Assert::AreEqual(1U, t->operationsQueue.size());
		sut.put(10, task2);
		Assert::AreEqual(2U, t->operationsQueue.size());
		sut.erase(10);
		Assert::AreEqual(3U, t->operationsQueue.size());
	}

	TEST_METHOD(commitChangesDocumentTree) {
		DataStore& sut = DataStore::get();

		sut.document.reset();
		sut.saveData();
		assert(sut.document.first_child.empty());

		Transaction t(sut.begin());
		sut.post(10, task1);
		// document must not change without commit
		Assert::IsTrue(sut.document.first_child().empty());

		t.commit();
		// document changes after commit
		Assert::IsFalse(sut.document.first_child().empty());

		Transaction t2(sut.begin());
		sut.erase(10);
		// document must not change without commit
		Assert::IsFalse(sut.document.first_child().empty());
		t2.commit();
		// document changes after commit
		Assert::IsTrue(sut.document.first_child().empty());
	}

	TEST_METHOD(rollbackCleanUpTransactionStack) {
		DataStore& sut = DataStore::get();
		Transaction t(sut.begin());
		Assert::AreEqual(1U, sut.transactionStack.size());
		t.rollback();
		Assert::AreEqual(0U, sut.transactionStack.size());
	}

	TEST_METHOD(getAllTasks) {
		DataStore& sut = DataStore::get();
		sut.document.append_child(L"task").
			append_child(pugi::xml_node_type::node_pcdata).set_value(L"what");
		std::vector<SerializedTask> result = sut.getAllTask();
		Assert::AreEqual(1U, result.size());

		sut.document.reset();
		sut.saveData();
	}

	TEST_METHOD(saveThenLoad) {
		DataStore& sut = DataStore::get();
		sut.document.append_child(L"task").
			append_child(pugi::xml_node_type::node_pcdata).set_value(L"what");
		bool result = sut.saveData();
		Assert::IsTrue(result);
		sut.loadData();
		std::wstring value = sut.document.child(L"task").child_value();
		Assert::AreEqual(std::wstring(L"what"), value);

		sut.document.reset();
		sut.saveData();
	}

	TEST_METHOD(pushOperationToTransactionWithoutDataStore) {
		Internal::Transaction sut;

		std::unique_ptr<Internal::IOperation> post =
			std::make_unique<Internal::PostOperation>(0, task1);
		sut.push(std::move(post));
		Assert::AreEqual(1U, sut.operationsQueue.size());

		std::unique_ptr<Internal::IOperation> put =
			std::make_unique<Internal::PutOperation>(0, task1);
		sut.push(std::move(put));
		Assert::AreEqual(2U, sut.operationsQueue.size());

		std::unique_ptr<Internal::IOperation> erase =
			std::make_unique<Internal::EraseOperation>(0);
		sut.push(std::move(erase));
		Assert::AreEqual(3U, sut.operationsQueue.size());

		sut.operationsQueue.clear();
	}
};
}  // namespace UnitTests
}  // namespace DataStore
}  // namespace You
<commit_msg>Better name for getAllTask test, clean up properly<commit_after>#include "stdafx.h"
#include "CppUnitTest.h"

#include "../dummy_values.h"
#include "internal/operations/erase_operation.h"
#include "internal/operations/post_operation.h"
#include "internal/operations/put_operation.h"
#include "internal/internal_datastore.h"

using Assert = Microsoft::VisualStudio::CppUnitTestFramework::Assert;

namespace You {
namespace DataStore {
namespace UnitTests {

using DataStore = You::DataStore::Internal::DataStore;

/// Unit Test Class for DataStore class
TEST_CLASS(DataStoreTest) {
public:
	TEST_METHOD(beginTransactionAddToTransactionStack) {
		DataStore& sut = DataStore::get();
		Assert::IsTrue(sut.transactionStack.empty());
		Transaction t(sut.begin());
		Assert::AreEqual(1U, sut.transactionStack.size());
	}

	TEST_METHOD(pushedOperationsAddedToTransactionOperationsQueue) {
		DataStore& sut = DataStore::get();
		Transaction t(sut.begin());
		sut.post(10, task1);
		Assert::AreEqual(1U, t->operationsQueue.size());
		sut.put(10, task2);
		Assert::AreEqual(2U, t->operationsQueue.size());
		sut.erase(10);
		Assert::AreEqual(3U, t->operationsQueue.size());
	}

	TEST_METHOD(commitChangesDocumentTree) {
		DataStore& sut = DataStore::get();

		sut.document.reset();
		sut.saveData();
		assert(sut.document.first_child.empty());

		Transaction t(sut.begin());
		sut.post(10, task1);
		// document must not change without commit
		Assert::IsTrue(sut.document.first_child().empty());

		t.commit();
		// document changes after commit
		Assert::IsFalse(sut.document.first_child().empty());

		Transaction t2(sut.begin());
		sut.erase(10);
		// document must not change without commit
		Assert::IsFalse(sut.document.first_child().empty());
		t2.commit();
		// document changes after commit
		Assert::IsTrue(sut.document.first_child().empty());
	}

	TEST_METHOD(rollbackCleanUpTransactionStack) {
		DataStore& sut = DataStore::get();
		Transaction t(sut.begin());
		Assert::AreEqual(1U, sut.transactionStack.size());
		t.rollback();
		Assert::AreEqual(0U, sut.transactionStack.size());
	}

	TEST_METHOD(getAllTasksFromTreeCorrectly) {
		DataStore& sut = DataStore::get();
		sut.document.append_child(L"task").
			append_child(pugi::xml_node_type::node_pcdata).set_value(L"what");
		std::vector<SerializedTask> result = sut.getAllTask();
		Assert::AreEqual(1U, result.size());

		// Clean up
		sut.document.reset();
		sut.saveData();
	}

	TEST_METHOD(saveThenLoad) {
		DataStore& sut = DataStore::get();
		sut.document.append_child(L"task").
			append_child(pugi::xml_node_type::node_pcdata).set_value(L"what");
		bool result = sut.saveData();
		Assert::IsTrue(result);
		sut.loadData();
		std::wstring value = sut.document.child(L"task").child_value();
		Assert::AreEqual(std::wstring(L"what"), value);

		sut.document.reset();
		sut.saveData();
	}

	TEST_METHOD(pushOperationToTransactionWithoutDataStore) {
		Internal::Transaction sut;

		std::unique_ptr<Internal::IOperation> post =
			std::make_unique<Internal::PostOperation>(0, task1);
		sut.push(std::move(post));
		Assert::AreEqual(1U, sut.operationsQueue.size());

		std::unique_ptr<Internal::IOperation> put =
			std::make_unique<Internal::PutOperation>(0, task1);
		sut.push(std::move(put));
		Assert::AreEqual(2U, sut.operationsQueue.size());

		std::unique_ptr<Internal::IOperation> erase =
			std::make_unique<Internal::EraseOperation>(0);
		sut.push(std::move(erase));
		Assert::AreEqual(3U, sut.operationsQueue.size());

		sut.operationsQueue.clear();
	}
};
}  // namespace UnitTests
}  // namespace DataStore
}  // namespace You
<|endoftext|>


<commit_before>//
// Copyright (c) 2016 CNRS
// Author: NMansard
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/device.hh>

#include <boost/foreach.hpp>
#include <Eigen/Core>

#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/geometry.hpp>

#include <hpp/pinocchio/fwd.hh>
//#include <hpp/pinocchio/distance-result.hh>
#include <hpp/pinocchio/extra-config-space.hh>
#include <hpp/pinocchio/joint.hh>

namespace hpp {
  namespace pinocchio {

    Device::
    Device(const std::string& name)
      : model_(new Model())
      , data_ ()
      , geomModel_(new GeomModel())
      , geomData_ ()
      , name_ (name)
      , jointVector_()
      , computationFlag_ (JOINT_POSITION)
      , obstacles_()
      , objectVector_ ()
      , weakPtr_()
    {
      invalidate();
    }

    // static method
    DevicePtr_t Device::
    create (const std::string & name)
    {
      DevicePtr_t res = DevicePtr_t(new Device(name)); // init shared ptr
      res->init (res);
      return res;
    }

    // static method
    DevicePtr_t Device::
    createCopy (const DevicePtr_t& device)
    {
      DevicePtr_t res = Device::create(device->name()); // init shared ptr
      res->model(device->modelPtr());  // Copy pointer to pinocchio model
      res->createData();    // Create a new data, dont copy the pointer.
      return res;
    }

    // static method
    DevicePtr_t Device::
    createCopyConst (const DeviceConstPtr_t& device)
    {
      DevicePtr_t res = Device::create(device->name()); // init shared ptr
      /* The copy of Pinocchio::Model is not implemented yet. */
      /* We need this feature to finish the implementation of this method. */
      assert( false && "TODO: createCopyConst is not implemented yet." );
      return res;
    }

    void Device::init(const DeviceWkPtr_t& weakPtr)
    {
      weakPtr_ = weakPtr;
      DevicePtr_t self (weakPtr_.lock());
      jointVector_ = JointVector(self);
      obstacles_ = ObjectVector(self,0,INNER);
      objectVector_ = DeviceObjectVector(self);
    }

    void Device::
    createData()
    {
      data_ = DataPtr_t( new Data(*model_) );
      // We assume that model is now complete and state can be resized.
      resizeState();
    }

    void Device::
    createGeomData()
    {
      geomData_ = GeomDataPtr_t( new GeomData(*geomModel_) );
      se3::computeBodyRadius(*model_,*geomModel_,*geomData_);
    }

    /* ---------------------------------------------------------------------- */
    /* --- JOINT ------------------------------------------------------------ */
    /* ---------------------------------------------------------------------- */

    JointPtr_t Device::
    rootJoint () const
    {
      return JointPtr_t( new Joint(weakPtr_.lock(),1) );
    }

    JointPtr_t Device::
    getJointAtConfigRank (const size_type& r) const
    {
      assert(model_);
      //BOOST_FOREACH( const se3::JointModel & j, // Skip "universe" joint
      //std::make_pair(model_->joints.begin()+1,model_->joints.end()) )
      BOOST_FOREACH( const se3::JointModel & j, model_->joints )
        {
          if( j.id()==0 ) continue; // Skip "universe" joint
          const size_type iq = j.idx_q() - r;
          if( 0 <= iq && iq < j.nq() ) return JointPtr_t( new Joint(weakPtr_.lock(),j.id()) );
        }
      assert(false && "The joint at config rank has not been found");
      return JointPtr_t();
    }

    JointPtr_t Device::
    getJointAtVelocityRank (const size_type& r) const
    {
      assert(model_);
      BOOST_FOREACH( const se3::JointModel & j,model_->joints )
        {
          if( j.id()==0 ) continue; // Skip "universe" joint
          const size_type iv = j.idx_v() - r;
          if( 0 <= iv && iv < j.nv() ) return JointPtr_t( new Joint(weakPtr_.lock(),j.id()) );
        }
      assert(false && "The joint at velocity rank has not been found");
      return JointPtr_t();
    }

    JointPtr_t Device::
    getJointByName (const std::string& name) const
    {
      assert(model_);
      if(! model_->existJointName(name))
	throw std::runtime_error ("Device " + name_ +
				  " does not have any joint named "
				  + name);
      JointIndex id = model_->getJointId(name);
      return JointPtr_t( new Joint(weakPtr_.lock(),id) );
    }

    JointPtr_t Device::
    getJointByBodyName (const std::string& name) const
    {
      assert(model_);
      if (model_->existFrame(name)) {
        se3::Model::FrameIndex bodyId = model_->getFrameId(name);
        if (model_->frames[bodyId].type == se3::BODY) {
          JointIndex jointId = model_->frames[bodyId].parent;
          //assert(jointId>=0);
          assert((int)jointId<model_->njoint);
          return JointPtr_t( new Joint(weakPtr_.lock(),jointId) );
        }
      }
      throw std::runtime_error ("Device " + name_ +
                                " has no joint with body of name "
                                + name);
    }

    size_type Device::
    configSize () const
    {
      assert(model_);
      return model_->nq + extraConfigSpace_.dimension();
    }

    size_type Device::
    numberDof () const
    {
      assert(model_);
      return model_->nv + extraConfigSpace_.dimension();
    }

    /* ---------------------------------------------------------------------- */
    /* --- CONFIG ----------------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

    /* Previous implementation of resizeState in hpp::model:: was setting the
     * new part of the configuration to neutral configuration. This is not
     * working but for empty extra-config. The former behavior is therefore not
     * propagated here. The configuration is resized without setting the new
     * memory.
     */
    void Device::
    resizeState()
    {
      // FIXME we should not use neutralConfiguration here.
      currentConfiguration_ = neutralConfiguration();
      // currentConfiguration_.resize(configSize());
      currentVelocity_.resize(numberDof());
      currentAcceleration_.resize(numberDof());
    }

    bool Device::
    currentConfiguration (ConfigurationIn_t configuration)
    {
      if (configuration != currentConfiguration_)
        {
          invalidate();
          currentConfiguration_ = configuration;
          return true;
	}
      return false;
    }

    Configuration_t Device::
    neutralConfiguration () const
    {
      Configuration_t n (configSize());
      n.head(model_->nq) = model().neutralConfiguration;
      n.tail(extraConfigSpace_.dimension()).setZero();
      return n;
    }

    const value_type& Device::
    mass () const
    {
      assert(data_);
      return data_->mass[0];
    }

    const vector3_t& Device::
    positionCenterOfMass () const
    {
      assert(data_);
      return data_->com[0];
    }

    const ComJacobian_t& Device::
    jacobianCenterOfMass () const
    {
      assert(data_);
      return data_->Jcom;
    }

    void Device::
    computeForwardKinematics ()
    {
      if(upToDate_) return;

      assert(model_);
      assert(data_);
      // a IMPLIES b === (b || ~a)
      // velocity IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&VELOCITY)) );
      // acceleration IMPLIES velocity
      assert( (computationFlag_&VELOCITY) || (!(computationFlag_&ACCELERATION)) );
      // com IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&COM)) );
      // jacobian IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&JACOBIAN)) );

      const size_type nq = model().nq;
      const size_type nv = model().nv;

      if (computationFlag_ & ACCELERATION )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq),
                               currentVelocity_.head(nv),currentAcceleration_.head(nv));
      else if (computationFlag_ & VELOCITY )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq),
                               currentVelocity_.head(nv));
      else if (computationFlag_ & JOINT_POSITION )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq));

      if (computationFlag_&COM)
        {
          if (computationFlag_|JACOBIAN)
            // TODO: Jcom should not recompute the kinematics (\sa pinocchio issue #219)
            se3::jacobianCenterOfMass(*model_,*data_,currentConfiguration_.head(nq),true);
          else
            se3::centerOfMass(*model_,*data_,currentConfiguration_.head(nq),true,false);
        }

      if(computationFlag_&JACOBIAN)
        se3::computeJacobians(*model_,*data_,currentConfiguration_.head(nq));
    }

    void Device::
    updateGeometryPlacements ()
    {
      if (!geomUpToDate_) {
        se3::updateGeometryPlacements(model(),data(),geomModel(),geomData());
        geomUpToDate_ = true;
      }
    }

    std::ostream& Device::
    print (std::ostream& os) const
    {
      for (JointVector::const_iterator it = jointVector_.begin (); it != jointVector_.end (); ++it)
        (*it)->display(os);
      return os;
    }

    /* ---------------------------------------------------------------------- */
    /* --- COLLISIONS ------------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

    bool Device::collisionTest (const bool stopAtFirstCollision)
    {
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
      updateGeometryPlacements();
      return se3::computeCollisions(geomData(),stopAtFirstCollision);
    }

    void Device::computeDistances ()
    {
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
      updateGeometryPlacements();
      se3::computeDistances (geomData());
    }

    const DistanceResults_t& Device::distanceResults () const
    {
      return geomData().distance_results;
    }

  } // namespace pinocchio
} // namespace hpp
<commit_msg>Device constructor initilize Data and GeometryData<commit_after>//
// Copyright (c) 2016 CNRS
// Author: NMansard
//
//
// This file is part of hpp-pinocchio
// hpp-pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-pinocchio  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/pinocchio/device.hh>

#include <boost/foreach.hpp>
#include <Eigen/Core>

#include <pinocchio/multibody/model.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/geometry.hpp>

#include <hpp/pinocchio/fwd.hh>
//#include <hpp/pinocchio/distance-result.hh>
#include <hpp/pinocchio/extra-config-space.hh>
#include <hpp/pinocchio/joint.hh>

namespace hpp {
  namespace pinocchio {

    Device::
    Device(const std::string& name)
      : model_(new Model())
      , data_ ()
      , geomModel_(new GeomModel())
      , geomData_ ()
      , name_ (name)
      , jointVector_()
      , computationFlag_ (JOINT_POSITION)
      , obstacles_()
      , objectVector_ ()
      , weakPtr_()
    {
      invalidate();
      createData();
      createGeomData();
    }

    // static method
    DevicePtr_t Device::
    create (const std::string & name)
    {
      DevicePtr_t res = DevicePtr_t(new Device(name)); // init shared ptr
      res->init (res);
      return res;
    }

    // static method
    DevicePtr_t Device::
    createCopy (const DevicePtr_t& device)
    {
      DevicePtr_t res = Device::create(device->name()); // init shared ptr
      res->model(device->modelPtr());  // Copy pointer to pinocchio model
      res->createData();    // Create a new data, dont copy the pointer.
      return res;
    }

    // static method
    DevicePtr_t Device::
    createCopyConst (const DeviceConstPtr_t& device)
    {
      DevicePtr_t res = Device::create(device->name()); // init shared ptr
      /* The copy of Pinocchio::Model is not implemented yet. */
      /* We need this feature to finish the implementation of this method. */
      assert( false && "TODO: createCopyConst is not implemented yet." );
      return res;
    }

    void Device::init(const DeviceWkPtr_t& weakPtr)
    {
      weakPtr_ = weakPtr;
      DevicePtr_t self (weakPtr_.lock());
      jointVector_ = JointVector(self);
      obstacles_ = ObjectVector(self,0,INNER);
      objectVector_ = DeviceObjectVector(self);
    }

    void Device::
    createData()
    {
      data_ = DataPtr_t( new Data(*model_) );
      // We assume that model is now complete and state can be resized.
      resizeState();
    }

    void Device::
    createGeomData()
    {
      geomData_ = GeomDataPtr_t( new GeomData(*geomModel_) );
      se3::computeBodyRadius(*model_,*geomModel_,*geomData_);
    }

    /* ---------------------------------------------------------------------- */
    /* --- JOINT ------------------------------------------------------------ */
    /* ---------------------------------------------------------------------- */

    JointPtr_t Device::
    rootJoint () const
    {
      return JointPtr_t( new Joint(weakPtr_.lock(),1) );
    }

    JointPtr_t Device::
    getJointAtConfigRank (const size_type& r) const
    {
      assert(model_);
      //BOOST_FOREACH( const se3::JointModel & j, // Skip "universe" joint
      //std::make_pair(model_->joints.begin()+1,model_->joints.end()) )
      BOOST_FOREACH( const se3::JointModel & j, model_->joints )
        {
          if( j.id()==0 ) continue; // Skip "universe" joint
          const size_type iq = j.idx_q() - r;
          if( 0 <= iq && iq < j.nq() ) return JointPtr_t( new Joint(weakPtr_.lock(),j.id()) );
        }
      assert(false && "The joint at config rank has not been found");
      return JointPtr_t();
    }

    JointPtr_t Device::
    getJointAtVelocityRank (const size_type& r) const
    {
      assert(model_);
      BOOST_FOREACH( const se3::JointModel & j,model_->joints )
        {
          if( j.id()==0 ) continue; // Skip "universe" joint
          const size_type iv = j.idx_v() - r;
          if( 0 <= iv && iv < j.nv() ) return JointPtr_t( new Joint(weakPtr_.lock(),j.id()) );
        }
      assert(false && "The joint at velocity rank has not been found");
      return JointPtr_t();
    }

    JointPtr_t Device::
    getJointByName (const std::string& name) const
    {
      assert(model_);
      if(! model_->existJointName(name))
	throw std::runtime_error ("Device " + name_ +
				  " does not have any joint named "
				  + name);
      JointIndex id = model_->getJointId(name);
      return JointPtr_t( new Joint(weakPtr_.lock(),id) );
    }

    JointPtr_t Device::
    getJointByBodyName (const std::string& name) const
    {
      assert(model_);
      if (model_->existFrame(name)) {
        se3::Model::FrameIndex bodyId = model_->getFrameId(name);
        if (model_->frames[bodyId].type == se3::BODY) {
          JointIndex jointId = model_->frames[bodyId].parent;
          //assert(jointId>=0);
          assert((int)jointId<model_->njoint);
          return JointPtr_t( new Joint(weakPtr_.lock(),jointId) );
        }
      }
      throw std::runtime_error ("Device " + name_ +
                                " has no joint with body of name "
                                + name);
    }

    size_type Device::
    configSize () const
    {
      assert(model_);
      return model_->nq + extraConfigSpace_.dimension();
    }

    size_type Device::
    numberDof () const
    {
      assert(model_);
      return model_->nv + extraConfigSpace_.dimension();
    }

    /* ---------------------------------------------------------------------- */
    /* --- CONFIG ----------------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

    /* Previous implementation of resizeState in hpp::model:: was setting the
     * new part of the configuration to neutral configuration. This is not
     * working but for empty extra-config. The former behavior is therefore not
     * propagated here. The configuration is resized without setting the new
     * memory.
     */
    void Device::
    resizeState()
    {
      // FIXME we should not use neutralConfiguration here.
      currentConfiguration_ = neutralConfiguration();
      // currentConfiguration_.resize(configSize());
      currentVelocity_.resize(numberDof());
      currentAcceleration_.resize(numberDof());
    }

    bool Device::
    currentConfiguration (ConfigurationIn_t configuration)
    {
      if (configuration != currentConfiguration_)
        {
          invalidate();
          currentConfiguration_ = configuration;
          return true;
	}
      return false;
    }

    Configuration_t Device::
    neutralConfiguration () const
    {
      Configuration_t n (configSize());
      n.head(model_->nq) = model().neutralConfiguration;
      n.tail(extraConfigSpace_.dimension()).setZero();
      return n;
    }

    const value_type& Device::
    mass () const
    {
      assert(data_);
      return data_->mass[0];
    }

    const vector3_t& Device::
    positionCenterOfMass () const
    {
      assert(data_);
      return data_->com[0];
    }

    const ComJacobian_t& Device::
    jacobianCenterOfMass () const
    {
      assert(data_);
      return data_->Jcom;
    }

    void Device::
    computeForwardKinematics ()
    {
      if(upToDate_) return;

      assert(model_);
      assert(data_);
      // a IMPLIES b === (b || ~a)
      // velocity IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&VELOCITY)) );
      // acceleration IMPLIES velocity
      assert( (computationFlag_&VELOCITY) || (!(computationFlag_&ACCELERATION)) );
      // com IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&COM)) );
      // jacobian IMPLIES position
      assert( (computationFlag_&JOINT_POSITION) || (!(computationFlag_&JACOBIAN)) );

      const size_type nq = model().nq;
      const size_type nv = model().nv;

      if (computationFlag_ & ACCELERATION )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq),
                               currentVelocity_.head(nv),currentAcceleration_.head(nv));
      else if (computationFlag_ & VELOCITY )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq),
                               currentVelocity_.head(nv));
      else if (computationFlag_ & JOINT_POSITION )
        se3::forwardKinematics(*model_,*data_,currentConfiguration_.head(nq));

      if (computationFlag_&COM)
        {
          if (computationFlag_|JACOBIAN)
            // TODO: Jcom should not recompute the kinematics (\sa pinocchio issue #219)
            se3::jacobianCenterOfMass(*model_,*data_,currentConfiguration_.head(nq),true);
          else
            se3::centerOfMass(*model_,*data_,currentConfiguration_.head(nq),true,false);
        }

      if(computationFlag_&JACOBIAN)
        se3::computeJacobians(*model_,*data_,currentConfiguration_.head(nq));
    }

    void Device::
    updateGeometryPlacements ()
    {
      if (!geomUpToDate_) {
        se3::updateGeometryPlacements(model(),data(),geomModel(),geomData());
        geomUpToDate_ = true;
      }
    }

    std::ostream& Device::
    print (std::ostream& os) const
    {
      for (JointVector::const_iterator it = jointVector_.begin (); it != jointVector_.end (); ++it)
        (*it)->display(os);
      return os;
    }

    /* ---------------------------------------------------------------------- */
    /* --- COLLISIONS ------------------------------------------------------- */
    /* ---------------------------------------------------------------------- */

    bool Device::collisionTest (const bool stopAtFirstCollision)
    {
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
      updateGeometryPlacements();
      return se3::computeCollisions(geomData(),stopAtFirstCollision);
    }

    void Device::computeDistances ()
    {
      /* Following hpp::model API, the forward kinematics (joint placement) is
       * supposed to have already been computed. */
      updateGeometryPlacements();
      se3::computeDistances (geomData());
    }

    const DistanceResults_t& Device::distanceResults () const
    {
      return geomData().distance_results;
    }

  } // namespace pinocchio
} // namespace hpp
<|endoftext|>


<commit_before>// The MIT License(MIT)
//
// Copyright 2017 bladez-fate
//
// 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.

#include "data.h"
#include "music.h"

#include "engine/easy.h"

namespace pilecode {

	namespace {
		int g_musicIdx = 0;
		bool g_musicDisabled = false;
	}

	void UpdateMusic()
	{
		if (!g_musicDisabled) {
			// switch background music tracks
			if (!music::g_background[g_musicIdx].IsPlaying()) {
				g_musicIdx = (g_musicIdx + 1) % music::g_backgroundCount;
				music::g_background[g_musicIdx].Play(0.2f);
			}
		}
		else {
			if (music::g_background[g_musicIdx].IsPlaying()) {
				music::g_background[g_musicIdx].Stop();
			}
			g_musicIdx = (g_musicIdx + 1) % music::g_backgroundCount;
		}
	}

	void ToggleMusic()
	{
		g_musicDisabled = !g_musicDisabled;
	}
}
<commit_msg>newline<commit_after>// The MIT License(MIT)
//
// Copyright 2017 bladez-fate
//
// 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.

#include "data.h"
#include "music.h"

#include "engine/easy.h"

namespace pilecode {

	namespace {
		int g_musicIdx = 0;
		bool g_musicDisabled = false;
	}

	void UpdateMusic()
	{
		if (!g_musicDisabled) {
			// switch background music tracks
			if (!music::g_background[g_musicIdx].IsPlaying()) {
				g_musicIdx = (g_musicIdx + 1) % music::g_backgroundCount;
				music::g_background[g_musicIdx].Play(0.2f);
			}
		}
		else {
			if (music::g_background[g_musicIdx].IsPlaying()) {
				music::g_background[g_musicIdx].Stop();
			}
			g_musicIdx = (g_musicIdx + 1) % music::g_backgroundCount;
		}
	}

	void ToggleMusic()
	{
		g_musicDisabled = !g_musicDisabled;
	}

}
<|endoftext|>