<commit_before>// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.
// See the LICENSE file in the project root for more information.

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XX                                                                           XX
XX             Lowering for ARM and ARM64 common code                        XX
XX                                                                           XX
XX  This encapsulates common logic for lowering trees for the ARM and ARM64  XX
XX  architectures.  For a more detailed view of what is lowering, please     XX
XX  take a look at Lower.cpp                                                 XX
XX                                                                           XX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
*/

#include "jitpch.h"
#ifdef _MSC_VER
#pragma hdrstop
#endif

#ifndef LEGACY_BACKEND // This file is ONLY used for the RyuJIT backend that uses the linear scan register allocator

#ifdef _TARGET_ARMARCH_ // This file is ONLY used for ARM and ARM64 architectures

#include "jit.h"
#include "sideeffects.h"
#include "lower.h"
#include "lsra.h"

//------------------------------------------------------------------------
// LowerStoreLoc: Lower a store of a lclVar
//
// Arguments:
//    storeLoc - the local store (GT_STORE_LCL_FLD or GT_STORE_LCL_VAR)
//
// Notes:
//    This involves:
//    - Widening operations of unsigneds.
//
void Lowering::LowerStoreLoc(GenTreeLclVarCommon* storeLoc)
{
    // Try to widen the ops if they are going into a local var.
    GenTree* op1 = storeLoc->gtGetOp1();
    if ((storeLoc->gtOper == GT_STORE_LCL_VAR) && (op1->gtOper == GT_CNS_INT))
    {
        GenTreeIntCon* con    = op1->AsIntCon();
        ssize_t        ival   = con->gtIconVal;
        unsigned       varNum = storeLoc->gtLclNum;
        LclVarDsc*     varDsc = comp->lvaTable + varNum;

        if (varDsc->lvIsSIMDType())
        {
            noway_assert(storeLoc->gtType != TYP_STRUCT);
        }
        unsigned size = genTypeSize(storeLoc);
        // If we are storing a constant into a local variable
        // we extend the size of the store here
        if ((size < 4) && !varTypeIsStruct(varDsc))
        {
            if (!varTypeIsUnsigned(varDsc))
            {
                if (genTypeSize(storeLoc) == 1)
                {
                    if ((ival & 0x7f) != ival)
                    {
                        ival = ival | 0xffffff00;
                    }
                }
                else
                {
                    assert(genTypeSize(storeLoc) == 2);
                    if ((ival & 0x7fff) != ival)
                    {
                        ival = ival | 0xffff0000;
                    }
                }
            }

            // A local stack slot is at least 4 bytes in size, regardless of
            // what the local var is typed as, so auto-promote it here
            // unless it is a field of a promoted struct
            // TODO-CQ: if the field is promoted shouldn't we also be able to do this?
            if (!varDsc->lvIsStructField)
            {
                storeLoc->gtType = TYP_INT;
                con->SetIconValue(ival);
            }
        }
    }
}

//------------------------------------------------------------------------
// LowerBlockStore: Set block store type
//
// Arguments:
//    blkNode       - The block store node of interest
//
// Return Value:
//    None.
//
void Lowering::LowerBlockStore(GenTreeBlk* blkNode)
{
    GenTree*  dstAddr  = blkNode->Addr();
    unsigned  size     = blkNode->gtBlkSize;
    GenTree*  source   = blkNode->Data();
    Compiler* compiler = comp;

    // Sources are dest address and initVal or source.
    GenTreePtr srcAddrOrFill = nullptr;
    bool       isInitBlk     = blkNode->OperIsInitBlkOp();

    if (!isInitBlk)
    {
        // CopyObj or CopyBlk
        if ((blkNode->OperGet() == GT_STORE_OBJ) && ((blkNode->AsObj()->gtGcPtrCount == 0) || blkNode->gtBlkOpGcUnsafe))
        {
            blkNode->SetOper(GT_STORE_BLK);
        }
        if (source->gtOper == GT_IND)
        {
            srcAddrOrFill = blkNode->Data()->gtGetOp1();
        }
    }

    if (isInitBlk)
    {
        GenTreePtr initVal = source;
        if (initVal->OperIsInitVal())
        {
            initVal = initVal->gtGetOp1();
        }
        srcAddrOrFill = initVal;

#ifdef _TARGET_ARM64_
        if ((size != 0) && (size <= INITBLK_UNROLL_LIMIT) && initVal->IsCnsIntOrI())
        {
            // TODO-ARM-CQ: Currently we generate a helper call for every
            // initblk we encounter.  Later on we should implement loop unrolling
            // code sequences to improve CQ.
            // For reference see the code in LowerXArch.cpp.
            NYI_ARM("initblk loop unrolling is currently not implemented.");

            // The fill value of an initblk is interpreted to hold a
            // value of (unsigned int8) however a constant of any size
            // may practically reside on the evaluation stack. So extract
            // the lower byte out of the initVal constant and replicate
            // it to a larger constant whose size is sufficient to support
            // the largest width store of the desired inline expansion.

            ssize_t fill = initVal->gtIntCon.gtIconVal & 0xFF;
            if (size < REGSIZE_BYTES)
            {
                initVal->gtIntCon.gtIconVal = 0x01010101 * fill;
            }
            else
            {
                initVal->gtIntCon.gtIconVal = 0x0101010101010101LL * fill;
                initVal->gtType             = TYP_LONG;
            }
            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
        }
        else
#endif // _TARGET_ARM64_
        {
            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindHelper;
        }
    }
    else
    {
        // CopyObj or CopyBlk
        // Sources are src and dest and size if not constant.

        if (blkNode->OperGet() == GT_STORE_OBJ)
        {
            // CopyObj
            GenTreeObj* objNode = blkNode->AsObj();

            unsigned slots = objNode->gtSlots;

#ifdef DEBUG
            // CpObj must always have at least one GC-Pointer as a member.
            assert(objNode->gtGcPtrCount > 0);

            assert(dstAddr->gtType == TYP_BYREF || dstAddr->gtType == TYP_I_IMPL);

            CORINFO_CLASS_HANDLE clsHnd    = objNode->gtClass;
            size_t               classSize = compiler->info.compCompHnd->getClassSize(clsHnd);
            size_t               blkSize   = roundUp(classSize, TARGET_POINTER_SIZE);

            // Currently, the EE always round up a class data structure so
            // we are not handling the case where we have a non multiple of pointer sized
            // struct. This behavior may change in the future so in order to keeps things correct
            // let's assert it just to be safe. Going forward we should simply
            // handle this case.
            assert(classSize == blkSize);
            assert((blkSize / TARGET_POINTER_SIZE) == slots);
            assert(objNode->HasGCPtr());
#endif

            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
        }
        else
        {
            // CopyBlk
            short     internalIntCount      = 0;
            regMaskTP internalIntCandidates = RBM_NONE;

#ifdef _TARGET_ARM64_
            // In case of a CpBlk with a constant size and less than CPBLK_UNROLL_LIMIT size
            // we should unroll the loop to improve CQ.
            // For reference see the code in lowerxarch.cpp.
            // TODO-ARM-CQ: cpblk loop unrolling is currently not implemented.

            if ((size != 0) && (size <= INITBLK_UNROLL_LIMIT))
            {
                blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
            }
            else
#endif // _TARGET_ARM64_
            {
                // In case we have a constant integer this means we went beyond
                // CPBLK_UNROLL_LIMIT bytes of size, still we should never have the case of
                // any GC-Pointers in the src struct.
                blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindHelper;
            }
        }
    }
}

//------------------------------------------------------------------------
// LowerCast: Lower GT_CAST(srcType, DstType) nodes.
//
// Arguments:
//    tree - GT_CAST node to be lowered
//
// Return Value:
//    None.
//
// Notes:
//    Casts from small int type to float/double are transformed as follows:
//    GT_CAST(byte, float/double)     =   GT_CAST(GT_CAST(byte, int32), float/double)
//    GT_CAST(sbyte, float/double)    =   GT_CAST(GT_CAST(sbyte, int32), float/double)
//    GT_CAST(int16, float/double)    =   GT_CAST(GT_CAST(int16, int32), float/double)
//    GT_CAST(uint16, float/double)   =   GT_CAST(GT_CAST(uint16, int32), float/double)
//
//    Similarly casts from float/double to a smaller int type are transformed as follows:
//    GT_CAST(float/double, byte)     =   GT_CAST(GT_CAST(float/double, int32), byte)
//    GT_CAST(float/double, sbyte)    =   GT_CAST(GT_CAST(float/double, int32), sbyte)
//    GT_CAST(float/double, int16)    =   GT_CAST(GT_CAST(double/double, int32), int16)
//    GT_CAST(float/double, uint16)   =   GT_CAST(GT_CAST(double/double, int32), uint16)
//
//    Note that for the overflow conversions we still depend on helper calls and
//    don't expect to see them here.
//    i) GT_CAST(float/double, int type with overflow detection)
//
void Lowering::LowerCast(GenTree* tree)
{
    assert(tree->OperGet() == GT_CAST);

    JITDUMP("LowerCast for: ");
    DISPNODE(tree);
    JITDUMP("\n");

    GenTreePtr op1     = tree->gtOp.gtOp1;
    var_types  dstType = tree->CastToType();
    var_types  srcType = op1->TypeGet();
    var_types  tmpType = TYP_UNDEF;

    if (varTypeIsFloating(srcType))
    {
        noway_assert(!tree->gtOverflow());
    }

    // Case of src is a small type and dst is a floating point type.
    if (varTypeIsSmall(srcType) && varTypeIsFloating(dstType))
    {
        NYI_ARM("Lowering for cast from small type to float"); // Not tested yet.
        // These conversions can never be overflow detecting ones.
        noway_assert(!tree->gtOverflow());
        tmpType = TYP_INT;
    }
    // case of src is a floating point type and dst is a small type.
    else if (varTypeIsFloating(srcType) && varTypeIsSmall(dstType))
    {
        NYI_ARM("Lowering for cast from float to small type"); // Not tested yet.
        tmpType = TYP_INT;
    }

    if (tmpType != TYP_UNDEF)
    {
        GenTreePtr tmp = comp->gtNewCastNode(tmpType, op1, tmpType);
        tmp->gtFlags |= (tree->gtFlags & (GTF_UNSIGNED | GTF_OVERFLOW | GTF_EXCEPT));

        tree->gtFlags &= ~GTF_UNSIGNED;
        tree->gtOp.gtOp1 = tmp;
        BlockRange().InsertAfter(op1, tmp);
    }
}

//------------------------------------------------------------------------
// LowerRotate: Lower GT_ROL and GT_ROL nodes.
//
// Arguments:
//    tree - the node to lower
//
// Return Value:
//    None.
//
void Lowering::LowerRotate(GenTreePtr tree)
{
    if (tree->OperGet() == GT_ROL)
    {
        // There is no ROL instruction on ARM. Convert ROL into ROR.
        GenTreePtr rotatedValue        = tree->gtOp.gtOp1;
        unsigned   rotatedValueBitSize = genTypeSize(rotatedValue->gtType) * 8;
        GenTreePtr rotateLeftIndexNode = tree->gtOp.gtOp2;

        if (rotateLeftIndexNode->IsCnsIntOrI())
        {
            ssize_t rotateLeftIndex                 = rotateLeftIndexNode->gtIntCon.gtIconVal;
            ssize_t rotateRightIndex                = rotatedValueBitSize - rotateLeftIndex;
            rotateLeftIndexNode->gtIntCon.gtIconVal = rotateRightIndex;
        }
        else
        {
            GenTreePtr tmp =
                comp->gtNewOperNode(GT_NEG, genActualType(rotateLeftIndexNode->gtType), rotateLeftIndexNode);
            BlockRange().InsertAfter(rotateLeftIndexNode, tmp);
            tree->gtOp.gtOp2 = tmp;
        }
        tree->ChangeOper(GT_ROR);
    }
}

#endif // _TARGET_ARMARCH_

#endif // !LEGACY_BACKEND
<commit_msg>[RyuJIT/ARM32] Enable casting from small int to float<commit_after>// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.
// See the LICENSE file in the project root for more information.

/*XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XX                                                                           XX
XX             Lowering for ARM and ARM64 common code                        XX
XX                                                                           XX
XX  This encapsulates common logic for lowering trees for the ARM and ARM64  XX
XX  architectures.  For a more detailed view of what is lowering, please     XX
XX  take a look at Lower.cpp                                                 XX
XX                                                                           XX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
XXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXXX
*/

#include "jitpch.h"
#ifdef _MSC_VER
#pragma hdrstop
#endif

#ifndef LEGACY_BACKEND // This file is ONLY used for the RyuJIT backend that uses the linear scan register allocator

#ifdef _TARGET_ARMARCH_ // This file is ONLY used for ARM and ARM64 architectures

#include "jit.h"
#include "sideeffects.h"
#include "lower.h"
#include "lsra.h"

//------------------------------------------------------------------------
// LowerStoreLoc: Lower a store of a lclVar
//
// Arguments:
//    storeLoc - the local store (GT_STORE_LCL_FLD or GT_STORE_LCL_VAR)
//
// Notes:
//    This involves:
//    - Widening operations of unsigneds.
//
void Lowering::LowerStoreLoc(GenTreeLclVarCommon* storeLoc)
{
    // Try to widen the ops if they are going into a local var.
    GenTree* op1 = storeLoc->gtGetOp1();
    if ((storeLoc->gtOper == GT_STORE_LCL_VAR) && (op1->gtOper == GT_CNS_INT))
    {
        GenTreeIntCon* con    = op1->AsIntCon();
        ssize_t        ival   = con->gtIconVal;
        unsigned       varNum = storeLoc->gtLclNum;
        LclVarDsc*     varDsc = comp->lvaTable + varNum;

        if (varDsc->lvIsSIMDType())
        {
            noway_assert(storeLoc->gtType != TYP_STRUCT);
        }
        unsigned size = genTypeSize(storeLoc);
        // If we are storing a constant into a local variable
        // we extend the size of the store here
        if ((size < 4) && !varTypeIsStruct(varDsc))
        {
            if (!varTypeIsUnsigned(varDsc))
            {
                if (genTypeSize(storeLoc) == 1)
                {
                    if ((ival & 0x7f) != ival)
                    {
                        ival = ival | 0xffffff00;
                    }
                }
                else
                {
                    assert(genTypeSize(storeLoc) == 2);
                    if ((ival & 0x7fff) != ival)
                    {
                        ival = ival | 0xffff0000;
                    }
                }
            }

            // A local stack slot is at least 4 bytes in size, regardless of
            // what the local var is typed as, so auto-promote it here
            // unless it is a field of a promoted struct
            // TODO-CQ: if the field is promoted shouldn't we also be able to do this?
            if (!varDsc->lvIsStructField)
            {
                storeLoc->gtType = TYP_INT;
                con->SetIconValue(ival);
            }
        }
    }
}

//------------------------------------------------------------------------
// LowerBlockStore: Set block store type
//
// Arguments:
//    blkNode       - The block store node of interest
//
// Return Value:
//    None.
//
void Lowering::LowerBlockStore(GenTreeBlk* blkNode)
{
    GenTree*  dstAddr  = blkNode->Addr();
    unsigned  size     = blkNode->gtBlkSize;
    GenTree*  source   = blkNode->Data();
    Compiler* compiler = comp;

    // Sources are dest address and initVal or source.
    GenTreePtr srcAddrOrFill = nullptr;
    bool       isInitBlk     = blkNode->OperIsInitBlkOp();

    if (!isInitBlk)
    {
        // CopyObj or CopyBlk
        if ((blkNode->OperGet() == GT_STORE_OBJ) && ((blkNode->AsObj()->gtGcPtrCount == 0) || blkNode->gtBlkOpGcUnsafe))
        {
            blkNode->SetOper(GT_STORE_BLK);
        }
        if (source->gtOper == GT_IND)
        {
            srcAddrOrFill = blkNode->Data()->gtGetOp1();
        }
    }

    if (isInitBlk)
    {
        GenTreePtr initVal = source;
        if (initVal->OperIsInitVal())
        {
            initVal = initVal->gtGetOp1();
        }
        srcAddrOrFill = initVal;

#ifdef _TARGET_ARM64_
        if ((size != 0) && (size <= INITBLK_UNROLL_LIMIT) && initVal->IsCnsIntOrI())
        {
            // TODO-ARM-CQ: Currently we generate a helper call for every
            // initblk we encounter.  Later on we should implement loop unrolling
            // code sequences to improve CQ.
            // For reference see the code in LowerXArch.cpp.
            NYI_ARM("initblk loop unrolling is currently not implemented.");

            // The fill value of an initblk is interpreted to hold a
            // value of (unsigned int8) however a constant of any size
            // may practically reside on the evaluation stack. So extract
            // the lower byte out of the initVal constant and replicate
            // it to a larger constant whose size is sufficient to support
            // the largest width store of the desired inline expansion.

            ssize_t fill = initVal->gtIntCon.gtIconVal & 0xFF;
            if (size < REGSIZE_BYTES)
            {
                initVal->gtIntCon.gtIconVal = 0x01010101 * fill;
            }
            else
            {
                initVal->gtIntCon.gtIconVal = 0x0101010101010101LL * fill;
                initVal->gtType             = TYP_LONG;
            }
            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
        }
        else
#endif // _TARGET_ARM64_
        {
            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindHelper;
        }
    }
    else
    {
        // CopyObj or CopyBlk
        // Sources are src and dest and size if not constant.

        if (blkNode->OperGet() == GT_STORE_OBJ)
        {
            // CopyObj
            GenTreeObj* objNode = blkNode->AsObj();

            unsigned slots = objNode->gtSlots;

#ifdef DEBUG
            // CpObj must always have at least one GC-Pointer as a member.
            assert(objNode->gtGcPtrCount > 0);

            assert(dstAddr->gtType == TYP_BYREF || dstAddr->gtType == TYP_I_IMPL);

            CORINFO_CLASS_HANDLE clsHnd    = objNode->gtClass;
            size_t               classSize = compiler->info.compCompHnd->getClassSize(clsHnd);
            size_t               blkSize   = roundUp(classSize, TARGET_POINTER_SIZE);

            // Currently, the EE always round up a class data structure so
            // we are not handling the case where we have a non multiple of pointer sized
            // struct. This behavior may change in the future so in order to keeps things correct
            // let's assert it just to be safe. Going forward we should simply
            // handle this case.
            assert(classSize == blkSize);
            assert((blkSize / TARGET_POINTER_SIZE) == slots);
            assert(objNode->HasGCPtr());
#endif

            blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
        }
        else
        {
            // CopyBlk
            short     internalIntCount      = 0;
            regMaskTP internalIntCandidates = RBM_NONE;

#ifdef _TARGET_ARM64_
            // In case of a CpBlk with a constant size and less than CPBLK_UNROLL_LIMIT size
            // we should unroll the loop to improve CQ.
            // For reference see the code in lowerxarch.cpp.
            // TODO-ARM-CQ: cpblk loop unrolling is currently not implemented.

            if ((size != 0) && (size <= INITBLK_UNROLL_LIMIT))
            {
                blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindUnroll;
            }
            else
#endif // _TARGET_ARM64_
            {
                // In case we have a constant integer this means we went beyond
                // CPBLK_UNROLL_LIMIT bytes of size, still we should never have the case of
                // any GC-Pointers in the src struct.
                blkNode->gtBlkOpKind = GenTreeBlk::BlkOpKindHelper;
            }
        }
    }
}

//------------------------------------------------------------------------
// LowerCast: Lower GT_CAST(srcType, DstType) nodes.
//
// Arguments:
//    tree - GT_CAST node to be lowered
//
// Return Value:
//    None.
//
// Notes:
//    Casts from small int type to float/double are transformed as follows:
//    GT_CAST(byte, float/double)     =   GT_CAST(GT_CAST(byte, int32), float/double)
//    GT_CAST(sbyte, float/double)    =   GT_CAST(GT_CAST(sbyte, int32), float/double)
//    GT_CAST(int16, float/double)    =   GT_CAST(GT_CAST(int16, int32), float/double)
//    GT_CAST(uint16, float/double)   =   GT_CAST(GT_CAST(uint16, int32), float/double)
//
//    Similarly casts from float/double to a smaller int type are transformed as follows:
//    GT_CAST(float/double, byte)     =   GT_CAST(GT_CAST(float/double, int32), byte)
//    GT_CAST(float/double, sbyte)    =   GT_CAST(GT_CAST(float/double, int32), sbyte)
//    GT_CAST(float/double, int16)    =   GT_CAST(GT_CAST(double/double, int32), int16)
//    GT_CAST(float/double, uint16)   =   GT_CAST(GT_CAST(double/double, int32), uint16)
//
//    Note that for the overflow conversions we still depend on helper calls and
//    don't expect to see them here.
//    i) GT_CAST(float/double, int type with overflow detection)
//
void Lowering::LowerCast(GenTree* tree)
{
    assert(tree->OperGet() == GT_CAST);

    JITDUMP("LowerCast for: ");
    DISPNODE(tree);
    JITDUMP("\n");

    GenTreePtr op1     = tree->gtOp.gtOp1;
    var_types  dstType = tree->CastToType();
    var_types  srcType = op1->TypeGet();
    var_types  tmpType = TYP_UNDEF;

    if (varTypeIsFloating(srcType))
    {
        noway_assert(!tree->gtOverflow());
    }

    // Case of src is a small type and dst is a floating point type.
    if (varTypeIsSmall(srcType) && varTypeIsFloating(dstType))
    {
        // These conversions can never be overflow detecting ones.
        noway_assert(!tree->gtOverflow());
        tmpType = TYP_INT;
    }
    // case of src is a floating point type and dst is a small type.
    else if (varTypeIsFloating(srcType) && varTypeIsSmall(dstType))
    {
        NYI_ARM("Lowering for cast from float to small type"); // Not tested yet.
        tmpType = TYP_INT;
    }

    if (tmpType != TYP_UNDEF)
    {
        GenTreePtr tmp = comp->gtNewCastNode(tmpType, op1, tmpType);
        tmp->gtFlags |= (tree->gtFlags & (GTF_UNSIGNED | GTF_OVERFLOW | GTF_EXCEPT));

        tree->gtFlags &= ~GTF_UNSIGNED;
        tree->gtOp.gtOp1 = tmp;
        BlockRange().InsertAfter(op1, tmp);
    }
}

//------------------------------------------------------------------------
// LowerRotate: Lower GT_ROL and GT_ROL nodes.
//
// Arguments:
//    tree - the node to lower
//
// Return Value:
//    None.
//
void Lowering::LowerRotate(GenTreePtr tree)
{
    if (tree->OperGet() == GT_ROL)
    {
        // There is no ROL instruction on ARM. Convert ROL into ROR.
        GenTreePtr rotatedValue        = tree->gtOp.gtOp1;
        unsigned   rotatedValueBitSize = genTypeSize(rotatedValue->gtType) * 8;
        GenTreePtr rotateLeftIndexNode = tree->gtOp.gtOp2;

        if (rotateLeftIndexNode->IsCnsIntOrI())
        {
            ssize_t rotateLeftIndex                 = rotateLeftIndexNode->gtIntCon.gtIconVal;
            ssize_t rotateRightIndex                = rotatedValueBitSize - rotateLeftIndex;
            rotateLeftIndexNode->gtIntCon.gtIconVal = rotateRightIndex;
        }
        else
        {
            GenTreePtr tmp =
                comp->gtNewOperNode(GT_NEG, genActualType(rotateLeftIndexNode->gtType), rotateLeftIndexNode);
            BlockRange().InsertAfter(rotateLeftIndexNode, tmp);
            tree->gtOp.gtOp2 = tmp;
        }
        tree->ChangeOper(GT_ROR);
    }
}

#endif // _TARGET_ARMARCH_

#endif // !LEGACY_BACKEND
<|endoftext|>


<commit_before>/*****************************************************************************
 * Licensed to Qualys, Inc. (QUALYS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * QUALYS licenses this file to You 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.
 ****************************************************************************/

/**
 * @file
 * @brief IronBee --- Output Intermediate Format as DOT.
 *
 * This utility streams protobuf.  There is a very similar routine,
 * intermediate_to_dot() which writes out an Automata.
 *
 * @author Christopher Alfeld <calfeld@qualys.com>
 */

#include <ironautomata/intermediate.hpp>

#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <fstream>

using namespace std;
using namespace IronAutomata::Intermediate;

/**
 * Convert a string to a vector of its bytes.
 *
 * @param[in]  bytes String of bytes.
 * @param[out] vec   Vector to write to.
 */
void bytes_to_vec(const std::string& bytes, vector<uint8_t>& vec)
{
    vec.clear();
    vec.reserve(bytes.size());
    vec.insert(vec.begin(), bytes.begin(), bytes.end());
}

/**
 * Limited conversion of a PB::Edge to edge_t.
 *
 * The conversion is minimal and should only be used to follow up with
 * edge value iterators.
 *
 * @param[in]  pb_edge Protobuf Edge.
 * @param[out] edge    Intermediate edge.
 */
void pb_edge_to_edge(const PB::Edge& pb_edge, Edge& edge)
{
    edge.clear();

    if (pb_edge.has_values_bm() && pb_edge.has_values()) {
        throw runtime_error(
            "Edge in chunk with both values and values bitmap."
        );
    }

    if (pb_edge.has_values_bm()) {
        bytes_to_vec(pb_edge.values_bm(), edge.bitmap());
    }
    else if (pb_edge.has_values()) {
        bytes_to_vec(pb_edge.values(), edge.vector());
    }
    else {
        throw runtime_error(
            "Edge in chunk with neither values or values_bm."
        );
    }
}

/**
 * Write the content of a string with escaping.
 *
 * Any non-printable character will be printed as decimal values in angle
 * brackets.
 *
 * @param[in] o Ostream to write to.
 * @param[in] v Vector to write.
 */
void output_content(ostream& o, const string& v)
{
    BOOST_FOREACH(char c, v) {
        switch (c) {
        case '&': cout << "&"; break;
        case '"': cout << """; break;
        case '\'': cout << "'"; break;
        case '<': cout << "<"; break;
        case '>': cout << ">"; break;
        default:
            if (c < 32 || c > 126) {
                cout << boost::format("⟨%d⟩") % uint32_t(c);
            }
            else {
                cout << boost::format("%c") % c;
            }
        }
    }
}

//! Main.
int main(int argc, char** argv)
{
    if (argc != 2) {
        cerr << "Usage: " << argv[0] << " <automata>" << endl;
        return 1;
    }

    ifstream input(argv[1]);
    if (! input) {
        cerr << "Error opening " << argv[1] << " for reading." << endl;
        return 1;
    }

    cout << "digraph A {" << endl;
    PB::Chunk chunk;
    for (;;) {
        bool at_eof = false;
        try {
            at_eof = ! read_chunk(input, chunk);
        }
        catch (const exception& e) {
            cerr << e.what() << endl;
            return 1;
        }

        if (at_eof) {
            break;
        }

        bool first_node = true;
        Edge edge;
        BOOST_FOREACH(const PB::Node& node, chunk.nodes()) {
            cout << "  " << node.id() << " [label=\"" << node.id() << "\"";

            if (first_node) {
                first_node = false;
                cout << ", shape=diamond";
            }
            cout << "];" << endl;

            BOOST_FOREACH(const PB::Edge& pb_edge, node.edges()) {
                try {
                    pb_edge_to_edge(pb_edge, edge);
                }
                catch (const exception& e) {
                    cerr << e.what() << endl;
                    return 1;
                }
                cout << "  " << node.id() << " -> " << pb_edge.target()
                     << " [weight=1000, label=\"";
                if (edge.epsilon()) {
                    cout << "ε";
                }
                else {
                    BOOST_FOREACH(uint8_t c, edge) {
                        output_content(cout, (boost::format("%c") % c).str());
                    }
                }
                if (pb_edge.has_advance() && ! pb_edge.advance()) {
                    cout << "\", color=red";
                }
                cout << "\"];" << endl;
            }

            if (node.has_default_target()) {
                cout << "  " << node.id() << " -> " << node.default_target()
                     << " [style=dashed, label=\"default\"";
                if (
                    node.has_advance_on_default() &&
                    ! node.advance_on_default()
                ) {
                    cout << ", color=red";
                }
                cout << "];" << endl;
            }

            if (node.has_first_output()) {
                cout << "  " << node.id() << " -> output"
                     << node.first_output() << " [style=dotted];" << endl;
            }
        }

        BOOST_FOREACH(const PB::Output& output, chunk.outputs()) {
            cout << "  output" << output.id() << " [shape=box, label=\"";
            output_content(cout, output.content());
            cout << "\"];" << endl;
            if (output.has_next()) {
                cout << "  output" << output.id() << " -> output"
                     << output.next() << " [style=dotted];" << endl;
            }
        }
    }
    cout << "}" << endl;
}
<commit_msg>automata/to_dot: Fix bug where too many nodes were diamonds.<commit_after>/*****************************************************************************
 * Licensed to Qualys, Inc. (QUALYS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * QUALYS licenses this file to You 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.
 ****************************************************************************/

/**
 * @file
 * @brief IronBee --- Output Intermediate Format as DOT.
 *
 * This utility streams protobuf.  There is a very similar routine,
 * intermediate_to_dot() which writes out an Automata.
 *
 * @author Christopher Alfeld <calfeld@qualys.com>
 */

#include <ironautomata/intermediate.hpp>

#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <fstream>

using namespace std;
using namespace IronAutomata::Intermediate;

/**
 * Convert a string to a vector of its bytes.
 *
 * @param[in]  bytes String of bytes.
 * @param[out] vec   Vector to write to.
 */
void bytes_to_vec(const std::string& bytes, vector<uint8_t>& vec)
{
    vec.clear();
    vec.reserve(bytes.size());
    vec.insert(vec.begin(), bytes.begin(), bytes.end());
}

/**
 * Limited conversion of a PB::Edge to edge_t.
 *
 * The conversion is minimal and should only be used to follow up with
 * edge value iterators.
 *
 * @param[in]  pb_edge Protobuf Edge.
 * @param[out] edge    Intermediate edge.
 */
void pb_edge_to_edge(const PB::Edge& pb_edge, Edge& edge)
{
    edge.clear();

    if (pb_edge.has_values_bm() && pb_edge.has_values()) {
        throw runtime_error(
            "Edge in chunk with both values and values bitmap."
        );
    }

    if (pb_edge.has_values_bm()) {
        bytes_to_vec(pb_edge.values_bm(), edge.bitmap());
    }
    else if (pb_edge.has_values()) {
        bytes_to_vec(pb_edge.values(), edge.vector());
    }
    else {
        throw runtime_error(
            "Edge in chunk with neither values or values_bm."
        );
    }
}

/**
 * Write the content of a string with escaping.
 *
 * Any non-printable character will be printed as decimal values in angle
 * brackets.
 *
 * @param[in] o Ostream to write to.
 * @param[in] v Vector to write.
 */
void output_content(ostream& o, const string& v)
{
    BOOST_FOREACH(char c, v) {
        switch (c) {
        case '&': cout << "&"; break;
        case '"': cout << """; break;
        case '\'': cout << "'"; break;
        case '<': cout << "<"; break;
        case '>': cout << ">"; break;
        default:
            if (c < 32 || c > 126) {
                cout << boost::format("⟨%d⟩") % uint32_t(c);
            }
            else {
                cout << boost::format("%c") % c;
            }
        }
    }
}

//! Main.
int main(int argc, char** argv)
{
    if (argc != 2) {
        cerr << "Usage: " << argv[0] << " <automata>" << endl;
        return 1;
    }

    ifstream input(argv[1]);
    if (! input) {
        cerr << "Error opening " << argv[1] << " for reading." << endl;
        return 1;
    }

    cout << "digraph A {" << endl;
    PB::Chunk chunk;
    bool first_node = true;
    for (;;) {
        bool at_eof = false;
        try {
            at_eof = ! read_chunk(input, chunk);
        }
        catch (const exception& e) {
            cerr << e.what() << endl;
            return 1;
        }

        if (at_eof) {
            break;
        }

        Edge edge;
        BOOST_FOREACH(const PB::Node& node, chunk.nodes()) {
            cout << "  " << node.id() << " [label=\"" << node.id() << "\"";

            if (first_node) {
                first_node = false;
                cout << ", shape=diamond";
            }
            cout << "];" << endl;

            BOOST_FOREACH(const PB::Edge& pb_edge, node.edges()) {
                try {
                    pb_edge_to_edge(pb_edge, edge);
                }
                catch (const exception& e) {
                    cerr << e.what() << endl;
                    return 1;
                }
                cout << "  " << node.id() << " -> " << pb_edge.target()
                     << " [weight=1000, label=\"";
                if (edge.epsilon()) {
                    cout << "ε";
                }
                else {
                    BOOST_FOREACH(uint8_t c, edge) {
                        output_content(cout, (boost::format("%c") % c).str());
                    }
                }
                if (pb_edge.has_advance() && ! pb_edge.advance()) {
                    cout << "\", color=red";
                }
                cout << "\"];" << endl;
            }

            if (node.has_default_target()) {
                cout << "  " << node.id() << " -> " << node.default_target()
                     << " [style=dashed, label=\"default\"";
                if (
                    node.has_advance_on_default() &&
                    ! node.advance_on_default()
                ) {
                    cout << ", color=red";
                }
                cout << "];" << endl;
            }

            if (node.has_first_output()) {
                cout << "  " << node.id() << " -> output"
                     << node.first_output() << " [style=dotted];" << endl;
            }
        }

        BOOST_FOREACH(const PB::Output& output, chunk.outputs()) {
            cout << "  output" << output.id() << " [shape=box, label=\"";
            output_content(cout, output.content());
            cout << "\"];" << endl;
            if (output.has_next()) {
                cout << "  output" << output.id() << " -> output"
                     << output.next() << " [style=dotted];" << endl;
            }
        }
    }
    cout << "}" << endl;
}
<|endoftext|>


<commit_before>// Copyright (c) 2011, Ben Scott.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//   * Redistributions of source code must retain the above copyright
//     notice, this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <unittest++/UnitTest++.h>
#include <base/base.h>

SUITE(base) {
  //----------------------------------------------------------------------------
  TEST(endian_swap_u16) {
    CHECK_EQUAL((uint16_t)0x0201, bl_endian_swap((uint16_t)0x0102));

    uint16_t val = 0x3456;
    bl_endian_swap(&val);
    CHECK_EQUAL((uint16_t)0x5634, val);

    uint16_t vals[] = { 0x1234, 0x6789 };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL((uint16_t)0x3412, vals[0]);
    CHECK_EQUAL((uint16_t)0x8967, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_u32) {
    CHECK_EQUAL(0x04030201U, bl_endian_swap(0x01020304U));

    uint32_t val = 0x87654321U;
    bl_endian_swap(&val);
    CHECK_EQUAL(0x21436587U, val);

    uint32_t vals[] = { 0x12345678U, 0xabcd4321U };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(0x78563412U, vals[0]);
    CHECK_EQUAL(0x2143cdabU, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_u64) {
    CHECK_EQUAL(0x0807060504030201ULL, bl_endian_swap(0x0102030405060708ULL));

    uint64_t val = 0x0123456789abcdefULL;
    bl_endian_swap(&val);
    CHECK_EQUAL(0xefcdab8967452301, val);

    uint64_t vals[] = { 0x0102030405060708ULL, 0x8091a2b3c4d5e6f7 };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(0x0807060504030201ULL, vals[0]);
    CHECK_EQUAL(0xf7e6d5c4b3a29180ULL, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_float) {
    union F2I {
      float     f;
      uint32_t  i;
    };
    F2I val, expected;
    val.f = 1.0f;
    expected.i = 0x0000803f;
    val.f = bl_endian_swap(val.f);

    CHECK_EQUAL(expected.i, val.i);
    CHECK_EQUAL(1.0f, bl_endian_swap(bl_endian_swap(1.0f)));

    val.f = 1.0f;
    bl_endian_swap(&val.f);
    CHECK_EQUAL(expected.i, val.i);
    bl_endian_swap(&val.f);
    CHECK_EQUAL(1.0f, val.f);

    float vals[] = { 1.0f, 1.0f };
    F2I conv;
    bl_endian_swap(vals, 2);
    conv.f = vals[0];
    CHECK_EQUAL(expected.i, conv.i);
    conv.f = vals[1];
    CHECK_EQUAL(expected.i, conv.i);
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(1.0f, vals[0]);
    CHECK_EQUAL(1.0f, vals[1]);
  }
}
<commit_msg>Clean whitespace. Curse you xcode.<commit_after>// Copyright (c) 2011, Ben Scott.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//   * Redistributions of source code must retain the above copyright
//     notice, this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above copyright
//     notice, this list of conditions and the following disclaimer in the
//     documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <unittest++/UnitTest++.h>
#include <base/base.h>

SUITE(base) {
  //----------------------------------------------------------------------------
  TEST(endian_swap_u16) {
    CHECK_EQUAL((uint16_t)0x0201, bl_endian_swap((uint16_t)0x0102));

    uint16_t val = 0x3456;
    bl_endian_swap(&val);
    CHECK_EQUAL((uint16_t)0x5634, val);

    uint16_t vals[] = { 0x1234, 0x6789 };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL((uint16_t)0x3412, vals[0]);
    CHECK_EQUAL((uint16_t)0x8967, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_u32) {
    CHECK_EQUAL(0x04030201U, bl_endian_swap(0x01020304U));

    uint32_t val = 0x87654321U;
    bl_endian_swap(&val);
    CHECK_EQUAL(0x21436587U, val);

    uint32_t vals[] = { 0x12345678U, 0xabcd4321U };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(0x78563412U, vals[0]);
    CHECK_EQUAL(0x2143cdabU, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_u64) {
    CHECK_EQUAL(0x0807060504030201ULL, bl_endian_swap(0x0102030405060708ULL));

    uint64_t val = 0x0123456789abcdefULL;
    bl_endian_swap(&val);
    CHECK_EQUAL(0xefcdab8967452301, val);

    uint64_t vals[] = { 0x0102030405060708ULL, 0x8091a2b3c4d5e6f7 };
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(0x0807060504030201ULL, vals[0]);
    CHECK_EQUAL(0xf7e6d5c4b3a29180ULL, vals[1]);
  }

  //----------------------------------------------------------------------------
  TEST(endian_swap_float) {
    union F2I {
      float     f;
      uint32_t  i;
    };
    F2I val, expected;
    val.f = 1.0f;
    expected.i = 0x0000803f;
    val.f = bl_endian_swap(val.f);

    CHECK_EQUAL(expected.i, val.i);
    CHECK_EQUAL(1.0f, bl_endian_swap(bl_endian_swap(1.0f)));

    val.f = 1.0f;
    bl_endian_swap(&val.f);
    CHECK_EQUAL(expected.i, val.i);
    bl_endian_swap(&val.f);
    CHECK_EQUAL(1.0f, val.f);

    float vals[] = { 1.0f, 1.0f };
    F2I conv;
    bl_endian_swap(vals, 2);
    conv.f = vals[0];
    CHECK_EQUAL(expected.i, conv.i);
    conv.f = vals[1];
    CHECK_EQUAL(expected.i, conv.i);
    bl_endian_swap(vals, 2);
    CHECK_EQUAL(1.0f, vals[0]);
    CHECK_EQUAL(1.0f, vals[1]);
  }
}
<|endoftext|>


<commit_before>/*****************************************************************************
 * PokerTH - The open source texas holdem engine                             *
 * Copyright (C) 2006-2011 Felix Hammer, Florian Thauer, Lothar May          *
 *                                                                           *
 * This program is free software: you can redistribute it and/or modify      *
 * it under the terms of the GNU Affero General Public License as            *
 * published by the Free Software Foundation, either version 3 of the        *
 * License, or (at your option) any later version.                           *
 *                                                                           *
 * This program 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 Affero General Public License for more details.                       *
 *                                                                           *
 * You should have received a copy of the GNU Affero General Public License  *
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.     *
 *****************************************************************************/

#include <core/crypthelper.h>

#include <core/openssl_wrapper.h>
#include <cstring>
#include <cstdio>

using namespace std;

// Helper function.
static int
fromHex(int ch)
{
	int retVal = -1;

	if (ch >= '0' && ch <= '9')
		retVal = ch - '0';
	else if (ch >= 'a' && ch <= 'f')
		retVal = ch - 'a' + 10;
	else if (ch >= 'A' && ch <= 'F')
		retVal = ch - 'A' + 10;

	return retVal;
}

HashBuf::~HashBuf()
{
}

std::string
HashBuf::ToString() const
{
	// Create a hex-based string from the MD5 data.
	string retValue;
	char tmpBuf[2 + 1];
	tmpBuf[sizeof(tmpBuf) - 1] = 0;
	const unsigned char *tmpData = GetData();
	for (int i = 0; i < GetDataSize(); i++) {
		sprintf(tmpBuf, "%02x", tmpData[i]);
		retValue += tmpBuf;
	}
	return retValue;
}

bool
HashBuf::FromString(const std::string &text)
{
	// Convert hex-based string to MD5 data.
	bool retVal = false;
	int tmpSize = GetDataSize();
	if (text.size() == 2 * (unsigned)tmpSize) {
		unsigned char *tmpData = GetData();
		const char *t = text.c_str();
		int i = 0;
		for (; i < tmpSize; i++) {
			int part1 = fromHex(*t++);
			if (part1 == -1)
				break;
			int part2 = fromHex(*t++);
			if (part2 == -1)
				break;
			*tmpData++ = (part1<<4) + part2;
		}
		retVal = i == tmpSize;
	}
	return retVal;
}

bool
HashBuf::IsZero() const
{
	int dataSize = GetDataSize();
	const unsigned char *tmpData = GetData();
	int i;
	for (i = 0; i < dataSize; i++) {
		if (tmpData[i] != 0)
			break;
	}
	return i == dataSize;
}

bool
HashBuf::operator==(const HashBuf &other) const
{
	return GetDataSize() == other.GetDataSize() && memcmp(GetData(), other.GetData(), GetDataSize()) == 0;
}

bool
HashBuf::operator<(const HashBuf &other) const
{
	int smallestDataSize = GetDataSize() < other.GetDataSize() ? GetDataSize() : other.GetDataSize();
	return memcmp(GetData(), other.GetData(), smallestDataSize) < 0;
}

MD5Buf::MD5Buf()
{
	memset(m_data, 0, sizeof(m_data));
}

unsigned char *
MD5Buf::GetData()
{
	return m_data;
}

const unsigned char *
MD5Buf::GetData() const
{
	return m_data;
}

int
MD5Buf::GetDataSize() const
{
	return sizeof(m_data);
}

SHA1Buf::SHA1Buf()
{
	memset(m_data, 0, sizeof(m_data));
}

unsigned char *
SHA1Buf::GetData()
{
	return m_data;
}

const unsigned char *
SHA1Buf::GetData() const
{
	return m_data;
}

int
SHA1Buf::GetDataSize() const
{
	return sizeof(m_data);
}

bool
CryptHelper::MD5Sum(const std::string &fileName, MD5Buf &buf)
{
	bool retVal = false;
	FILE *file = fopen(fileName.c_str(), "rb");

	if (file) {
		// Calculate MD5 sum of file.
		unsigned char *readBuf = new unsigned char[8192];
		size_t numBytes;

#ifdef HAVE_OPENSSL
		MD5_CTX context;
		MD5_Init(&context);
		while ((numBytes = fread(readBuf, 1, sizeof(readBuf), file)) > 0) {
			MD5_Update(&context, readBuf, numBytes);
		}
		MD5_Final(buf.GetData(), &context);
#else
		gcry_md_hd_t hash;
		gcry_md_open(&hash, GCRY_MD_MD5, 0);
		while ((numBytes = fread(readBuf, 1, sizeof(readBuf), file)) > 0) {
			gcry_md_write(hash, readBuf, numBytes);
		}
		memcpy(buf.GetData(), gcry_md_read(hash, GCRY_MD_MD5), MD5_DATA_SIZE);
		gcry_md_close(hash);
#endif

		retVal = ferror(file) == 0;

		delete[] readBuf;
		fclose(file);
	}
	return retVal;
}

bool
CryptHelper::SHA1Hash(const unsigned char *data, unsigned dataSize, SHA1Buf &buf)
{
	bool retVal;
#ifdef HAVE_OPENSSL
	retVal = SHA1(data, dataSize, buf.GetData()) != NULL;
#else
	// We use the shortcut since we assume that the system supports SHA1.
	// This call has no error return value.
	gcry_md_hash_buffer(GCRY_MD_SHA1, buf.GetData(), data, dataSize);
	retVal = true;
#endif
	return retVal;
}

bool
CryptHelper::HMACSha1(const unsigned char *keyData, unsigned keySize, const unsigned char *plainData, unsigned plainSize, SHA1Buf &buf)
{
	bool retVal;
#ifdef HAVE_OPENSSL
	unsigned hashLen = 0;
	HMAC(EVP_sha1(), keyData, keySize, plainData, plainSize, buf.GetData(), &hashLen);
	retVal = hashLen == (unsigned)buf.GetDataSize();
#else
	retVal = false;
	gcry_md_hd_t hd;
	gcry_error_t err = gcry_md_open(&hd, GCRY_MD_SHA1, GCRY_MD_FLAG_HMAC);
	if (!err) {
		err = gcry_md_setkey(hd, keyData, keySize);
		if (!err) {
			gcry_md_write(hd, plainData, plainSize);
			unsigned char *hash = gcry_md_read(hd, 0);
			if (hash) {
				memcpy(buf.GetData(), hash, buf.GetDataSize());
				retVal = true;
			}
		}
		gcry_md_close(hd);
	}
#endif
	return retVal;
}

void
CryptHelper::BytesToKey(const unsigned char *keyData, unsigned keySize, unsigned char *key, unsigned char *iv)
{
	// The key/iv derivation is kind of like EVP_BytesToKey of OpenSSL with count 2 and no salt.
	// EVP_BytesToKey is not used because there is nothing like it in GnuTLS or gcrypt.
	SHA1Buf tmpBuf1, tmpBuf2, keyBuf1, keyBuf2;
	// First 20 bytes
	CryptHelper::SHA1Hash(keyData, keySize, tmpBuf1);
	CryptHelper::SHA1Hash(tmpBuf1.GetData(), tmpBuf1.GetDataSize(), keyBuf1);
	// Second 20 bytes (we only need a total of 32 bytes, but anyway).
	unsigned tmpKeySize = keySize + keyBuf1.GetDataSize();
	unsigned char *tmpKeyData = new unsigned char[tmpKeySize];
	// Concatenate our first hash and the key data.
	memcpy(tmpKeyData, keyBuf1.GetData(), keyBuf1.GetDataSize());
	memcpy(tmpKeyData + keyBuf1.GetDataSize(), keyData, keySize);
	CryptHelper::SHA1Hash(tmpKeyData, tmpKeySize, tmpBuf2);
	CryptHelper::SHA1Hash(tmpBuf2.GetData(), tmpBuf2.GetDataSize(), keyBuf2);
	delete[] tmpKeyData;
	// Copy the hashes to key/iv.
	memcpy(key, keyBuf1.GetData(), AES_BLOCK_SIZE);
	unsigned tmpivBytes = keyBuf1.GetDataSize() - AES_BLOCK_SIZE;
	memcpy(iv, keyBuf1.GetData() + AES_BLOCK_SIZE, tmpivBytes);
	memcpy(iv + tmpivBytes, keyBuf2.GetData(), AES_BLOCK_SIZE - tmpivBytes);
}

bool
CryptHelper::AES128Encrypt(const unsigned char *keyData, unsigned keySize, const string &plainStr, std::vector<unsigned char> &outCipher)
{
	bool retVal = false;
	unsigned plainSize = static_cast<unsigned>(plainStr.size());
	if (keySize && plainSize) {
		unsigned char key[AES_BLOCK_SIZE];
		unsigned char iv[AES_BLOCK_SIZE];
		BytesToKey(keyData, keySize, key, iv);
		// Add padding to plain data.
		unsigned paddedPlainSize = ADD_PADDING(plainSize);
		unsigned char *paddedPlainStr = new unsigned char[ADD_PADDING(plainSize)];
		memset(paddedPlainStr, 0, paddedPlainSize);
		memcpy(paddedPlainStr, plainStr.c_str(), plainSize);
		// Perform the encryption.
		int cipherSize = paddedPlainSize;
		outCipher.resize(cipherSize);

#ifdef HAVE_OPENSSL
		EVP_CIPHER_CTX encryptCtx;
		EVP_CIPHER_CTX_init(&encryptCtx);
		int outCipherSize = cipherSize;

		int success = EVP_EncryptInit(&encryptCtx, EVP_aes_128_cbc(), key, iv);
		EVP_CIPHER_CTX_set_padding(&encryptCtx, 0);
		if (success) {
			success = EVP_EncryptUpdate(&encryptCtx, &outCipher[0], &outCipherSize, paddedPlainStr, paddedPlainSize);

			if (success && outCipherSize) {
				// Since padding is off, this will not modify the cipher. However, parameters need to be set.
				EVP_EncryptFinal(&encryptCtx, &outCipher[0], &outCipherSize);
				retVal = true;
			}
		} else
			outCipher.clear();
#else
		gcry_cipher_hd_t hd;
		gcry_error_t err = gcry_cipher_open(&hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_CBC, 0);
		if (!err) {
			gcry_cipher_setkey(hd, key, sizeof(key));
			gcry_cipher_setiv(hd, iv, sizeof(iv));
			err = gcry_cipher_encrypt(hd, &outCipher[0], cipherSize, paddedPlainStr, paddedPlainSize);
			if (!err)
				retVal = true;
			else
				outCipher.clear();
		} else
			outCipher.clear();

		gcry_cipher_close(hd);
#endif
		delete[] paddedPlainStr;
	}
	return retVal;
}

bool
CryptHelper::AES128Decrypt(const unsigned char *keyData, unsigned keySize, const unsigned char *cipher, unsigned cipherSize, string &outPlain)
{
	bool retVal = false;
	if (keySize && cipherSize) {
		unsigned char key[AES_BLOCK_SIZE];
		unsigned char iv[AES_BLOCK_SIZE];
		BytesToKey(keyData, keySize, key, iv);
		outPlain.resize(cipherSize);
#ifdef HAVE_OPENSSL
		EVP_CIPHER_CTX decryptCtx;
		EVP_CIPHER_CTX_init(&decryptCtx);
		int outPlainSize = cipherSize;

		int success = EVP_DecryptInit(&decryptCtx, EVP_aes_128_cbc(), key, iv);
		EVP_CIPHER_CTX_set_padding(&decryptCtx, 0);
		if (success) {
			success = EVP_DecryptUpdate(&decryptCtx, (unsigned char *)&outPlain[0], &outPlainSize, cipher, cipherSize);

			if (success && outPlainSize) {
				// Since padding is off, this will not modify the plain text. However, parameters need to be set.
				EVP_DecryptFinal(&decryptCtx, (unsigned char *)outPlain.c_str(), &outPlainSize);
				retVal = true;
			}
		} else
			outPlain.clear();
#else
		gcry_cipher_hd_t hd;
		gcry_error_t err = gcry_cipher_open(&hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_CBC, 0);
		if (!err) {
			gcry_cipher_setkey(hd, key, sizeof(key));
			gcry_cipher_setiv(hd, iv, sizeof(iv));
			err = gcry_cipher_decrypt(hd, &outPlain[0], outPlain.size(), cipher, cipherSize);
			if (!err)
				retVal = true;
			else
				outPlain.clear();
		} else
			outPlain.clear();

		gcry_cipher_close(hd);
#endif
		// Remove trailing zeroes (padding).
		if (!outPlain.empty()) {
			size_t pos = outPlain.find_first_of('\0');
			if (pos != string::npos)
				outPlain = outPlain.substr(0, pos);
		}
	}
	return retVal;
}

<commit_msg>Fixing cppcheck issue.<commit_after>/*****************************************************************************
 * PokerTH - The open source texas holdem engine                             *
 * Copyright (C) 2006-2011 Felix Hammer, Florian Thauer, Lothar May          *
 *                                                                           *
 * This program is free software: you can redistribute it and/or modify      *
 * it under the terms of the GNU Affero General Public License as            *
 * published by the Free Software Foundation, either version 3 of the        *
 * License, or (at your option) any later version.                           *
 *                                                                           *
 * This program 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 Affero General Public License for more details.                       *
 *                                                                           *
 * You should have received a copy of the GNU Affero General Public License  *
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.     *
 *****************************************************************************/

#include <core/crypthelper.h>

#include <core/openssl_wrapper.h>
#include <cstring>
#include <cstdio>

using namespace std;

// Helper function.
static int
fromHex(int ch)
{
	int retVal = -1;

	if (ch >= '0' && ch <= '9')
		retVal = ch - '0';
	else if (ch >= 'a' && ch <= 'f')
		retVal = ch - 'a' + 10;
	else if (ch >= 'A' && ch <= 'F')
		retVal = ch - 'A' + 10;

	return retVal;
}

HashBuf::~HashBuf()
{
}

std::string
HashBuf::ToString() const
{
	// Create a hex-based string from the MD5 data.
	string retValue;
	char tmpBuf[2 + 1];
	tmpBuf[sizeof(tmpBuf) - 1] = 0;
	const unsigned char *tmpData = GetData();
	for (int i = 0; i < GetDataSize(); i++) {
		sprintf(tmpBuf, "%02x", tmpData[i]);
		retValue += tmpBuf;
	}
	return retValue;
}

bool
HashBuf::FromString(const std::string &text)
{
	// Convert hex-based string to MD5 data.
	bool retVal = false;
	int tmpSize = GetDataSize();
	if (text.size() == 2 * (unsigned)tmpSize) {
		unsigned char *tmpData = GetData();
		const char *t = text.c_str();
		int i = 0;
		for (; i < tmpSize; i++) {
			int part1 = fromHex(*t++);
			if (part1 == -1)
				break;
			int part2 = fromHex(*t++);
			if (part2 == -1)
				break;
			*tmpData++ = (part1<<4) + part2;
		}
		retVal = i == tmpSize;
	}
	return retVal;
}

bool
HashBuf::IsZero() const
{
	int dataSize = GetDataSize();
	const unsigned char *tmpData = GetData();
	int i;
	for (i = 0; i < dataSize; i++) {
		if (tmpData[i] != 0)
			break;
	}
	return i == dataSize;
}

bool
HashBuf::operator==(const HashBuf &other) const
{
	return GetDataSize() == other.GetDataSize() && memcmp(GetData(), other.GetData(), GetDataSize()) == 0;
}

bool
HashBuf::operator<(const HashBuf &other) const
{
	int smallestDataSize = GetDataSize() < other.GetDataSize() ? GetDataSize() : other.GetDataSize();
	return memcmp(GetData(), other.GetData(), smallestDataSize) < 0;
}

MD5Buf::MD5Buf()
{
	memset(m_data, 0, sizeof(m_data));
}

unsigned char *
MD5Buf::GetData()
{
	return m_data;
}

const unsigned char *
MD5Buf::GetData() const
{
	return m_data;
}

int
MD5Buf::GetDataSize() const
{
	return sizeof(m_data);
}

SHA1Buf::SHA1Buf()
{
	memset(m_data, 0, sizeof(m_data));
}

unsigned char *
SHA1Buf::GetData()
{
	return m_data;
}

const unsigned char *
SHA1Buf::GetData() const
{
	return m_data;
}

int
SHA1Buf::GetDataSize() const
{
	return sizeof(m_data);
}

bool
CryptHelper::MD5Sum(const std::string &fileName, MD5Buf &buf)
{
	bool retVal = false;
	FILE *file = fopen(fileName.c_str(), "rb");

	if (file) {
		// Calculate MD5 sum of file.
		unsigned char *readBuf = new unsigned char[8192];
		size_t numBytes;

#ifdef HAVE_OPENSSL
		MD5_CTX context;
		MD5_Init(&context);
		while ((numBytes = fread(readBuf, 1, sizeof(readBuf), file)) > 0) {
			MD5_Update(&context, readBuf, numBytes);
		}
		MD5_Final(buf.GetData(), &context);
#else
		gcry_md_hd_t hash;
		gcry_md_open(&hash, GCRY_MD_MD5, 0);
		while ((numBytes = fread(readBuf, 1, sizeof(readBuf), file)) > 0) {
			gcry_md_write(hash, readBuf, numBytes);
		}
		memcpy(buf.GetData(), gcry_md_read(hash, GCRY_MD_MD5), MD5_DATA_SIZE);
		gcry_md_close(hash);
#endif

		retVal = ferror(file) == 0;

		delete[] readBuf;
		fclose(file);
	}
	return retVal;
}

bool
CryptHelper::SHA1Hash(const unsigned char *data, unsigned dataSize, SHA1Buf &buf)
{
	bool retVal;
#ifdef HAVE_OPENSSL
	retVal = SHA1(data, dataSize, buf.GetData()) != NULL;
#else
	// We use the shortcut since we assume that the system supports SHA1.
	// This call has no error return value.
	gcry_md_hash_buffer(GCRY_MD_SHA1, buf.GetData(), data, dataSize);
	retVal = true;
#endif
	return retVal;
}

bool
CryptHelper::HMACSha1(const unsigned char *keyData, unsigned keySize, const unsigned char *plainData, unsigned plainSize, SHA1Buf &buf)
{
	bool retVal;
#ifdef HAVE_OPENSSL
	unsigned hashLen = 0;
	HMAC(EVP_sha1(), keyData, keySize, plainData, plainSize, buf.GetData(), &hashLen);
	retVal = hashLen == (unsigned)buf.GetDataSize();
#else
	retVal = false;
	gcry_md_hd_t hd;
	gcry_error_t err = gcry_md_open(&hd, GCRY_MD_SHA1, GCRY_MD_FLAG_HMAC);
	if (!err) {
		err = gcry_md_setkey(hd, keyData, keySize);
		if (!err) {
			gcry_md_write(hd, plainData, plainSize);
			unsigned char *hash = gcry_md_read(hd, 0);
			if (hash) {
				memcpy(buf.GetData(), hash, buf.GetDataSize());
				retVal = true;
			}
		}
		gcry_md_close(hd);
	}
#endif
	return retVal;
}

void
CryptHelper::BytesToKey(const unsigned char *keyData, unsigned keySize, unsigned char *key, unsigned char *iv)
{
	// The key/iv derivation is kind of like EVP_BytesToKey of OpenSSL with count 2 and no salt.
	// EVP_BytesToKey is not used because there is nothing like it in GnuTLS or gcrypt.
	SHA1Buf tmpBuf1, tmpBuf2, keyBuf1, keyBuf2;
	// First 20 bytes
	CryptHelper::SHA1Hash(keyData, keySize, tmpBuf1);
	CryptHelper::SHA1Hash(tmpBuf1.GetData(), tmpBuf1.GetDataSize(), keyBuf1);
	// Second 20 bytes (we only need a total of 32 bytes, but anyway).
	unsigned tmpKeySize = keySize + keyBuf1.GetDataSize();
	unsigned char *tmpKeyData = new unsigned char[tmpKeySize];
	// Concatenate our first hash and the key data.
	memcpy(tmpKeyData, keyBuf1.GetData(), keyBuf1.GetDataSize());
	memcpy(tmpKeyData + keyBuf1.GetDataSize(), keyData, keySize);
	CryptHelper::SHA1Hash(tmpKeyData, tmpKeySize, tmpBuf2);
	CryptHelper::SHA1Hash(tmpBuf2.GetData(), tmpBuf2.GetDataSize(), keyBuf2);
	delete[] tmpKeyData;
	// Copy the hashes to key/iv.
	memcpy(key, keyBuf1.GetData(), AES_BLOCK_SIZE);
	unsigned tmpivBytes = keyBuf1.GetDataSize() - AES_BLOCK_SIZE;
	memcpy(iv, keyBuf1.GetData() + AES_BLOCK_SIZE, tmpivBytes);
	memcpy(iv + tmpivBytes, keyBuf2.GetData(), AES_BLOCK_SIZE - tmpivBytes);
}

bool
CryptHelper::AES128Encrypt(const unsigned char *keyData, unsigned keySize, const string &plainStr, std::vector<unsigned char> &outCipher)
{
	bool retVal = false;
	unsigned plainSize = static_cast<unsigned>(plainStr.size());
	if (keySize && plainSize) {
		unsigned char key[AES_BLOCK_SIZE];
		unsigned char iv[AES_BLOCK_SIZE];
		BytesToKey(keyData, keySize, key, iv);
		// Add padding to plain data.
		unsigned paddedPlainSize = ADD_PADDING(plainSize);
		unsigned char *paddedPlainStr = (unsigned char *)calloc(paddedPlainSize, 1);
		memcpy(paddedPlainStr, plainStr.c_str(), plainSize);
		// Perform the encryption.
		int cipherSize = paddedPlainSize;
		outCipher.resize(cipherSize);

#ifdef HAVE_OPENSSL
		EVP_CIPHER_CTX encryptCtx;
		EVP_CIPHER_CTX_init(&encryptCtx);
		int outCipherSize = cipherSize;

		int success = EVP_EncryptInit(&encryptCtx, EVP_aes_128_cbc(), key, iv);
		EVP_CIPHER_CTX_set_padding(&encryptCtx, 0);
		if (success) {
			success = EVP_EncryptUpdate(&encryptCtx, &outCipher[0], &outCipherSize, paddedPlainStr, paddedPlainSize);

			if (success && outCipherSize) {
				// Since padding is off, this will not modify the cipher. However, parameters need to be set.
				EVP_EncryptFinal(&encryptCtx, &outCipher[0], &outCipherSize);
				retVal = true;
			}
		} else
			outCipher.clear();
#else
		gcry_cipher_hd_t hd;
		gcry_error_t err = gcry_cipher_open(&hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_CBC, 0);
		if (!err) {
			gcry_cipher_setkey(hd, key, sizeof(key));
			gcry_cipher_setiv(hd, iv, sizeof(iv));
			err = gcry_cipher_encrypt(hd, &outCipher[0], cipherSize, paddedPlainStr, paddedPlainSize);
			if (!err)
				retVal = true;
			else
				outCipher.clear();
		} else
			outCipher.clear();

		gcry_cipher_close(hd);
#endif
		free(paddedPlainStr);
	}
	return retVal;
}

bool
CryptHelper::AES128Decrypt(const unsigned char *keyData, unsigned keySize, const unsigned char *cipher, unsigned cipherSize, string &outPlain)
{
	bool retVal = false;
	if (keySize && cipherSize) {
		unsigned char key[AES_BLOCK_SIZE];
		unsigned char iv[AES_BLOCK_SIZE];
		BytesToKey(keyData, keySize, key, iv);
		outPlain.resize(cipherSize);
#ifdef HAVE_OPENSSL
		EVP_CIPHER_CTX decryptCtx;
		EVP_CIPHER_CTX_init(&decryptCtx);
		int outPlainSize = cipherSize;

		int success = EVP_DecryptInit(&decryptCtx, EVP_aes_128_cbc(), key, iv);
		EVP_CIPHER_CTX_set_padding(&decryptCtx, 0);
		if (success) {
			success = EVP_DecryptUpdate(&decryptCtx, (unsigned char *)&outPlain[0], &outPlainSize, cipher, cipherSize);

			if (success && outPlainSize) {
				// Since padding is off, this will not modify the plain text. However, parameters need to be set.
				EVP_DecryptFinal(&decryptCtx, (unsigned char *)outPlain.c_str(), &outPlainSize);
				retVal = true;
			}
		} else
			outPlain.clear();
#else
		gcry_cipher_hd_t hd;
		gcry_error_t err = gcry_cipher_open(&hd, GCRY_CIPHER_AES128, GCRY_CIPHER_MODE_CBC, 0);
		if (!err) {
			gcry_cipher_setkey(hd, key, sizeof(key));
			gcry_cipher_setiv(hd, iv, sizeof(iv));
			err = gcry_cipher_decrypt(hd, &outPlain[0], outPlain.size(), cipher, cipherSize);
			if (!err)
				retVal = true;
			else
				outPlain.clear();
		} else
			outPlain.clear();

		gcry_cipher_close(hd);
#endif
		// Remove trailing zeroes (padding).
		if (!outPlain.empty()) {
			size_t pos = outPlain.find_first_of('\0');
			if (pos != string::npos)
				outPlain = outPlain.substr(0, pos);
		}
	}
	return retVal;
}

<|endoftext|>


<commit_before>//: ----------------------------------------------------------------------------
//: Copyright (C) 2016 Verizon.  All Rights Reserved.
//: All Rights Reserved
//:
//: \file:    stat_h.cc
//: \details: TODO
//: \author:  Reed P. Morrison
//: \date:    01/19/2016
//:
//:   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.
//:
//: ----------------------------------------------------------------------------

//: ----------------------------------------------------------------------------
//: Includes
//: ----------------------------------------------------------------------------
#include "hlx/http_status.h"
#include "hlx/proxy_h.h"
#include "hlx/subr.h"
#include "hlx/rqst.h"
#include "hlx/resp.h"
#include "hlx/hlx.h"
#include "ndebug.h"
#include "hconn.h"
#include "nbq.h"

namespace ns_hlx {

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
proxy_h::proxy_h(const std::string &a_ups_host, const std::string &a_route):
        default_rqst_h(),
        m_ups_host(a_ups_host),
        m_route(a_route),
        // 10 min
        m_timeout_ms(600000)
{
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
proxy_h::~proxy_h(void)
{
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
h_resp_t proxy_h::do_default(hconn &a_hconn, rqst &a_rqst, const url_pmap_t &a_url_pmap)
{
        ns_hlx::subr &l_subr = ns_hlx::create_subr(a_hconn);
        // Remove route from url
        std::string l_route = a_rqst.get_url();
        std::string::size_type i_s = l_route.find(m_route);
        if (i_s != std::string::npos)
        {
                l_route.erase(i_s, m_route.length());
        }
        std::string l_url = m_ups_host + l_route;
        const char *l_body_data = a_rqst.get_body_data();
        uint64_t l_body_data_len = a_rqst.get_body_len();

        // subr setup
        l_subr.init_with_url(l_url);
        l_subr.set_completion_cb(s_completion_cb);
        l_subr.set_error_cb(s_error_cb);
        l_subr.set_data(this);
        l_subr.set_headers(a_rqst.get_headers());
        l_subr.set_keepalive(true);
        l_subr.set_timeout_ms(m_timeout_ms);
        l_subr.set_max_parallel(m_max_parallel);
        l_subr.set_verb(a_rqst.get_method_str());
        l_subr.set_body_data(l_body_data, l_body_data_len);
        l_subr.set_requester_hconn(&a_hconn);

        int32_t l_s;
        l_s = queue_subr(a_hconn, l_subr);
        if(l_s != HLX_STATUS_OK)
        {
                return ns_hlx::H_RESP_SERVER_ERROR;
        }
        return ns_hlx::H_RESP_DONE;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
int32_t proxy_h::s_completion_cb(subr &a_subr, nconn &a_nconn, resp &a_resp)
{
        // Create resp
        hconn *l_hconn = a_subr.get_requester_hconn();
        if(!l_hconn)
        {
                return STATUS_ERROR;
        }
        nbq *l_out_q = l_hconn->m_out_q;
        if(!l_out_q)
        {
                return STATUS_ERROR;
        }
        // TODO needed ???
        l_out_q->reset_write();
        nbq *l_in_q = a_resp.get_q();
        if(!l_in_q)
        {
                return STATUS_ERROR;
        }
        l_in_q->reset_read();
        int64_t l_size;
        l_size = l_out_q->write_q(*l_in_q);
        if(l_size < 0)
        {
                return STATUS_ERROR;
        }

        // access info
        l_hconn->m_access_info.m_resp_status = (http_status_t)a_resp.get_status();
        // TODO enum cast is bad -will fix

        int32_t l_s;
        l_s = queue_resp(*l_hconn);
        if(l_s != HLX_STATUS_OK)
        {
                return H_RESP_SERVER_ERROR;
        }
        return STATUS_OK;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
int32_t proxy_h::s_error_cb(subr &a_subr, nconn &a_nconn)
{
        proxy_h *l_proxy_h = (proxy_h *)a_subr.get_data();
        if(!l_proxy_h)
        {
                return STATUS_ERROR;
        }
        if(!a_subr.get_requester_hconn())
        {
                return STATUS_ERROR;
        }
        l_proxy_h->send_json_resp_err(*a_subr.get_requester_hconn(),
                                      false,
                                      // TODO use supports keep-alives from client request
                                      a_subr.get_fallback_status_code());
        return STATUS_OK;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
bool proxy_h::get_do_default(void)
{
        return true;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
void proxy_h::set_timeout_ms(uint32_t a_val)
{
        m_timeout_ms = a_val;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
void proxy_h::set_max_parallel(uint32_t a_val)
{
        m_max_parallel = a_val;
}

} //namespace ns_hlx {
<commit_msg>Adding max parallel upstream connection setting to proxy handler.<commit_after>//: ----------------------------------------------------------------------------
//: Copyright (C) 2016 Verizon.  All Rights Reserved.
//: All Rights Reserved
//:
//: \file:    stat_h.cc
//: \details: TODO
//: \author:  Reed P. Morrison
//: \date:    01/19/2016
//:
//:   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.
//:
//: ----------------------------------------------------------------------------

//: ----------------------------------------------------------------------------
//: Includes
//: ----------------------------------------------------------------------------
#include "hlx/http_status.h"
#include "hlx/proxy_h.h"
#include "hlx/subr.h"
#include "hlx/rqst.h"
#include "hlx/resp.h"
#include "hlx/hlx.h"
#include "ndebug.h"
#include "hconn.h"
#include "nbq.h"

namespace ns_hlx {

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
proxy_h::proxy_h(const std::string &a_ups_host, const std::string &a_route):
        default_rqst_h(),
        m_ups_host(a_ups_host),
        m_route(a_route),
        // 10 min
        m_timeout_ms(600000),
        m_max_parallel(-1)
{
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
proxy_h::~proxy_h(void)
{
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
h_resp_t proxy_h::do_default(hconn &a_hconn, rqst &a_rqst, const url_pmap_t &a_url_pmap)
{
        ns_hlx::subr &l_subr = ns_hlx::create_subr(a_hconn);
        // Remove route from url
        std::string l_route = a_rqst.get_url();
        std::string::size_type i_s = l_route.find(m_route);
        if (i_s != std::string::npos)
        {
                l_route.erase(i_s, m_route.length());
        }
        std::string l_url = m_ups_host + l_route;
        const char *l_body_data = a_rqst.get_body_data();
        uint64_t l_body_data_len = a_rqst.get_body_len();

        // subr setup
        l_subr.init_with_url(l_url);
        l_subr.set_completion_cb(s_completion_cb);
        l_subr.set_error_cb(s_error_cb);
        l_subr.set_data(this);
        l_subr.set_headers(a_rqst.get_headers());
        l_subr.set_keepalive(true);
        l_subr.set_timeout_ms(m_timeout_ms);
        l_subr.set_max_parallel(m_max_parallel);
        l_subr.set_verb(a_rqst.get_method_str());
        l_subr.set_body_data(l_body_data, l_body_data_len);
        l_subr.set_requester_hconn(&a_hconn);

        int32_t l_s;
        l_s = queue_subr(a_hconn, l_subr);
        if(l_s != HLX_STATUS_OK)
        {
                return ns_hlx::H_RESP_SERVER_ERROR;
        }
        return ns_hlx::H_RESP_DONE;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
int32_t proxy_h::s_completion_cb(subr &a_subr, nconn &a_nconn, resp &a_resp)
{
        // Create resp
        hconn *l_hconn = a_subr.get_requester_hconn();
        if(!l_hconn)
        {
                return STATUS_ERROR;
        }
        nbq *l_out_q = l_hconn->m_out_q;
        if(!l_out_q)
        {
                return STATUS_ERROR;
        }
        // TODO needed ???
        l_out_q->reset_write();
        nbq *l_in_q = a_resp.get_q();
        if(!l_in_q)
        {
                return STATUS_ERROR;
        }
        l_in_q->reset_read();
        int64_t l_size;
        l_size = l_out_q->write_q(*l_in_q);
        if(l_size < 0)
        {
                return STATUS_ERROR;
        }

        // access info
        l_hconn->m_access_info.m_resp_status = (http_status_t)a_resp.get_status();
        // TODO enum cast is bad -will fix

        int32_t l_s;
        l_s = queue_resp(*l_hconn);
        if(l_s != HLX_STATUS_OK)
        {
                return H_RESP_SERVER_ERROR;
        }
        return STATUS_OK;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
int32_t proxy_h::s_error_cb(subr &a_subr, nconn &a_nconn)
{
        proxy_h *l_proxy_h = (proxy_h *)a_subr.get_data();
        if(!l_proxy_h)
        {
                return STATUS_ERROR;
        }
        if(!a_subr.get_requester_hconn())
        {
                return STATUS_ERROR;
        }
        l_proxy_h->send_json_resp_err(*a_subr.get_requester_hconn(),
                                      false,
                                      // TODO use supports keep-alives from client request
                                      a_subr.get_fallback_status_code());
        return STATUS_OK;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
bool proxy_h::get_do_default(void)
{
        return true;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
void proxy_h::set_timeout_ms(uint32_t a_val)
{
        m_timeout_ms = a_val;
}

//: ----------------------------------------------------------------------------
//: \details: TODO
//: \return:  TODO
//: \param:   TODO
//: ----------------------------------------------------------------------------
void proxy_h::set_max_parallel(int32_t a_val)
{
        m_max_parallel = a_val;
}

} //namespace ns_hlx {
<|endoftext|>


<commit_before>#include <aidu_vision/WPass3.h>
#include <geometry_msgs/Twist.h>
#include <math.h>

using namespace aidu;

LaserScanHandler::LaserScanHandler(): core::Node::Node(){

  //subscribing and pubishing
  scanmsg = nh->subscribe("/scan", 1, &LaserScanHandler::LaserScanCallback, this);
  speedpublisher = nh->advertise<geometry_msgs::Twist>("/base/speed",1);

  // iniatilising values

    //control values
    targetdistance=0.50;// (m)
    targetangle=0;

  //ROS_INFO("created sensor handler");
}

void LaserScanHandler::LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr& scanmsg){

  //Position and angle control with Laserscan data
  geometry_msgs::Twist twist;
  double range_min=scanmsg->range_min;
  double range_max=scanmsg->range_max;
  double angle_increment=scanmsg->angle_increment;
  int size =(scanmsg->angle_max-scanmsg->angle_min)/(scanmsg->angle_increment);
  int number=0;
  double total=0,avg_dist=0;;

  //transforming to x and y coordinates
  double angle=scanmsg->angle_min;
  double x[size],y[size];
  for (int i=0;i<=size;i++){
    y[i]=cos(angle)*scanmsg->ranges[i];
    x[i]=sin(angle)*scanmsg->ranges[i];
    angle+=angle_increment;
  }

  //calculating distance from wall
  avg_dist=average(y,size);
  ROS_INFO("dist %f",avg_dist);

  //calculating angle of the wall
  double diffdist[size-1];
  for (int i=0;i<=size-1;i++){
    diffdist[i]=(y[i]-y[i+1])/(x[i]-x[i+1]);
  }
  double avg_diffdist=average(diffdist,size-1);
  double theta=atan(avg_diffdist);
  ROS_INFO("theta:%f",theta);

  // initialising control values
  double KpL = 1.5;
  double KdL=1.0;
  double KpA =1.7;
  double KdA = 10.0;
  //calculating angle with wall

  //calculating errors
  double errP,errA;
  errP=avg_dist-targetdistance;
  errA=theta-targetangle;

  //sending speed to Base
  twist.linear.x=errP*KpL;
  twist.angular.z=errA*KpA;

  ROS_INFO("speed:%f  angle:%f",twist.linear.x,twist.angular.z);
  speedpublisher.publish(twist);
  ros::spinOnce();

}

double LaserScanHandler::average(double array[], int size){
  double total=0;
  int number=0;
  for (int i=0;i<=size;i++){
    if(isnan(array[i])==0){
      total+=array[i];
      number++;
    }
  }
  if(number!=0){
    return total/(double)number;}
  else{
      return(0);
  }

}

int main(int argc, char** argv) {
  ros::init(argc, argv, "wpass3");
  LaserScanHandler laserscanhandler;
  laserscanhandler.spin();
  return 0;
}<commit_msg>Implemented door passage<commit_after>#include <aidu_vision/WPass3.h>
#include <geometry_msgs/Twist.h>
#include <math.h>

using namespace aidu;

LaserScanHandler::LaserScanHandler(): core::Node::Node(){

  //subscribing and pubishing
  scanmsg = nh->subscribe("/scan", 1, &LaserScanHandler::LaserScanCallback, this);
  speedpublisher = nh->advertise<geometry_msgs::Twist>("/base/speed",1);

  // iniatilising values

    //control values
    targetdistance=0.50;// (m)
    targetangle=0;

  //ROS_INFO("created sensor handler");
}

void LaserScanHandler::LaserScanCallback(const sensor_msgs::LaserScan::ConstPtr& scanmsg){

  //receiving Kinect data
  double range_min=scanmsg->range_min;
  double range_max=scanmsg->range_max;
  double angle_increment=scanmsg->angle_increment;
  int size =(scanmsg->angle_max-scanmsg->angle_min)/(scanmsg->angle_increment);

  //transforming to x and y coordinates
  double angle=scanmsg->angle_min;
  double x[size],y[size];
  for (int i=0;i<=size;i++){
    y[i]=cos(angle)*scanmsg->ranges[i];
    x[i]=sin(angle)*scanmsg->ranges[i];
    angle+=angle_increment;
  }
  geometry_msgs::Twist twist;

  /*
  //Position and angle control with Laserscan data

  int number=0;
  double total=0,avg_dist=0;;

  //calculating distance from wall
  avg_dist=average(y,size);
  ROS_INFO("dist %f",avg_dist);

  //calculating angle of the wall
  double diffdist[size-1];
  for (int i=0;i<=size-1;i++){
    diffdist[i]=(y[i]-y[i+1])/(x[i]-x[i+1]);
  }
  double avg_diffdist=average(diffdist,size-1);
  double theta=atan(avg_diffdist);
  ROS_INFO("theta:%f",theta);

  // initialising control values
  double KpL = 1.5;
  //double KdL=1.0;
  double KpA =1.7;
  //double KdA = 10.0;
  //calculating angle with wall

  //calculating errors
  double errP,errA;
  errP=avg_dist-targetdistance;
  errA=theta-targetangle;

  //sending speed to Base
  twist.linear.x=errP*KpL;
  twist.angular.z=errA*KpA;

  // Basis laten stoppen als muur te dichtbij
  double treshhold=0.1;
  for (int i=0;i<=size;i++){
    if(y[i]<treshhold){
      twist.linear.x=0.0;
      twist.angular.z=0.0;
    }
  }

  */

  // Door de deur reiden
  double baseWidth=0.8; // breedte van basis + marge
  double treshhold=1.0; // (m)
  double kp=1.7;
  double angleCorrectionleft=0.0,angleCorrectionright=0.0;
  for (int i=0; i<=size; i++){
    //ROS_INFO("x[%d]=%f  y[%d]=%f",i,x[i],i,y[i]);
    if (x[i]>-baseWidth/2.0 && x[i]<-0.1 && y[i]<treshhold){
      angleCorrectionleft=std::min((y[i]-treshhold)*kp,angleCorrectionleft);
    }
    if (x[i]<baseWidth/2.0 && x[i]>0.1 && y[i]<treshhold){
      angleCorrectionright=std::max((y[i]-treshhold)*-kp,angleCorrectionright);
    }
  }

  twist.angular.z=angleCorrectionleft+angleCorrectionright;
  twist.linear.x=0.4;
  ROS_INFO("speed:%f  angle:%f",twist.linear.x,twist.angular.z);
  speedpublisher.publish(twist);
  ros::spinOnce();

}

double LaserScanHandler::average(double array[], int size){
  double total=0;
  int number=0;
  for (int i=0;i<=size;i++){
    if(isnan(array[i])==0){
      total+=array[i];
      number++;
    }
  }
  if(number!=0){
    return total/(double)number;}
  else{
      return(0);
  }

}

int main(int argc, char** argv) {
  ros::init(argc, argv, "wpass3");
  LaserScanHandler laserscanhandler;
  laserscanhandler.spin();
  return 0;
}<|endoftext|>


<commit_before>#include <string>
#include <vector>

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TransformStamped.h>
#include <moveit_msgs/RobotTrajectory.h>

class MoveGroupInterface {
  moveit::planning_interface::MoveGroup move_group_;

  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener listener_;

  geometry_msgs::Pose tomapo_;
  std::vector<geometry_msgs::Pose> waypoints_;

  double eef_length_;
  double shift_margin_;

  moveit::core::VariableBounds rail_bounds_;
  double sign_;

public:
  MoveGroupInterface(const std::string& group_name, const ros::NodeHandle& node_handle)
    : move_group_ {group_name},
      buffer_ {},
      listener_ {buffer_},
      tomapo_ {},
      waypoints_ {},
      rail_bounds_ {move_group_.getRobotModel()->getJointModel("rail_to_shaft_joint")->getVariableBounds()[0]},
      sign_ {1.0}
  {
    node_handle.getParam("effector_length", eef_length_);
    node_handle.getParam("shift_margin", shift_margin_);

    double joint_tolerance;
    node_handle.getParam("joint_tolerance", joint_tolerance);
    move_group_.setGoalJointTolerance(joint_tolerance);

    move_group_.allowReplanning(true);
    move_group_.setPlanningTime(5.0);
  }

  bool queryTargetExistence()
  {
    try {
      geometry_msgs::TransformStamped transform_stamped_ {buffer_.lookupTransform("rail", "tomato", ros::Time(0), ros::Duration(1.0))};
      tomapo_.position.x = transform_stamped_.transform.translation.x;
      tomapo_.position.y = transform_stamped_.transform.translation.y;
      tomapo_.position.z = transform_stamped_.transform.translation.z;
      tomapo_.orientation.x = 0;
      tomapo_.orientation.y = 0;
      tomapo_.orientation.z = 0;
      tomapo_.orientation.w = 1.0;
      return true;
    } catch (const tf2::TransformException& ex) {
      ROS_INFO_STREAM(ex.what());
      return false;
    }
  }

  bool startSequence()
  {
    waypoints_.clear();

    geometry_msgs::Pose pose1 {tomapo_};
    pose1.position.x -= eef_length_;
    move_group_.setPoseTarget(pose1);

    moveit::planning_interface::MoveGroup::Plan plan; // NEED?
    move_group_.plan(plan);
    if (!move_group_.execute(plan)) return false;

    geometry_msgs::Pose pose2 {tomapo_};
    waypoints_.push_back(pose2);

    geometry_msgs::Pose pose3 {tomapo_};
    pose3.orientation = tf::createQuaternionMsgFromRollPitchYaw(1.0, 0, 0); // TODO
    waypoints_.push_back(pose3);

    geometry_msgs::Pose pose4 {tomapo_};
    pose4.position.x -= eef_length_;
    waypoints_.push_back(pose4);

    return move_group_.execute(planCartesianPath(waypoints_));
  }

  bool shift()
  {
    std::vector<double> joint_values {move_group_.getCurrentJointValues()};

    joint_values[1] =  0;
    joint_values[2] = -0.7854;
    joint_values[3] =  1.5707;
    joint_values[4] = -0.7854;
    joint_values[5] =  0;

    move_group_.setJointValueTarget(joint_values);
    moveit::planning_interface::MoveGroup::Plan plan;
    move_group_.plan(plan);

    move_group_.execute(plan);

    if (joint_values[0] > (rail_bounds_.max_position_ - shift_margin_)) sign_ = -1.0;
    else if (joint_values[0] < (rail_bounds_.min_position_ + shift_margin_)) sign_ = 1.0;

    joint_values[0] += sign_ * 1.0;

    move_group_.setJointValueTarget(joint_values);
    move_group_.plan(plan);

    return move_group_.execute(plan);
  }

private:
  moveit::planning_interface::MoveGroup::Plan planCartesianPath(std::vector<geometry_msgs::Pose>& waypoints)
  {
    moveit_msgs::RobotTrajectory msg;
    move_group_.computeCartesianPath(waypoints, 0.10, 0.0, msg);

    robot_trajectory::RobotTrajectory trajectory {move_group_.getRobotModel(), move_group_.getName()};
    trajectory.setRobotTrajectoryMsg(*move_group_.getCurrentState(), msg);

    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    iptp.computeTimeStamps(trajectory);

    moveit::planning_interface::MoveGroup::Plan plan;
    plan.trajectory_ = msg;

    return plan;
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "arcsys2_move_group_interface_node");
  ros::NodeHandle node_handle {"~"};

  ros::AsyncSpinner spinner {1};
  spinner.start();

  MoveGroupInterface interface {"arcsys2", node_handle};

  while (ros::ok()) {
    while (!interface.queryTargetExistence()) interface.shift();
    interface.startSequence();
  }

  spinner.stop();

  return 0;
}
<commit_msg>Add function and param for side-change<commit_after>#include <string>
#include <vector>

#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TransformStamped.h>
#include <moveit_msgs/RobotTrajectory.h>

class MoveGroupInterface {
  moveit::planning_interface::MoveGroup move_group_;

  tf2_ros::Buffer buffer_;
  tf2_ros::TransformListener listener_;

  geometry_msgs::Pose tomapo_;
  std::vector<geometry_msgs::Pose> waypoints_;

  double eef_length_;
  double shift_margin_;
  bool reverse_coordinate_;

  moveit::core::VariableBounds rail_bounds_;
  double sign_;

public:
  MoveGroupInterface(const std::string& group_name, const ros::NodeHandle& node_handle)
    : move_group_ {group_name},
      buffer_ {},
      listener_ {buffer_},
      tomapo_ {},
      waypoints_ {},
      rail_bounds_ {move_group_.getRobotModel()->getJointModel("rail_to_shaft_joint")->getVariableBounds()[0]},
      sign_ {1.0}
  {
    node_handle.getParam("effector_length", eef_length_);
    node_handle.getParam("shift_margin", shift_margin_);
    node_handle.getParam("reverse_coordinate", reverse_coordinate_);

    double joint_tolerance;
    node_handle.getParam("joint_tolerance", joint_tolerance);
    move_group_.setGoalJointTolerance(joint_tolerance);

    move_group_.allowReplanning(true);
    move_group_.setPlanningTime(5.0);
  }

  bool queryTargetExistence()
  {
    try {
      geometry_msgs::TransformStamped transform_stamped_ {buffer_.lookupTransform("rail", "tomato", ros::Time(0), ros::Duration(1.0))};
      tomapo_.position.x = transform_stamped_.transform.translation.x;
      tomapo_.position.y = transform_stamped_.transform.translation.y;
      tomapo_.position.z = transform_stamped_.transform.translation.z;
      tomapo_.orientation.x = 0;
      tomapo_.orientation.y = 0;
      tomapo_.orientation.z = 0;
      tomapo_.orientation.w = 1.0;
      return true;
    } catch (const tf2::TransformException& ex) {
      ROS_INFO_STREAM(ex.what());
      return false;
    }
  }

  bool startSequence()
  {
    waypoints_.clear();

    geometry_msgs::Pose pose1 {tomapo_};
    pose1.position.x -= eef_length_;
    move_group_.setPoseTarget(pose1);

    moveit::planning_interface::MoveGroup::Plan plan; // NEED?
    move_group_.plan(plan);
    if (!move_group_.execute(plan)) return false;

    geometry_msgs::Pose pose2 {tomapo_};
    waypoints_.push_back(pose2);

    geometry_msgs::Pose pose3 {tomapo_};
    pose3.orientation = tf::createQuaternionMsgFromRollPitchYaw(1.0, 0, 0); // TODO
    waypoints_.push_back(pose3);

    geometry_msgs::Pose pose4 {tomapo_};
    pose4.position.x -= eef_length_;
    waypoints_.push_back(pose4);

    return move_group_.execute(planCartesianPath(waypoints_));
  }

  bool shift()
  {
    std::vector<double> joint_values {move_group_.getCurrentJointValues()};

    joint_values[1] =  0;
    joint_values[2] = -0.7854;
    joint_values[3] =  1.5707;
    joint_values[4] = -0.7854;
    joint_values[5] =  0;

    move_group_.setJointValueTarget(joint_values);
    moveit::planning_interface::MoveGroup::Plan plan;
    move_group_.plan(plan);

    move_group_.execute(plan);

    // if (joint_values[0] > (rail_bounds_.max_position_ - shift_margin_)) sign_ = -1.0;
    // else if (joint_values[0] < (rail_bounds_.min_position_ + shift_margin_)) sign_ = 1.0;

    updateShiftSign(joint_values[0]);
    joint_values[0] += sign_ * 1.0;

    move_group_.setJointValueTarget(joint_values);
    move_group_.plan(plan);

    return move_group_.execute(plan);
  }

private:
  moveit::planning_interface::MoveGroup::Plan planCartesianPath(std::vector<geometry_msgs::Pose>& waypoints)
  {
    moveit_msgs::RobotTrajectory msg;
    move_group_.computeCartesianPath(waypoints, 0.10, 0.0, msg);

    robot_trajectory::RobotTrajectory trajectory {move_group_.getRobotModel(), move_group_.getName()};
    trajectory.setRobotTrajectoryMsg(*move_group_.getCurrentState(), msg);

    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    iptp.computeTimeStamps(trajectory);

    moveit::planning_interface::MoveGroup::Plan plan;
    plan.trajectory_ = msg;

    return plan;
  }

  void updateShiftSign(const double& joint_value)
  {
    if (rail_bounds_.max_position_ - shift_margin_ < joint_value)
      reverse_coordinate_ ? sign_ = +1.0 : sign_ = -1.0;

    if (joint_value < 0 + shift_margin_)
      reverse_coordinate_ ? sign_ = -1.0 : sign_ = +1.0;
  }
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "arcsys2_move_group_interface_node");
  ros::NodeHandle node_handle {"~"};

  ros::AsyncSpinner spinner {1};
  spinner.start();

  MoveGroupInterface interface {"arcsys2", node_handle};

  while (ros::ok()) {
    while (!interface.queryTargetExistence()) interface.shift();
    interface.startSequence();
  }

  spinner.stop();

  return 0;
}
<|endoftext|>