//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//
#include <boost/test/unit_test.hpp>

#include "test/TensorHelpers.hpp"
#include "LayerTests.hpp"

#include "backends/CpuTensorHandle.hpp"
#include "backends/NeonLayerSupport.hpp"
#include "backends/NeonWorkloadFactory.hpp"
#include "backends/RefWorkloadFactory.hpp"
#include "backends/test/TensorCopyUtils.hpp"
#include "ActivationFixture.hpp"

#include "WorkloadTestUtils.hpp"

#include "test/UnitTests.hpp"

BOOST_AUTO_TEST_SUITE(Compute_ArmComputeNeon)
using FactoryType = armnn::NeonWorkloadFactory;

// ============================================================================
// UNIT tests

// Convolution
ARMNN_AUTO_TEST_CASE(SimpleConvolution1d, Convolution1dTest, true)

ARMNN_AUTO_TEST_CASE(SimpleConvolution2d, SimpleConvolution2d3x5Test, true)
ARMNN_AUTO_TEST_CASE(SimpleConvolution2dSquare, SimpleConvolution2d3x3Test, true)
ARMNN_AUTO_TEST_CASE(UnbiasedConvolution2d, SimpleConvolution2d3x5Test, false)
ARMNN_AUTO_TEST_CASE(UnbiasedConvolution2dSquare, SimpleConvolution2d3x3Test, false)
ARMNN_AUTO_TEST_CASE(SimpleConvolution2dAsymmetricPadding, Convolution2dAsymmetricPaddingTest)

namespace
{

armnn::Convolution2dDescriptor MakeConv2dDesc(uint32_t strideX, uint32_t strideY,
    uint32_t padLeft = 0, uint32_t padRight = 0, uint32_t padTop = 0, uint32_t padBottom = 0)
{
    armnn::Convolution2dDescriptor result;
    result.m_StrideX = strideX;
    result.m_StrideY = strideY;
    result.m_PadLeft = padLeft;
    result.m_PadRight = padRight;
    result.m_PadTop = padTop;
    result.m_PadBottom = padBottom;
    result.m_BiasEnabled = true;
    return result;
}

}

BOOST_AUTO_TEST_CASE(Conv2dUtils)
{
    // The only preferred Neon convolution is 1x1 with padding=0 and stride size {1,2,3}.
    armnn::TensorShape shape1x1({ 1,1,1,1 });
    armnn::TensorInfo info1x1(shape1x1, armnn::DataType::Float32);
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 1)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 2)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 3)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(2, 1)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(2, 2)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(2, 3)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(3, 1)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(3, 2)));
    BOOST_TEST(armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(3, 3)));

    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(4, 1)));
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(4, 5)));
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(3, 6)));

    // non zero padding is not preferred for direct convolution
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 1, 1, 0)));
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 1, 0, 1)));
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info1x1, MakeConv2dDesc(1, 1, 1, 1)));

    // 2x2 filter not preferred for direct convolution
    armnn::TensorShape shape2x2({ 1,1,2,2 });
    armnn::TensorInfo info2x2(shape2x2, armnn::DataType::Float32);
    BOOST_TEST(!armnn::IsNeonDirectConvolutionPreferred(info2x2, MakeConv2dDesc(1, 1)));
}

// Depthwise Convolution
ARMNN_AUTO_TEST_CASE(DepthwiseConvolution2dDepthMul1, DepthwiseConvolution2dDepthMul1Test, true)
ARMNN_AUTO_TEST_CASE(UnbiasedDepthwiseConvolution2dDepthMul1, DepthwiseConvolution2dDepthMul1Test, false)
ARMNN_AUTO_TEST_CASE(DepthwiseConvolution2dDepthMul1Uint8, DepthwiseConvolution2dDepthMul1Uint8Test, true)
ARMNN_AUTO_TEST_CASE(UnbiasedDepthwiseConvolution2dDepthMul1Uint8, DepthwiseConvolution2dDepthMul1Uint8Test, false)

ARMNN_AUTO_TEST_CASE(DepthwiseConvolution2dAsymmetric, DepthwiseConvolution2dAsymmetricTest, true)
ARMNN_AUTO_TEST_CASE(UnbiasedDepthwiseConvolution2dAsymmetric, DepthwiseConvolution2dAsymmetricTest, false)

namespace
{

armnn::DepthwiseConvolution2dDescriptor MakeDepthwiseConv2dDesc(uint32_t strideX, uint32_t strideY,
    uint32_t depthMultiplier = 1, uint32_t padLeft = 0, uint32_t padRight = 0,
    uint32_t padTop = 0, uint32_t padBottom = 0)
{
    boost::ignore_unused(depthMultiplier);

    armnn::DepthwiseConvolution2dDescriptor desc;

    desc.m_PadLeft = padLeft;
    desc.m_PadRight = padRight;

    desc.m_PadTop = padTop;
    desc.m_PadBottom = padBottom;
    desc.m_StrideX = strideX;
    desc.m_StrideY = strideY;
    desc.m_BiasEnabled = false;

    return desc;
}

armnn::TensorInfo CreateOutputTensorInfo(const armnn::TensorInfo& inputInfo,
                                         const armnn::TensorInfo& weightsInfo,
                                         const armnn::DepthwiseConvolution2dDescriptor& descriptor,
                                         armnn::DataType dataType)
{
    const armnn::TensorShape& inputShape  = inputInfo.GetShape();
    const armnn::TensorShape& filterShape = weightsInfo.GetShape();

    unsigned int inWidth = inputShape[3];
    unsigned int inHeight = inputShape[2];
    unsigned int inBatchSize = inputShape[0];

    unsigned int filterWidth = filterShape[3];
    unsigned int readWidth = (inWidth + descriptor.m_PadLeft + descriptor.m_PadRight) - (filterWidth);
    unsigned int outWidth =  1u + (readWidth / descriptor.m_StrideX);

    unsigned int filterHeight = filterShape[2];
    unsigned int readHeight = (inHeight + descriptor.m_PadTop + descriptor.m_PadBottom) - (filterHeight);
    unsigned int outHeight = 1u + (readHeight / descriptor.m_StrideY);
    unsigned int depthMultiplier = filterShape[0];

    unsigned int outChannels = filterShape[1] * depthMultiplier;
    unsigned int outBatchSize = inBatchSize;

    armnn::TensorShape outputShape({outBatchSize, outChannels, outHeight, outWidth});
    return armnn::TensorInfo(outputShape, dataType);
}
}

BOOST_AUTO_TEST_CASE(DepthwiseConv2dUtils)
{
    const armnn::DataType dataType = armnn::DataType::Float32;

    armnn::TensorInfo inputInfo({1, 1, 10, 10 }, dataType);
    armnn::TensorInfo outputInfo;
    armnn::TensorInfo weightsInfo3x3({ 1, 1, 3, 3 }, dataType);
    armnn::TensorInfo biasesInfo;

    armnn::DepthwiseConvolution2dDescriptor descriptor;

    // Strides supported: 1,2,3
    descriptor = MakeDepthwiseConv2dDesc(1, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(1, 2);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(1, 3);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(2, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(2, 2);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(2, 3);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(3, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(3, 2);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    descriptor = MakeDepthwiseConv2dDesc(3, 3);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    // Supported stride 4
    descriptor = MakeDepthwiseConv2dDesc(4, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));

    // Supported weights shape 1x1
    armnn::TensorInfo weightsInfo1x1({ 1, 1, 1, 1 }, armnn::DataType::Float32);
    descriptor = MakeDepthwiseConv2dDesc(1, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo1x1, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo1x1, biasesInfo));

    // Supported shape 2x2
    armnn::TensorInfo weightsInfo2x2({ 1, 1, 2, 2 }, armnn::DataType::Float32);
    descriptor = MakeDepthwiseConv2dDesc(1, 1);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo2x2, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo2x2, biasesInfo));

    // Asymmetric padding
    descriptor = MakeDepthwiseConv2dDesc(1, 1, 1, 1, 2, 1, 2);
    outputInfo = CreateOutputTensorInfo(inputInfo, weightsInfo3x3, descriptor, dataType);
    BOOST_TEST(armnn::IsDepthwiseConvolutionSupportedNeon(inputInfo, outputInfo, descriptor,
                                                          weightsInfo3x3, biasesInfo));
}

// Pooling
ARMNN_AUTO_TEST_CASE(SimpleMaxPooling2dSize3x3Stride2x4, SimpleMaxPooling2dSize3x3Stride2x4Test, true)
ARMNN_AUTO_TEST_CASE(SimpleMaxPooling2dSize3x3Stride2x4Uint8, SimpleMaxPooling2dSize3x3Stride2x4Uint8Test, true)
ARMNN_AUTO_TEST_CASE(SimpleAveragePooling2d, SimpleAveragePooling2dTest)
ARMNN_AUTO_TEST_CASE(SimpleAveragePooling2dUint8, SimpleAveragePooling2dUint8Test)

ARMNN_AUTO_TEST_CASE(LargeTensorsAveragePooling2d, LargeTensorsAveragePooling2dTest)
ARMNN_AUTO_TEST_CASE(LargeTensorsAveragePooling2dUint8, LargeTensorsAveragePooling2dUint8Test)

ARMNN_AUTO_TEST_CASE(SimpleL2Pooling2d, SimpleL2Pooling2dTest)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_SimpleL2Pooling2dUint8, SimpleL2Pooling2dUint8Test)
ARMNN_AUTO_TEST_CASE(L2Pooling2dSize3Stride1, L2Pooling2dSize3Stride1Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_L2Pooling2dSize3Stride1Uint8, L2Pooling2dSize3Stride1Uint8Test)
ARMNN_AUTO_TEST_CASE(L2Pooling2dSize3Stride3, L2Pooling2dSize3Stride3Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_L2Pooling2dSize3Stride3Uint8, L2Pooling2dSize3Stride3Uint8Test)
ARMNN_AUTO_TEST_CASE(L2Pooling2dSize3Stride4, L2Pooling2dSize3Stride4Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_L2Pooling2dSize3Stride4Uint8, L2Pooling2dSize3Stride4Uint8Test)
ARMNN_AUTO_TEST_CASE(L2Pooling2dSize7, L2Pooling2dSize7Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_L2Pooling2dSize7Uint8, L2Pooling2dSize7Uint8Test)
ARMNN_AUTO_TEST_CASE(L2Pooling2dSize9, L2Pooling2dSize9Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_L2Pooling2dSize9Uint8, L2Pooling2dSize9Uint8Test)

// Ignore padding values for pooling but count padding fields into the divisor
ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleMaxPooling2d, IgnorePaddingSimpleMaxPooling2dTest)
ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleMaxPooling2dUint8, IgnorePaddingSimpleMaxPooling2dUint8Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingMaxPooling2dSize3, IgnorePaddingMaxPooling2dSize3Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingMaxPooling2dSize3Uint8, IgnorePaddingMaxPooling2dSize3Uint8Test)

ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleAveragePooling2d, IgnorePaddingSimpleAveragePooling2dTest)
ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleAveragePooling2dUint8, IgnorePaddingSimpleAveragePooling2dUint8Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleAveragePooling2dNoPadding, IgnorePaddingSimpleAveragePooling2dNoPaddingTest)
ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleAveragePooling2dNoPaddingUint8,
    IgnorePaddingSimpleAveragePooling2dNoPaddingUint8Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingAveragePooling2dSize3, IgnorePaddingAveragePooling2dSize3Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingAveragePooling2dSize3Uint8, IgnorePaddingAveragePooling2dSize3Uint8Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingAveragePooling2dSize3x2Stride2x2,
                             IgnorePaddingAveragePooling2dSize3x2Stride2x2Test, false)
ARMNN_AUTO_TEST_CASE(IgnorePaddingAveragePooling2dSize3x2Stride2x2NoPadding,
                             IgnorePaddingAveragePooling2dSize3x2Stride2x2Test,
                                          true)

ARMNN_AUTO_TEST_CASE(IgnorePaddingSimpleL2Pooling2d, IgnorePaddingSimpleL2Pooling2dTest)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_IgnorePaddingSimpleL2Pooling2dUint8, IgnorePaddingSimpleL2Pooling2dUint8Test)
ARMNN_AUTO_TEST_CASE(IgnorePaddingL2Pooling2dSize3, IgnorePaddingL2Pooling2dSize3Test)
ARMNN_AUTO_TEST_CASE(UNSUPPORTED_IgnorePaddingL2Pooling2dSize3Uint8, IgnorePaddingL2Pooling2dSize3Uint8Test)

// Activation
ARMNN_AUTO_TEST_CASE(ConstantLinearActivation, ConstantLinearActivationTest)

ARMNN_AUTO_TEST_CASE(SimpleSoftmaxBeta1, SimpleSoftmaxTest, 1.0f)
ARMNN_AUTO_TEST_CASE(SimpleSoftmaxBeta2, SimpleSoftmaxTest, 2.0f)

ARMNN_AUTO_TEST_CASE(SimpleSoftmaxBeta1Uint8, SimpleSoftmaxUint8Test, 1.0f)
ARMNN_AUTO_TEST_CASE(SimpleSoftmaxBeta2Uint8, SimpleSoftmaxUint8Test, 2.0f)

ARMNN_AUTO_TEST_CASE(ReLu1Uint8, BoundedReLuUint8UpperAndLowerBoundTest)
ARMNN_AUTO_TEST_CASE(ReLu6Uint8, BoundedReLuUint8UpperBoundOnlyTest)

// Softmax
BOOST_AUTO_TEST_CASE(Softmax4dSupport)
{
    const unsigned int numDimensions = 4u;
    std::array<unsigned int, numDimensions> dimensionSizes;
    dimensionSizes.fill(1u);

    const armnn::TensorInfo inputInfo(numDimensions, &dimensionSizes.front(), armnn::DataType::Float32);
    const armnn::TensorInfo outputInfo(numDimensions, &dimensionSizes.front(), armnn::DataType::Float32);

    // 4D Softmax should be reported as unsupported on the NEON backend
    BOOST_TEST(!armnn::IsSoftmaxSupportedNeon(inputInfo, outputInfo, armnn::SoftmaxDescriptor()));
}

// Splitter
ARMNN_AUTO_TEST_CASE(SimpleSplitter, SplitterTest)
ARMNN_AUTO_TEST_CASE(SimpleSplitterUint8, SplitterUint8Test)

ARMNN_AUTO_TEST_CASE(CopyViaSplitter, CopyViaSplitterTest)
ARMNN_AUTO_TEST_CASE(CopyViaSplitterUint8, CopyViaSplitterUint8Test)

// Merger
ARMNN_AUTO_TEST_CASE(SimpleMerger, MergerTest)
ARMNN_AUTO_TEST_CASE(MergerUint8, MergerUint8Test)

// Fully Connected
ARMNN_AUTO_TEST_CASE(SimpleFullyConnected, FullyConnectedFloat32Test, false, false)
ARMNN_AUTO_TEST_CASE(SimpleFullyConnectedWithBias, FullyConnectedFloat32Test, true, false)
ARMNN_AUTO_TEST_CASE(SimpleFullyConnectedWithTranspose, FullyConnectedFloat32Test, false, true)
ARMNN_AUTO_TEST_CASE(FullyConnectedLarge, FullyConnectedLargeTest, false)
ARMNN_AUTO_TEST_CASE(FullyConnectedLargeTransposed, FullyConnectedLargeTest, true)

// Add
ARMNN_AUTO_TEST_CASE(SimpleAdd, AdditionTest)
ARMNN_AUTO_TEST_CASE(AddBroadcast1Element, AdditionBroadcast1ElementTest)

// Mul
ARMNN_AUTO_TEST_CASE(SimpleMultiplication, MultiplicationTest)
ARMNN_AUTO_TEST_CASE(MultiplicationBroadcast1Element, MultiplicationBroadcast1ElementTest)
ARMNN_AUTO_TEST_CASE(MultiplicationBroadcast1DVector, MultiplicationBroadcast1DVectorTest)

// Batch Norm
ARMNN_AUTO_TEST_CASE(BatchNorm, BatchNormTest)

// Constant
ARMNN_AUTO_TEST_CASE(Constant, ConstantTest)
ARMNN_AUTO_TEST_CASE(ConstantUint8, ConstantTestUint8)

// Concatenation
ARMNN_AUTO_TEST_CASE(Concatenation1d, Concatenation1dTest)
ARMNN_AUTO_TEST_CASE(Concatenation1dUint8, Concatenation1dUint8Test)

ARMNN_AUTO_TEST_CASE(Concatenation2dDim0, Concatenation2dDim0Test)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim0Uint8, Concatenation2dDim0Uint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim1, Concatenation2dDim1Test)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim1Uint8, Concatenation2dDim1Uint8Test)

ARMNN_AUTO_TEST_CASE(Concatenation2dDim0DiffInputDims, Concatenation2dDim0DiffInputDimsTest)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim0DiffInputDimsUint8, Concatenation2dDim0DiffInputDimsUint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim1DiffInputDims, Concatenation2dDim1DiffInputDimsTest)
ARMNN_AUTO_TEST_CASE(Concatenation2dDim1DiffInputDimsUint8, Concatenation2dDim1DiffInputDimsUint8Test)

ARMNN_AUTO_TEST_CASE(Concatenation3dDim0, Concatenation3dDim0Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim0Uint8, Concatenation3dDim0Uint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim1, Concatenation3dDim1Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim1Uint8, Concatenation3dDim1Uint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim2, Concatenation3dDim2Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim2Uint8, Concatenation3dDim2Uint8Test)

ARMNN_AUTO_TEST_CASE(Concatenation3dDim0DiffInputDims, Concatenation3dDim0DiffInputDimsTest)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim0DiffInputDimsUint8, Concatenation3dDim0DiffInputDimsUint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim1DiffInputDims, Concatenation3dDim1DiffInputDimsTest)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim1DiffInputDimsUint8, Concatenation3dDim1DiffInputDimsUint8Test)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim2DiffInputDims, Concatenation3dDim2DiffInputDimsTest)
ARMNN_AUTO_TEST_CASE(Concatenation3dDim2DiffInputDimsUint8, Concatenation3dDim2DiffInputDimsUint8Test)

// L2 Normalization
ARMNN_AUTO_TEST_CASE(L2Normalization1d, L2Normalization1dTest);
ARMNN_AUTO_TEST_CASE(L2Normalization2d, L2Normalization2dTest);
ARMNN_AUTO_TEST_CASE(L2Normalization3d, L2Normalization3dTest);
ARMNN_AUTO_TEST_CASE(L2Normalization4d, L2Normalization4dTest);

// Floor
ARMNN_AUTO_TEST_CASE(SimpleFloor, SimpleFloorTest)

// Reshape
ARMNN_AUTO_TEST_CASE(SimpleReshapeFloat32, SimpleReshapeFloat32Test)
ARMNN_AUTO_TEST_CASE(SimpleReshapeUint8, SimpleReshapeUint8Test)

// Permute
ARMNN_AUTO_TEST_CASE(SimplePermuteFloat32, SimplePermuteFloat32Test)
ARMNN_AUTO_TEST_CASE(SimplePermuteUint8, SimplePermuteUint8Test)
ARMNN_AUTO_TEST_CASE(PermuteFloat32ValueSet1, PermuteFloat32ValueSet1Test)
ARMNN_AUTO_TEST_CASE(PermuteFloat32ValueSet2, PermuteFloat32ValueSet2Test)
ARMNN_AUTO_TEST_CASE(PermuteFloat32ValueSet3, PermuteFloat32ValueSet3Test)

// ============================================================================
// COMPARE tests

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareConv2dWithReference, CompareConvolution2dTest)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareDepthwiseConv2dWithReferenceFloat32, CompareDepthwiseConvolution2dTest<float>)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareDepthwiseConv2dWithReferenceUint8, CompareDepthwiseConvolution2dTest<uint8_t>)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareNormalizationWithinWithReference, CompareNormalizationTest,
                                 armnn::NormalizationAlgorithmChannel::Within,
                                 armnn::NormalizationAlgorithmMethod::LocalBrightness)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareNormalizationAcrossWithReference, CompareNormalizationTest,
                                 armnn::NormalizationAlgorithmChannel::Across,
                                 armnn::NormalizationAlgorithmMethod::LocalBrightness)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareMaxPooling2dWithReference, ComparePooling2dTest, armnn::PoolingAlgorithm::Max)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareMaxPooling2dWithReferenceUint8, ComparePooling2dUint8Test,
                                 armnn::PoolingAlgorithm::Max)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareAveragePooling2dWithReference, ComparePooling2dTest,
                                 armnn::PoolingAlgorithm::Average)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareAveragePooling2dWithReferenceUint8, ComparePooling2dUint8Test,
                                 armnn::PoolingAlgorithm::Average)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareL2Pooling2dWithReference, ComparePooling2dTest, armnn::PoolingAlgorithm::L2)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(UNSUPPORTED_CompareL2Pooling2dWithReferenceUint8, ComparePooling2dUint8Test,
                                 armnn::PoolingAlgorithm::L2)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareSoftmaxBeta1WithReference, CompareSoftmaxTest, 1.0f)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareSoftmaxBeta2WithReference, CompareSoftmaxTest, 2.0f)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareSoftmaxUint8Beta1WithReference, CompareSoftmaxUint8Test, 1.0f)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareSoftmaxUint8Beta2WithReference, CompareSoftmaxUint8Test, 2.0f)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareAddition, CompareAdditionTest)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareMultiplicationWithReference, CompareMultiplicationTest)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(CompareBatchNorm, CompareBatchNormTest)

ARMNN_COMPARE_REF_AUTO_TEST_CASE(ReLu1, CompareBoundedReLuTest, 1.0f, -1.0f)
ARMNN_COMPARE_REF_AUTO_TEST_CASE(ReLu6, CompareBoundedReLuTest, 6.0f, 0.0f)

// ============================================================================
// FIXTURE tests

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareSigmoidActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::Sigmoid, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareTanhActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::TanH, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareLinearActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::Linear, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareReLuActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::ReLu, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareBoundedReLuActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::BoundedReLu, 5u)
ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareBoundedReLuActivationWithReferenceUint8, ActivationFixture,
                                    CompareActivationUint8Test, armnn::ActivationFunction::BoundedReLu)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareSoftReLuActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::SoftReLu, 1u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareLeakyReLuActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::LeakyReLu, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareAbsActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::Abs, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareSqrtActivationWithReference, PositiveActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::Sqrt, 5u)

ARMNN_COMPARE_REF_FIXTURE_TEST_CASE(CompareSquareActivationWithReference, ActivationFixture,
                                    CompareActivationTest, armnn::ActivationFunction::Square, 5u)
BOOST_AUTO_TEST_SUITE_END()


/*
  Authors: Darya Filippova, Geet Duggal, Rob Patro
  dfilippo | geet | robp @cs.cmu.edu
  See LICENSE.txt included with this distribution.
*/

#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>

#include <boost/range/irange.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "Version.hpp"
#include "ArmatusUtil.hpp"
#include "mpi.h"

using namespace std;

typedef struct {
    int sk, fk; /* Зона ответственности k-го процесса, по оси y */
    int rank; /* Номер текущего процесса. (k) */
    int size; /* Число процессов. (p) */
} parall_st;

void dec(const int rank, const int ny, const int size, int *sk, int *fk){
    *sk = (ny / size) * rank;
    *fk = *sk + ny / size;
    if (rank == size - 1) *fk = ny;
}

int main(int argc, char* argv[]) {
  auto str = R"(
	***********************************
	****                           ****
	****        ARMATUS 2.3        ****
	****                           ****
	***********************************
	)";

  namespace po = boost::program_options;
  using std::string;
  using std::cerr;

  class Params {
    public:
    string inputFile;
    string outputPrefix;
    double gammaMax;
    size_t k;
    double stepSize;
    int minMeanSamples;
    int resolution;
    bool outputMultiscale;
    bool raoFormat;
    bool noNormalization;  // No normalization, just raw counts (Rao format)
    bool sparseFormat;
    bool justGammaMax;
    string chrom;
  };

  MPI_Init(&argc, &argv);
  parall_st l;
  MPI_Comm_rank(MPI_COMM_WORLD, &l.rank);
  MPI_Comm_size(MPI_COMM_WORLD, &l.size);

  Params p;

  // Declare the supported options.
  po::options_description opts("armatus options");
  opts.add_options()
  ("parseRaoFormat,R", po::value<bool>(&p.raoFormat)->zero_tokens()->default_value(false), "Parse the Rao data format instead of Dixon et al. (Provide, for example, GM12878_combined/5kb_resolution_intrachromosomal/chr1/MAPQGE30/chr1_5kb,  and KR normalization is used.")
  ("noNormalization,N", po::value<bool>(&p.noNormalization)->zero_tokens()->default_value(false), "No normalization (Rao et al. format)")
  ("parseSparseFormat,S", po::value<bool>(&p.sparseFormat)->zero_tokens()->default_value(false), "Parse sparse matrix format (input a 3 column text file)")
  ("gammaMax,g", po::value<double>(&p.gammaMax)->required(), "gamma-max (highest resolution to generate domains)")
  ("justGammaMax,j", po::value<bool>(&p.justGammaMax)->zero_tokens()->default_value(false), "Just obtain domains at the maximum Gamma")
  ("help,h", "produce help message")
  ("input,i", po::value<string>(&p.inputFile)->required(), "input matrix file")
  ("topK,k", po::value<size_t>(&p.k)->default_value(1), "Compute the top k optimal solutions")
  ("outputMultiscale,m", po::value<bool>(&p.outputMultiscale)->zero_tokens()->default_value(false), "Output multiscale domains to files as well")
  ("resolution,r", po::value<int>(&p.resolution)->default_value(1), "Resolution of data")
  ("chromosome,c", po::value<string>(&p.chrom)->default_value("N/A"), "Chromosome")
  ("minMeanSamples,n", po::value<int>(&p.minMeanSamples)->default_value(100), "Minimum required number of samples to compute a mean")
  ("output,o", po::value<string>(&p.outputPrefix)->required(), "output filename prefix")
  ("stepSize,s", po::value<double>(&p.stepSize)->default_value(0.05), "Step size to increment resolution parameter")
  ;

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, opts), vm);

    if (vm.count("help")) {
      cerr << str << "\n";
      cerr << opts << "\n";
      std::exit(1);
    }

    po::notify(vm);

    if (vm.count("input")) {

      MatrixProperties matProp;
      if (p.raoFormat) {
          matProp = parseRaoMatrix(p.inputFile, p.resolution, p.chrom, p.noNormalization);
      }
      else if (p.sparseFormat) {
	matProp = parseSparseMatrix(p.inputFile, p.resolution, p.chrom);
      }
      else {
          matProp = parseGZipMatrix(p.inputFile, p.resolution, p.chrom);
      }
      auto mat = matProp.matrix;

      if (l.rank == 0) {
          cerr << "MatrixParser read matrix of size: " << mat->size1() << " x " << mat->size2()  << "\n";
          if (p.outputMultiscale) cerr << "Multiresoultion ensemble will be written to files" << endl;
          cerr << "Reading input from " << p.inputFile << ".\n";
      }

      dec(l.rank, (int)(p.gammaMax/p.stepSize)+1, l.size, &l.sk, &l.fk);

      auto dEnsemble = multiscaleDomains(mat, l.sk, l.fk, p.stepSize, p.k, p.minMeanSamples, p.justGammaMax);

      cerr << "Domain ensemble size: " << dEnsemble.domainSets.size() << endl;
      auto dConsensus = consensusDomains(dEnsemble);

      auto consensusFile = p.outputPrefix + ".consensus.txt";
      cerr << "Writing consensus domains to: " << consensusFile << endl;;
      outputDomains(dConsensus, consensusFile, matProp);

      if (p.outputMultiscale) {
        cerr << "Writing multiscale domains" << endl;
        for (auto dSetIdx : boost::irange(size_t{0}, dEnsemble.domainSets.size())) {
          auto& dSet = dEnsemble.domainSets[dSetIdx];
          size_t multiOptIdx = dEnsemble.optidx[dSetIdx];
          float gamma = dEnsemble.resolutions[dSetIdx];
          stringstream multiscaleFile;
          //multiscaleFile << p.outputPrefix << ".gamma." << std::fixed << std::setprecision(log10(p.gammaMax/p.stepSize)+1) << gamma << "." << setfill('0') << setw(log10(p.k)+1) << multiOptIdx << ".txt";
          multiscaleFile << p.outputPrefix << ".gamma." << gamma << "." << multiOptIdx << ".txt";
          cout << dSetIdx << "\t" << multiscaleFile.str() << endl;
          outputDomains(dSet, multiscaleFile.str(),matProp);
        }
      }

    } else {
      cerr << "Input file was not set.\n";
      std::exit(1);
    }

  } catch (po::error& e) {
    cerr << "exception : [" << e.what() << "]. Exiting.\n";
    std::exit(1);
  }

  MPI_Finalize();

  return 0;
}


#include <algorithm>
#include <boost/optional.hpp>
#include <climits>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <functional>
#include <iomanip>
#include <iostream>
#include <limits>
#include <list>
#include <map>
#include <numeric>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <tuple>
#include <type_traits>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#define REP(i, n) for(int i = 0, i##_MACRO = (n); i < i##_MACRO; i++)
#define RANGE(i, a, b) for(int i = (a), i##_MACRO = (b); i < i##_MACRO; i++)
#define EACH(e, a) for(auto&& e : a)
#define ALL(a) std::begin(a), std::end(a)
#define RALL(a) std::rbegin(a), std::rend(a)
#define FILL(a, n) memset((a), n, sizeof(a))
#define FILLZ(a) FILL(a, 0)
#define INT(x) (static_cast<int>(x))
#define PRECISION(x) std::fixed << std::setprecision(x)

using namespace std;

using ll = long long;
using VI = std::vector<int>;
using VI2D = std::vector<vector<int>>;
using VLL = std::vector<long long>;
using VLL2D = std::vector<vector<long long>>;

constexpr int INF = 2e9;
constexpr double EPS = 1e-10;
constexpr double PI = acos(-1.0);

constexpr int dx[] = {-1, 0, 1, 0};
constexpr int dy[] = {0, -1, 0, 1};

template <typename T>
constexpr int sign(T x) {
	return x < 0 ? -1 : x > 0 ? 1 : 0;
}

template <>
constexpr int sign(double x) {
	return x < -EPS ? -1 : x > EPS ? 1 : 0;
}

template <typename T, typename U>
constexpr void chmax(T& m, U x) {
	m = max(m, x);
}

template <typename T, typename U>
constexpr void chmin(T& m, U x) {
	m = min(m, x);
}

template <typename T>
constexpr T square(T x) {
	return x * x;
}

template <long long MOD = 1000000007>
class ModInt {
	public:
	long long n;

	constexpr ModInt() : n(0) {}
	constexpr ModInt(long long n) : n(n < 0 ? n + MOD : n % MOD) {}

	constexpr long long get() const { return this->n; }
	constexpr long long get_mod() const { return MOD; }

	constexpr ModInt inv() const { return pow<ModInt<MOD>>(*this, MOD - 2); }

	constexpr ModInt& operator=(const long long rhs) {
		return *this = ModInt(rhs);
	}
	constexpr ModInt& operator+=(const ModInt rhs) {
		return *this = ModInt(this->n + rhs.n);
	}
	constexpr ModInt& operator-=(const ModInt rhs) {
		return *this = ModInt(this->n - rhs.n);
	}
	constexpr ModInt& operator*=(const ModInt rhs) {
		return *this = ModInt(this->n * rhs.n);
	}
	constexpr ModInt& operator/=(const ModInt rhs) {
		return *this *= rhs.inv();
	}
	constexpr bool operator==(const ModInt rhs) const {
		return this->n == rhs.n;
	}
};

template <long long MOD>
constexpr ModInt<MOD> operator+(const ModInt<MOD>& lhs,
								const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) += rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator+(const ModInt<MOD>& lhs, const long long& rhs) {
	return ModInt<MOD>(lhs) += rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator+(const long long& lhs, const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) += rhs;
}

template <long long MOD>
constexpr ModInt<MOD> operator-(const ModInt<MOD>& lhs,
								const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) -= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator-(const ModInt<MOD>& lhs, const long long& rhs) {
	return ModInt<MOD>(lhs) -= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator-(const long long& lhs, const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) -= rhs;
}

template <long long MOD>
constexpr ModInt<MOD> operator*(const ModInt<MOD>& lhs,
								const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) *= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator*(const ModInt<MOD>& lhs, const long long& rhs) {
	return ModInt<MOD>(lhs) *= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator*(const long long& lhs, const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) *= rhs;
}

template <long long MOD>
constexpr ModInt<MOD> operator/(const ModInt<MOD>& lhs,
								const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) /= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator/(const ModInt<MOD>& lhs, const long long& rhs) {
	return ModInt<MOD>(lhs) /= rhs;
}
template <long long MOD>
constexpr ModInt<MOD> operator/(const long long& lhs, const ModInt<MOD>& rhs) {
	return ModInt<MOD>(lhs) /= rhs;
}

template <long long MOD>
std::ostream& operator<<(std::ostream& os, const ModInt<MOD>& x) {
	return os << x.n;
}

template <long long MOD>
std::istream& operator>>(std::istream& is, const ModInt<MOD>& x) {
	return is >> x.n;
}

template <typename T>
class Counter {
	using iterator = typename std::unordered_map<T, std::size_t>::iterator;
	using const_iterator =
		typename std::unordered_map<T, std::size_t>::const_iterator;

	std::unordered_map<T, std::size_t> m;

	public:
	template <typename U>
	Counter(U& v) {
		for(T& e : v) {
			this->m[e]++;
		}
	}

	std::vector<T> keys() const {
		std::vector<T> v(this->size());
		int i = 0;
		for(const std::pair<T, std::size_t>& e : this->m) {
			v[i++] = e.first;
		}
		return v;
	}

	std::vector<size_t> values() const {
		std::vector<size_t> v(this->size());
		int i = 0;
		for(const std::pair<T, std::size_t>& e : this->m) {
			v[i++] = e.second;
		}
		return v;
	}

	std::size_t size() const { return this->m.size(); }
	iterator begin() { return this->m.begin(); }
	iterator end() { return this->m.end(); }
	const_iterator cbegin() const { return this->m.cbegin(); }
	const_iterator cend() const { return this->m.cend(); }

	const std::size_t& operator[](T a) const& { return this->m[a]; }
	std::size_t& operator[](T a) & { return this->m[a]; }
	std::size_t operator[](T a) const&& { return std::move(this->m[a]); }
};

int main() {
	int n;
	string s;
	cin >> n;
	cin >> s;

	Counter<char> count(s);
	ModInt<> result = 1;
	EACH(e, count.values()) { result *= e + 1; }

	cout << result - 1 << endl;
	return 0;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2017-2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_SRS_TRANSFORMATION_HPP
#define BOOST_GEOMETRY_SRS_TRANSFORMATION_HPP

#include <string>
#include <type_traits>

#include <boost/throw_exception.hpp>

#include <boost/geometry/algorithms/convert.hpp>

#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/static_assert.hpp>

#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/multi_point.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/geometries/segment.hpp>

#include <boost/geometry/srs/projection.hpp>
#include <boost/geometry/srs/projections/grids.hpp>
#include <boost/geometry/srs/projections/impl/pj_transform.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

namespace boost { namespace geometry
{

namespace projections { namespace detail
{

template <typename T1, typename T2>
inline bool same_object(T1 const& , T2 const& )
{
    return false;
}

template <typename T>
inline bool same_object(T const& o1, T const& o2)
{
    return boost::addressof(o1) == boost::addressof(o2);
}

template
<
    typename PtIn,
    typename PtOut,
    bool SameUnits = std::is_same
                        <
                            typename geometry::detail::cs_angular_units<PtIn>::type,
                            typename geometry::detail::cs_angular_units<PtOut>::type
                        >::value
>
struct transform_geometry_point_coordinates
{
    static inline void apply(PtIn const& in, PtOut & out, bool /*enable_angles*/)
    {
        geometry::set<0>(out, geometry::get<0>(in));
        geometry::set<1>(out, geometry::get<1>(in));
    }
};

template <typename PtIn, typename PtOut>
struct transform_geometry_point_coordinates<PtIn, PtOut, false>
{
    static inline void apply(PtIn const& in, PtOut & out, bool enable_angles)
    {
        if (enable_angles)
        {
            geometry::set_from_radian<0>(out, geometry::get_as_radian<0>(in));
            geometry::set_from_radian<1>(out, geometry::get_as_radian<1>(in));
        }
        else
        {
            geometry::set<0>(out, geometry::get<0>(in));
            geometry::set<1>(out, geometry::get<1>(in));
        }
    }
};

template <typename Geometry, typename CT>
struct transform_geometry_point
{
    typedef typename geometry::point_type<Geometry>::type point_type;

    typedef geometry::model::point
        <
            typename select_most_precise
                <
                    typename geometry::coordinate_type<point_type>::type,
                    CT
                >::type,
            geometry::dimension<point_type>::type::value,
            typename geometry::coordinate_system<point_type>::type
        > type;

    template <typename PtIn, typename PtOut>
    static inline void apply(PtIn const& in, PtOut & out, bool enable_angles)
    {
        transform_geometry_point_coordinates<PtIn, PtOut>::apply(in, out, enable_angles);
        projections::detail::copy_higher_dimensions<2>(in, out);
    }
};

template <typename Geometry, typename CT>
struct transform_geometry_range_base
{
    struct convert_strategy
    {
        convert_strategy(bool enable_angles)
            : m_enable_angles(enable_angles)
        {}

        template <typename PtIn, typename PtOut>
        void apply(PtIn const& in, PtOut & out)
        {
            transform_geometry_point<Geometry, CT>::apply(in, out, m_enable_angles);
        }

        bool m_enable_angles;
    };

    template <typename In, typename Out>
    static inline void apply(In const& in, Out & out, bool enable_angles)
    {
        // Change the order and/or closure if needed
        // In - arbitrary geometry
        // Out - either Geometry or std::vector
        // So the order and closure of In and Geometry shoudl be compared
        // std::vector's order is assumed to be the same as of Geometry
        geometry::detail::conversion::range_to_range
            <
                In,
                Out,
                geometry::point_order<In>::value != geometry::point_order<Out>::value
            >::apply(in, out, convert_strategy(enable_angles));
    }
};

template
<
    typename Geometry,
    typename CT,
    typename Tag = typename geometry::tag<Geometry>::type
>
struct transform_geometry
{};

template <typename Point, typename CT>
struct transform_geometry<Point, CT, point_tag>
    : transform_geometry_point<Point, CT>
{};

template <typename Segment, typename CT>
struct transform_geometry<Segment, CT, segment_tag>
{
    typedef geometry::model::segment
        <
            typename transform_geometry_point<Segment, CT>::type
        > type;

    template <typename In, typename Out>
    static inline void apply(In const& in, Out & out, bool enable_angles)
    {
        apply<0>(in, out, enable_angles);
        apply<1>(in, out, enable_angles);
    }

private:
    template <std::size_t Index, typename In, typename Out>
    static inline void apply(In const& in, Out & out, bool enable_angles)
    {
        geometry::detail::indexed_point_view<In const, Index> in_pt(in);
        geometry::detail::indexed_point_view<Out, Index> out_pt(out);
        transform_geometry_point<Segment, CT>::apply(in_pt, out_pt, enable_angles);
    }
};

template <typename MultiPoint, typename CT>
struct transform_geometry<MultiPoint, CT, multi_point_tag>
    : transform_geometry_range_base<MultiPoint, CT>
{
    typedef model::multi_point
        <
            typename transform_geometry_point<MultiPoint, CT>::type
        > type;
};

template <typename LineString, typename CT>
struct transform_geometry<LineString, CT, linestring_tag>
    : transform_geometry_range_base<LineString, CT>
{
    typedef model::linestring
        <
            typename transform_geometry_point<LineString, CT>::type
        > type;
};

template <typename Ring, typename CT>
struct transform_geometry<Ring, CT, ring_tag>
    : transform_geometry_range_base<Ring, CT>
{
    typedef model::ring
        <
            typename transform_geometry_point<Ring, CT>::type,
            geometry::point_order<Ring>::value == clockwise,
            geometry::closure<Ring>::value == closed
        > type;
};

template
<
    typename OutGeometry,
    typename CT,
    bool EnableTemporary = ! std::is_same
                                <
                                    typename select_most_precise
                                        <
                                            typename geometry::coordinate_type<OutGeometry>::type,
                                            CT
                                        >::type,
                                    typename geometry::coordinate_type<OutGeometry>::type
                                >::value
>
struct transform_geometry_wrapper
{
    typedef transform_geometry<OutGeometry, CT> transform;
    typedef typename transform::type type;

    template <typename InGeometry>
    transform_geometry_wrapper(InGeometry const& in, OutGeometry & out, bool input_angles)
        : m_out(out)
    {
        transform::apply(in, m_temp, input_angles);
    }

    type & get() { return m_temp; }
    void finish() { geometry::convert(m_temp, m_out); } // this is always copy 1:1 without changing the order or closure

private:
    type m_temp;
    OutGeometry & m_out;
};

template
<
    typename OutGeometry,
    typename CT
>
struct transform_geometry_wrapper<OutGeometry, CT, false>
{
    typedef transform_geometry<OutGeometry, CT> transform;
    typedef OutGeometry type;

    transform_geometry_wrapper(OutGeometry const& in, OutGeometry & out, bool input_angles)
        : m_out(out)
    {
        if (! same_object(in, out))
            transform::apply(in, out, input_angles);
    }

    template <typename InGeometry>
    transform_geometry_wrapper(InGeometry const& in, OutGeometry & out, bool input_angles)
        : m_out(out)
    {
        transform::apply(in, out, input_angles);
    }

    OutGeometry & get() { return m_out; }
    void finish() {}

private:
    OutGeometry & m_out;
};

template <typename CT>
struct transform_range
{
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename RangeIn, typename RangeOut,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             RangeIn const& in, RangeOut & out,
                             Grids const& grids1, Grids const& grids2)
    {
        // NOTE: this has to be consistent with pj_transform()
        bool const input_angles = !par1.is_geocent && par1.is_latlong;

        transform_geometry_wrapper<RangeOut, CT> wrapper(in, out, input_angles);

        bool res = true;
        try
        {
            res = pj_transform(proj1, par1, proj2, par2, wrapper.get(), grids1, grids2);
        }
        catch (projection_exception const&)
        {
            res = false;
        }
        catch(...)
        {
            BOOST_RETHROW
        }

        wrapper.finish();

        return res;
    }
};

template <typename Policy>
struct transform_multi
{
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename MultiIn, typename MultiOut,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             MultiIn const& in, MultiOut & out,
                             Grids const& grids1, Grids const& grids2)
    {
        if (! same_object(in, out))
            range::resize(out, boost::size(in));

        return apply(proj1, par1, proj2, par2,
                     boost::begin(in), boost::end(in),
                     boost::begin(out),
                     grids1, grids2);
    }

private:
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename InIt, typename OutIt,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             InIt in_first, InIt in_last, OutIt out_first,
                             Grids const& grids1, Grids const& grids2)
    {
        bool res = true;
        for ( ; in_first != in_last ; ++in_first, ++out_first )
        {
            if ( ! Policy::apply(proj1, par1, proj2, par2, *in_first, *out_first, grids1, grids2) )
            {
                res = false;
            }
        }
        return res;
    }
};

template
<
    typename Geometry,
    typename CT,
    typename Tag = typename geometry::tag<Geometry>::type
>
struct transform
    : not_implemented<Tag>
{};

template <typename Point, typename CT>
struct transform<Point, CT, point_tag>
{
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename PointIn, typename PointOut,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             PointIn const& in, PointOut & out,
                             Grids const& grids1, Grids const& grids2)
    {
        // NOTE: this has to be consistent with pj_transform()
        bool const input_angles = !par1.is_geocent && par1.is_latlong;

        transform_geometry_wrapper<PointOut, CT> wrapper(in, out, input_angles);

        typedef typename transform_geometry_wrapper<PointOut, CT>::type point_type;
        point_type * ptr = boost::addressof(wrapper.get());

        std::pair<point_type *, point_type *> range = std::make_pair(ptr, ptr + 1);

        bool res = true;
        try
        {
            res = pj_transform(proj1, par1, proj2, par2, range, grids1, grids2);
        }
        catch (projection_exception const&)
        {
            res = false;
        }
        catch(...)
        {
            BOOST_RETHROW
        }

        wrapper.finish();

        return res;
    }
};

template <typename MultiPoint, typename CT>
struct transform<MultiPoint, CT, multi_point_tag>
    : transform_range<CT>
{};

template <typename Segment, typename CT>
struct transform<Segment, CT, segment_tag>
{
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename SegmentIn, typename SegmentOut,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             SegmentIn const& in, SegmentOut & out,
                             Grids const& grids1, Grids const& grids2)
    {
        // NOTE: this has to be consistent with pj_transform()
        bool const input_angles = !par1.is_geocent && par1.is_latlong;

        transform_geometry_wrapper<SegmentOut, CT> wrapper(in, out, input_angles);

        typedef typename geometry::point_type
            <
                typename transform_geometry_wrapper<SegmentOut, CT>::type
            >::type point_type;

        point_type points[2];

        geometry::detail::assign_point_from_index<0>(wrapper.get(), points[0]);
        geometry::detail::assign_point_from_index<1>(wrapper.get(), points[1]);

        std::pair<point_type*, point_type*> range = std::make_pair(points, points + 2);

        bool res = true;
        try
        {
            res = pj_transform(proj1, par1, proj2, par2, range, grids1, grids2);
        }
        catch (projection_exception const&)
        {
            res = false;
        }
        catch(...)
        {
            BOOST_RETHROW
        }

        geometry::detail::assign_point_to_index<0>(points[0], wrapper.get());
        geometry::detail::assign_point_to_index<1>(points[1], wrapper.get());

        wrapper.finish();

        return res;
    }
};

template <typename Linestring, typename CT>
struct transform<Linestring, CT, linestring_tag>
    : transform_range<CT>
{};

template <typename MultiLinestring, typename CT>
struct transform<MultiLinestring, CT, multi_linestring_tag>
    : transform_multi<transform_range<CT> >
{};

template <typename Ring, typename CT>
struct transform<Ring, CT, ring_tag>
    : transform_range<CT>
{};

template <typename Polygon, typename CT>
struct transform<Polygon, CT, polygon_tag>
{
    template
    <
        typename Proj1, typename Par1,
        typename Proj2, typename Par2,
        typename PolygonIn, typename PolygonOut,
        typename Grids
    >
    static inline bool apply(Proj1 const& proj1, Par1 const& par1,
                             Proj2 const& proj2, Par2 const& par2,
                             PolygonIn const& in, PolygonOut & out,
                             Grids const& grids1, Grids const& grids2)
    {
        bool r1 = transform_range
                    <
                        CT
                    >::apply(proj1, par1, proj2, par2,
                             geometry::exterior_ring(in),
                             geometry::exterior_ring(out),
                             grids1, grids2);
        bool r2 = transform_multi
                    <
                        transform_range<CT>
                     >::apply(proj1, par1, proj2, par2,
                              geometry::interior_rings(in),
                              geometry::interior_rings(out),
                              grids1, grids2);
        return r1 && r2;
    }
};

template <typename MultiPolygon, typename CT>
struct transform<MultiPolygon, CT, multi_polygon_tag>
    : transform_multi
        <
            transform
                <
                    typename boost::range_value<MultiPolygon>::type,
                    CT,
                    polygon_tag
                >
        >
{};

}} // namespace projections::detail

namespace srs
{

/*!
    \brief Representation of projection
    \details Either dynamic or static projection representation
    \ingroup projection
    \tparam Proj1 default_dynamic or static projection parameters
    \tparam Proj2 default_dynamic or static projection parameters
    \tparam CT calculation type used internally
*/
template
<
    typename Proj1 = srs::dynamic,
    typename Proj2 = srs::dynamic,
    typename CT = double
>
class transformation
{
    typedef typename projections::detail::promote_to_double<CT>::type calc_t;

public:
    // Both static and default constructed
    transformation()
    {}

    // First dynamic, second static and default constructed
    template
    <
        typename Parameters1,
        std::enable_if_t
            <
                std::is_same<Proj1, srs::dynamic>::value
             && projections::dynamic_parameters<Parameters1>::is_specialized,
                int
            > = 0
    >
    explicit transformation(Parameters1 const& parameters1)
        : m_proj1(parameters1)
    {}

    // First static, second static and default constructed
    explicit transformation(Proj1 const& parameters1)
        : m_proj1(parameters1)
    {}

    // Both dynamic
    template
    <
        typename Parameters1, typename Parameters2,
        std::enable_if_t
            <
                std::is_same<Proj1, srs::dynamic>::value
             && std::is_same<Proj2, srs::dynamic>::value
             && projections::dynamic_parameters<Parameters1>::is_specialized
             && projections::dynamic_parameters<Parameters2>::is_specialized,
                int
            > = 0
    >
    transformation(Parameters1 const& parameters1,
                   Parameters2 const& parameters2)
        : m_proj1(parameters1)
        , m_proj2(parameters2)
    {}

    // First dynamic, second static
    template
    <
        typename Parameters1,
        std::enable_if_t
            <
                std::is_same<Proj1, srs::dynamic>::value
             && projections::dynamic_parameters<Parameters1>::is_specialized,
                int
            > = 0
    >
    transformation(Parameters1 const& parameters1,
                   Proj2 const& parameters2)
        : m_proj1(parameters1)
        , m_proj2(parameters2)
    {}

    // First static, second dynamic
    template
    <
        typename Parameters2,
        std::enable_if_t
            <
                std::is_same<Proj2, srs::dynamic>::value
             && projections::dynamic_parameters<Parameters2>::is_specialized,
                int
            > = 0
    >
    transformation(Proj1 const& parameters1,
                   Parameters2 const& parameters2)
        : m_proj1(parameters1)
        , m_proj2(parameters2)
    {}

    // Both static
    transformation(Proj1 const& parameters1,
                   Proj2 const& parameters2)
        : m_proj1(parameters1)
        , m_proj2(parameters2)
    {}

    template <typename GeometryIn, typename GeometryOut>
    bool forward(GeometryIn const& in, GeometryOut & out) const
    {
        return forward(in, out, transformation_grids<detail::empty_grids_storage>());
    }

    template <typename GeometryIn, typename GeometryOut>
    bool inverse(GeometryIn const& in, GeometryOut & out) const
    {
        return inverse(in, out, transformation_grids<detail::empty_grids_storage>());
    }

    template <typename GeometryIn, typename GeometryOut, typename GridsStorage>
    bool forward(GeometryIn const& in, GeometryOut & out,
                 transformation_grids<GridsStorage> const& grids) const
    {
        BOOST_GEOMETRY_STATIC_ASSERT(
            (projections::detail::same_tags<GeometryIn, GeometryOut>::value),
            "Not supported combination of Geometries.",
            GeometryIn, GeometryOut);

        return projections::detail::transform
                <
                    GeometryOut,
                    calc_t
                >::apply(m_proj1.proj(), m_proj1.proj().params(),
                         m_proj2.proj(), m_proj2.proj().params(),
                         in, out,
                         grids.src_grids,
                         grids.dst_grids);
    }

    template <typename GeometryIn, typename GeometryOut, typename GridsStorage>
    bool inverse(GeometryIn const& in, GeometryOut & out,
                 transformation_grids<GridsStorage> const& grids) const
    {
        BOOST_GEOMETRY_STATIC_ASSERT(
            (projections::detail::same_tags<GeometryIn, GeometryOut>::value),
            "Not supported combination of Geometries.",
            GeometryIn, GeometryOut);

        return projections::detail::transform
                <
                    GeometryOut,
                    calc_t
                >::apply(m_proj2.proj(), m_proj2.proj().params(),
                         m_proj1.proj(), m_proj1.proj().params(),
                         in, out,
                         grids.dst_grids,
                         grids.src_grids);
    }

    template <typename GridsStorage>
    inline transformation_grids<GridsStorage> initialize_grids(GridsStorage & grids_storage) const
    {
        transformation_grids<GridsStorage> result(grids_storage);

        using namespace projections::detail;
        pj_gridlist_from_nadgrids(m_proj1.proj().params(),
                                  result.src_grids);
        pj_gridlist_from_nadgrids(m_proj2.proj().params(),
                                  result.dst_grids);

        return result;
    }

private:
    projections::proj_wrapper<Proj1, CT> m_proj1;
    projections::proj_wrapper<Proj2, CT> m_proj2;
};

} // namespace srs

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_SRS_TRANSFORMATION_HPP


/**
 * Orbit plotter takes a filename and a time. It reads the ephemeris
 * data from the file and constructs orbits consisting of 6 hours
 * prior to and later than the specified time for each satellite
 * in the file, and emits them.
 *
 * Copyright 2013 Bruce Ide
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <boost/bind.hpp>
#include "coordinates.hpp"
#include "ge_object.hpp"
#include <map>
#include "sp3_factory.hpp"
#include <vector>
#include <iomanip>

#ifndef _HPP_ORBIT_PLOTTER
#define _HPP_ORBIT_PLOTTER

namespace fr {

  namespace demo {

    class orbit_plotter {
      const double six_hours = 21600.0;
      const double time_step = 60;

      typedef std::vector<fr::data::sp3_factory::sp3_pair> pair_vector;
      typedef typename pair_vector::iterator pair_vector_iterator;
      typedef std::map<std::string, pair_vector> satellite_map;

      typedef typename satellite_map::iterator satellite_map_iterator;
      satellite_map all_satellites;

      double time_at;
      void populate_map(const std::string &satellite_name, const fr::data::sp3_factory::sp3_pair &line)
      {
	// Only take data from 6 hours prior to six hours after the orbit

	if ((line.first > time_at - six_hours) && (line.first < time_at + six_hours)) {
	  all_satellites[satellite_name].push_back(line);
	}
      }

      // First point and second point are separated by 5 minutes of time
      // in the GPS data files. Interpolate a point every minute for points
      // 1 - 3 (0 and 4 are the real ones.) Note we only write real point 0
      // since next iterator real point 4 will now be real point 0 (and
      // we write the last point in the orbit when we reach the end.)

      void populate_points(const fr::coordinates::ecef_vel &first, const double &time_first, const fr::coordinates::ecef_vel &second, const double &time_second, std::vector<fr::coordinates::lat_long> &points)
      {
	points.push_back(fr::coordinates::converter<fr::coordinates::lat_long>()(first));

	fr::coordinates::tod_eci_vel first_eci = fr::coordinates::converter<fr::coordinates::tod_eci_vel>()(first, time_first);
	fr::coordinates::tod_eci_vel second_eci = fr::coordinates::converter<fr::coordinates::tod_eci_vel>()(second,time_second);

	for(double i = time_first + time_step; i <= time_second - time_step; i += time_step) {
	  fr::coordinates::tod_eci_vel between = first_eci.interpolate(time_first, second_eci, time_second, i);
	  points.push_back(fr::coordinates::converter<fr::coordinates::lat_long>()(between, i));
	}
	// if I push back the second one, I'll have duplicate points in my
	// orbit.
      }

    public:
      orbit_plotter(const std::string &filename, double time_at) : time_at(time_at)
      {
	fr::data::sp3_factory factory(filename);
	factory.data_available.connect(boost::bind(&orbit_plotter::populate_map, this, _1, _2));
	factory.process();
      }

      // Returns a REALLY BIG STRING. This is atrocious, but not really
      // what I'm trying to demonstrate
      std::string plot()
      {
	std::string retval("");
	for(satellite_map_iterator i = all_satellites.begin(); i != all_satellites.end() ; ++i) {
	  std::vector<fr::coordinates::lat_long> orbit;
	  // i->first is satellite name. i->second is a vector of sp3 pairs
	  for (pair_vector_iterator j = i->second.begin(); j != i->second.end(); ++j) {
	    pair_vector_iterator peek_next = j + 1;
	    if (peek_next == i->second.end()) {
	      fr::coordinates::lat_long last = fr::coordinates::converter<fr::coordinates::lat_long>()(j->second);
	      orbit.push_back(last);
	    } else {
	      populate_points(j->second, j->first, peek_next->second, peek_next->first, orbit);
	    }
	  }
	  // Now that we have all the points
	  fr::data::ge_object ge_orbit(orbit);
	  ge_orbit.set_altitude_mode(true);
	  ge_orbit.set_tessellate(true);
	  std::string satname("Satellite ");
	  satname.append(i->first);
	  ge_orbit.set_name(satname);
	  ge_orbit.set_description("12 hour orbit for a GPS satellite");
	  retval.append(ge_orbit.to_string());
	}
	return retval;
      }

    };

  }

}

#endif


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <VisualSIS.h>

#include <exception>
#include <iostream>
#include <utility>

#include <Eigen/Dense>
#include <iCub/ctrl/math.h>
#include <opencv2/core/core.hpp>
#include <opencv2/core/cuda.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/cudaimgproc.hpp>
#include <opencv2/cudawarping.hpp>
#include <yarp/eigen/Eigen.h>
#include <yarp/math/Math.h>
#include <yarp/os/Time.h>

using namespace bfl;
using namespace cv;
using namespace Eigen;
using namespace iCub::ctrl;
using namespace iCub::iKin;
using namespace yarp::eigen;
using namespace yarp::math;
using namespace yarp::os;
using yarp::sig::Vector;
using yarp::sig::ImageOf;
using yarp::sig::PixelRgb;

VisualSIS::VisualSIS(const yarp::os::ConstString& cam_sel,
                     const int img_width, const int img_height,
                     const int num_particles,
                     const double resample_ratio) :
    cam_sel_(cam_sel),
    img_width_(img_width),
    img_height_(img_height),
    num_particles_(num_particles),
    resample_ratio_(resample_ratio)
{
    descriptor_length_ = (img_width_/block_size_*2-1) * (img_height_/block_size_*2-1) * bin_number_ * 4;

    cuda_hog_ = cuda::HOG::create(Size(img_width_, img_height_), Size(block_size_, block_size_), Size(block_size_/2, block_size_/2), Size(block_size_/2, block_size_/2), bin_number_);
    cuda_hog_->setDescriptorFormat(cuda::HOG::DESCR_FORMAT_ROW_BY_ROW);
    cuda_hog_->setGammaCorrection(true);
    cuda_hog_->setWinStride(Size(img_width_, img_height_));

    port_image_in_.open     ("/hand-tracking/" + cam_sel_ + "/img:i");
    port_estimates_out_.open("/hand-tracking/" + cam_sel_ + "/result/estimates:o");

    img_in_.resize(320, 240);
    img_in_.zero();

    setCommandPort();
}

void VisualSIS::initialization()
{
    pred_particle_ = MatrixXf(7, num_particles_);
    pred_weight_   = VectorXf(num_particles_, 1);

    cor_particle_ = MatrixXf(7, num_particles_);
    cor_weight_   = VectorXf(num_particles_, 1);

    estimate_extraction_.clear();

    initialization_->initialize(pred_particle_, pred_weight_);

    prediction_->getExogenousModel().setProperty("ICFW_INIT");

    skip("all", false);
}

VisualSIS::~VisualSIS() noexcept
{
    port_image_in_.close();
    port_estimates_out_.close();
}

void VisualSIS::filteringStep()
{
    std::vector<float> descriptors_cam_left (descriptor_length_);
    cuda::GpuMat       cuda_img             (Size(img_width_, img_height_), CV_8UC3);
    cuda::GpuMat       cuda_img_alpha       (Size(img_width_, img_height_), CV_8UC4);
    cuda::GpuMat       descriptors_cam_cuda (Size(descriptor_length_, 1),   CV_32F );

    ImageOf<PixelRgb>* tmp_imgin = YARP_NULLPTR;
    tmp_imgin = port_image_in_.read(false);
    if (tmp_imgin != YARP_NULLPTR)
    {
        init_img_in_ = true;
        img_in_ = *tmp_imgin;
    }

    if (init_img_in_)
    {
        /* PROCESS CURRENT MEASUREMENT */
        Mat measurement;

        measurement = cvarrToMat(img_in_.getIplImage());
        cuda_img.upload(measurement);
        cuda::resize(cuda_img, cuda_img, Size(img_width_, img_height_));
        cuda::cvtColor(cuda_img, cuda_img_alpha, COLOR_BGR2BGRA, 4);
        cuda_hog_->compute(cuda_img_alpha, descriptors_cam_cuda);
        descriptors_cam_cuda.download(descriptors_cam_left);

        /* PREDICTION */
        if (getFilteringStep() != 0)
            prediction_->predict(cor_particle_, cor_weight_,
                                 pred_particle_, pred_weight_);

        /* CORRECTION */
        correction_->getVisualObservationModel().setProperty("VP_PARAMS");
        correction_->correct(pred_particle_, pred_weight_, descriptors_cam_left,
                             cor_particle_, cor_weight_);
        cor_weight_ /= cor_weight_.sum();

        /* STATE ESTIMATE EXTRACTION FROM PARTICLE SET */
        VectorXf out_particle = estimate_extraction_.extract(cor_particle_, cor_weight_);

        /* RESAMPLING */
        std::cout << "Step: " << getFilteringStep() << std::endl;
        std::cout << "Neff: " << resampling_->neff(cor_weight_) << std::endl;
        if (resampling_->neff(cor_weight_) < std::round(num_particles_ * resample_ratio_))
        {
            std::cout << "Resampling!" << std::endl;

            MatrixXf res_particle(7, num_particles_);
            VectorXf res_weight(num_particles_, 1);
            VectorXf res_parent(num_particles_, 1);

            resampling_->resample(cor_particle_, cor_weight_,
                                  res_particle, res_weight,
                                  res_parent);

            cor_particle_ = res_particle;
            cor_weight_   = res_weight;
        }

        /* STATE ESTIMATE OUTPUT */
        /* INDEX FINGERTIP */
//        Vector q = readRootToEE();
//        icub_kin_arm_.setAng(q.subVector(0, 9) * (M_PI/180.0));
//        Vector chainjoints;
//        if (analogs_) icub_kin_finger_[1].getChainJoints(q.subVector(3, 18), analogs, chainjoints, right_hand_analogs_bounds_);
//        else          icub_kin_finger_[1].getChainJoints(q.subVector(3, 18), chainjoints);
//        icub_kin_finger_[1].setAng(chainjoints * (M_PI/180.0));
//
//        Vector l_ee_t(3);
//        toEigen(l_ee_t) = out_particle.col(0).head(3).cast<double>();
//        l_ee_t.push_back(1.0);
//
//        Vector l_ee_o(3);
//        toEigen(l_ee_o) = out_particle.col(0).tail(3).normalized().cast<double>();
//        l_ee_o.push_back(static_cast<double>(out_particle.col(0).tail(3).norm()));
//
//        yarp::sig::Matrix l_Ha = axis2dcm(l_ee_o);
//        l_Ha.setCol(3, l_ee_t);
//        Vector l_i_x = (l_Ha * (icub_kin_finger_[1].getH(3, true).getCol(3))).subVector(0, 2);
//        Vector l_i_o = dcm2axis(l_Ha * icub_kin_finger_[1].getH(3, true));
//        l_i_o.setSubvector(0, l_i_o.subVector(0, 2) * l_i_o[3]);
//
//
//        Vector r_ee_t(3);
//        toEigen(r_ee_t) = out_particle.col(1).head(3).cast<double>();
//        r_ee_t.push_back(1.0);
//
//        Vector r_ee_o(3);
//        toEigen(r_ee_o) = out_particle.col(1).tail(3).normalized().cast<double>();
//        r_ee_o.push_back(static_cast<double>(out_particle.col(1).tail(3).norm()));
//
//        yarp::sig::Matrix r_Ha = axis2dcm(r_ee_o);
//        r_Ha.setCol(3, r_ee_t);
//        Vector r_i_x = (r_Ha * (icub_kin_finger_[1].getH(3, true).getCol(3))).subVector(0, 2);
//        Vector r_i_o = dcm2axis(r_Ha * icub_kin_finger_[1].getH(3, true));
//        r_i_o.setSubvector(0, r_i_o.subVector(0, 2) * r_i_o[3]);
//
//
//        Vector& estimates_out = port_estimates_out_.prepare();
//        estimates_out.resize(12);
//        estimates_out.setSubvector(0, l_i_x);
//        estimates_out.setSubvector(3, l_i_o.subVector(0, 2));
//        estimates_out.setSubvector(6, r_i_x);
//        estimates_out.setSubvector(9, r_i_o.subVector(0, 2));
//        port_estimates_out_.write();

        /* PALM */
        Vector& estimates_out = port_estimates_out_.prepare();
        estimates_out.resize(7);
        toEigen(estimates_out) = out_particle.cast<double>();
        port_estimates_out_.write();
        /* ********** */
    }
}

void VisualSIS::getResult() { }

bool VisualSIS::attach(yarp::os::Port &source)
{
    return this->yarp().attachAsServer(source);
}

bool VisualSIS::setCommandPort()
{
    std::cout << "Opening RPC command port." << std::endl;
    if (!port_rpc_command_.open("/hand-tracking/" + cam_sel_ + "/cmd:i"))
    {
        std::cerr << "Cannot open the RPC command port." << std::endl;
        return false;
    }
    if (!attach(port_rpc_command_))
    {
        std::cerr << "Cannot attach the RPC command port." << std::endl;
        return false;
    }
    std::cout << "RPC command port opened and attached. Ready to recieve commands!" << std::endl;

    return true;
}

bool VisualSIS::run_filter()
{
    run();

    return true;
}

bool VisualSIS::reset_filter()
{
    reset();

    return true;
}

bool VisualSIS::stop_filter()
{
    reboot();

    return true;
}

bool VisualSIS::skip_step(const std::string& what_step, const bool status)
{
    return skip(what_step, status);
}

bool VisualSIS::use_analogs(const bool status)
{
    if (status)
        return correction_->getVisualObservationModel().setProperty("VP_ANALOGS_ON");
    else
        return correction_->getVisualObservationModel().setProperty("VP_ANALOGS_OFF");
}

std::vector<std::string> VisualSIS::get_info()
{
    std::vector<std::string> info;

    info.push_back("<| Information about Visual SIR Particle Filter |>");
    info.push_back("<| The Particle Filter is " + std::string(isRunning() ? "not " : "") + "running |>");
    info.push_back("<| Filtering step: " + std::to_string(getFilteringStep()) + " |>");
    info.push_back("<| Using " + cam_sel_ + " camera images |>");
    info.push_back("<| Using " + std::to_string(num_particles_) + " particles |>");

    std::vector<std::string> est_ext_info = estimate_extraction_.getInfo();

    info.insert(info.end(), est_ext_info.begin(), est_ext_info.end());

    return info;
}

bool VisualSIS::set_estimates_extraction_method(const std::string& method)
{
    if (method == "mean")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::mean);

        return true;
    }
    else if (method == "smean")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::smean);

        return true;
    }
    else if (method == "wmean")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::wmean);

        return true;
    }
    else if (method == "emean")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::emean);

        return true;
    }
    else if (method == "mode")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::mode);

        return true;
    }
    else if (method == "smode")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::smode);

        return true;
    }
    else if (method == "wmode")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::wmode);

        return true;
    }
    else if (method == "emode")
    {
        estimate_extraction_.setMethod(EstimatesExtraction::ExtractionMethod::emode);

        return true;
    }

    return false;
}

bool VisualSIS::set_mobile_average_window(const int16_t window)
{
    if (window > 0)
        return estimate_extraction_.setMobileAverageWindowSize(window);
    else
        return false;
}

bool VisualSIS::quit()
{
    return teardown();
}


/*! \file fm2dir.hpp
    \brief Templated class which computes the Fast Marching Directional (FM2Directional).

    It uses as a main container the nDGridMap class. The nDGridMap type T
    has to be an FMDirectionalCell or something inherited from it.

    The leafsize of the grid map is ignored since it has to be > = 1 and that
    depends on the units employed.

    The type of the heap introduced is very important for the behaviour of the
    algorithm. The following heaps are provided:

    - FMDaryHeap wrap for the Boost D_ary heap (generalization of binary heaps).
      Set by default if no other heap is specified. The arity has been set to 2
      (binary heap) since it has been tested to be the more efficient in this algorithm.
    - FMFibHeap wrap for the Boost Fibonacci heap.
    - FMPriorityQueue wrap to the std::PriorityQueue class. This heap implies the implementation
      of the Simplified FMM (SFMM) method, done automatically because of the FMPriorityQueue::increase implementation.

    Copyright (C) 2014 Javier V. Gomez and Jose Pardeiro
    www.javiervgomez.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.
    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 General Public License for more details.
    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.*/

#ifndef FM2DIR_H_
#define FM2DIR_H_

#include <iostream>
#include <cmath>
#include <algorithm>
#include <numeric>
#include <fstream>
#include <array>
#include <limits>

#include <boost/math/constants/constants.hpp>

#include "../fmm/fmdata/fmdirectionalcell.h"
#include "../fm2/fm2.hpp"
#include "../gradientdescent/gradientdescent.hpp"

#define PI boost::math::constants::pi<double>()

// TODO: include suppoert to other solvers (GMM, FIM, UFMM). It requires a better way of setting parameters.

template < class grid_t, class heap_t = FMDaryHeap<FMCell> >  class FM2Dir : public FM2<grid_t, heap_t> {

    typedef std::vector< std::array< double, grid_t::getNDims() > > path_t;
    typedef FM2<grid_t, heap_t > FM2Base;

    public:

    FM2Dir
    (const std::string& name = "FM2Dir") : FM2Base(name) { }

        /**
         * Sets the input grid in which operations will be performed.
         *
         * @param g input grid map.
         */
        virtual void setEnvironment
        (grid_t * g) {
            grid_ = g;
            narrow_band_.setMaxSize(grid_->size());
            ndims_ = grid_->getNDims();
        }

        /**
         * Sets the initial points by the indices in the nDGridMap and
         * computes the initialization of the Fast Marching Square calling
         * the init() function.
         *
         * @param initial_point contains the index of the initial point of the query.
         *
         * @param fmm2_sources contains the indices of the initial points corresponding to all black cells.
         *
         * @param goal_idx contains the index of the goal point.
         *
         * @see init()
         */

        virtual void setInitialAndGoalPoints
        (const std::vector <int> & initial_point, const std::vector <int> & fmm2_sources, const int goal_idx) {
            initial_point_ = initial_point;
            fmm2_sources_ = fmm2_sources;
            goal_idx_ = goal_idx;
        }

        /**
         * Sets the initial points by the indices in the nDGridMap and
         * computes the initialization of the Fast Marching Method calling
         * the init() function.
         *
         * @param init_points contains the indices of the init points.
         *
         * @param save_velocity selects if the velocity profile must be saved
         *
         * @see init()
         */
        virtual void setInitialPoints
        (const std::vector<int> & init_points, const bool save_velocity = false) {
            init_points_ = init_points;
            for (const int &i: init_points) {
            grid_->getCell(i).setArrivalTime(0);
            grid_->getCell(i).setDirectionalTime(0);
            grid_->getCell(i).setState(FMState::FROZEN);
            }

            if (init_points.size() > 1)
                init();
            else
                init(save_velocity, true);
        }

        /**
        * Internal function although it is set to public so it can be accessed if desired.
        *
        * Computes the Fast Marching Method initialization from the initial points given. Programmed following the paper:
          A. Valero, J.V. Gómez, S. Garrido and L. Moreno, The Path to Efficiency: Fast Marching Method for Safer,
          More Efficient Mobile Robot Trajectories, IEEE Robotics and Automation Magazine, Vol. 20, No. 4, 2013.
        *
        * @param save_velocity selects if the velocity profile must be saved
        *
        * @param directional selects if directional heuristic must be applied
        *
        * @see setInitialPoints()
        */
        virtual void init
        (const bool save_velocity = false, const bool directional = false) {
            // TODO: neighbors computed twice for every cell. We can save time here.
            // TODO: check if the previous steps have been done (loading grid map and setting initial points.)
            int j = 0;
            int n_neighs = 0;
            for (int &i: init_points_) { // For each initial point
            n_neighs = grid_->getNeighbors(i, neighbors);
                for (int s = 0; s < n_neighs; ++s){  // For each neighbor
                    j = neighbors[s];
                    if ((grid_->getCell(j).getState() ==  FMState::FROZEN) || grid_->getCell(j).isOccupied() || grid_->getCell(j).getVelocity() ==  0) // If Frozen or obstacle
                        continue;
                    else {
                        double new_arrival_time = solveEikonal(j);
                        double dir_time = 0;

                        if (directional ==  true)
                            dir_time = new_arrival_time;

                        if (grid_->getCell(j).getState() ==  FMState::NARROW) { // Updating narrow band if necessary.
                            if (new_arrival_time < grid_->getCell(j).getArrivalTime()) {
                                grid_->getCell(j).setArrivalTime(new_arrival_time);

                            if (save_velocity)
                                velocity_map_[j] = vel;

                            if (directional)
                                grid_->getCell(j).setDirectionalTime(dir_time);
                            narrow_band_.increase( &(grid_->getCell(j))  ) ;
                            }
                        }
                        else {
                            grid_->getCell(j).setState(FMState::NARROW);
                            grid_->getCell(j).setArrivalTime(new_arrival_time);

                            if (save_velocity)
                                velocity_map_[j] = vel;

                            if (directional ==  true)
                                grid_->getCell(j).setDirectionalTime(dir_time);
                            narrow_band_.push( &(grid_->getCell(j)) );
                        } // neighbors open.
                    } // neighbors not frozen.
                } // For each neighbor.
            } // For each initial point.
        } // init()

        //IMPORTANT NOTE: Assuming inc(1) = inc(y)  = ... =  leafsize_
        // Possible improvement: If we include the neighbors in the cells information
        // this could be (most probably) speeded up.
        // This implementation is focused to be used with any number of dimensions.

        /**
        * Solves the Eikonal equation for a given cell using the heuristic criteria of the FM2 Directional.
        * This function is generalized to any number of dimensions.
        *
        * @param idx index of the cell to be evaluated.
        *
        * @param idx_source index of the source cell of the wave. If this value is -1 the heuristic is not applied
        *
        * @return the distance (or time of arrival) value.
        */
        virtual double solveEikonal
        (const int & idx, const int & idx_source = -1) {
            // TODO: Here neighbors are computed and then in the computeFM. There should be a way to avoid computing
            // neighbors twice.

            int a = grid_t::getNDims(); // a parameter of the Eikonal equation.

            double updatedT;
            sumT = 0;
            sumTT = 0;
            double minTInDim = 0;

            vel = 0;

            if (idx_source == -1) {
                for (int dim = 0; dim < grid_t::getNDims(); ++dim) {
                    minTInDim = grid_->getMinValueInDim(idx, dim);
                    if (!isinf(minTInDim)) {
                        Tvalues[dim] = minTInDim;
                        sumT +=  Tvalues[dim];
                        TTvalues[dim] = Tvalues[dim]*Tvalues[dim];
                        sumTT +=  TTvalues[dim];
                    }
                    else {
                        Tvalues[dim] = 0;
                        TTvalues[dim] = 0;
                        a -= 1 ;
                    }
                }

                vel = grid_->getCell(idx).getVelocity();
            }
            else {

                for (int dim = 0; dim < grid_t::getNDims(); ++dim) {
                    minTInDim = getMinValueInDimDirectional(idx, dim);
                    if (!isinf(minTInDim)) {
                        Tvalues[dim] = minTInDim;
                        sumT +=  Tvalues[dim];
                        TTvalues[dim] = Tvalues[dim]*Tvalues[dim];
                        sumTT +=  TTvalues[dim];
                    }
                    else {
                        Tvalues[dim] = 0;
                        TTvalues[dim] = 0;
                        a -= 1 ;
                    }
                }

                if (grid_->getCell(idx).getVelocity() < grid_->getCell(idx_source).getVelocity() && grid_->getCell(idx).getVelocity() > 0.05)
                    vel = 1;
                else
                    vel = grid_->getCell(idx).getVelocity();
            }

            double b = -2*sumT;
            double c = sumTT - grid_->getLeafSize() * grid_->getLeafSize()/(vel * vel); // leafsize not taken into account here.
            double quad_term = b*b - 4*a*c;
            if (quad_term < 0) {
                double minT = *(std::min_element(Tvalues.begin(), Tvalues.end()));
                updatedT = 1/(vel * vel) + minT; // leafsize not taken into account here.
            }
            else
                updatedT = (-b + sqrt(quad_term))/(2*a);

            return updatedT;
        }

        /**
         * Main Fast Marching Function. It requires to call first the setInitialPoints() function.
         *
         * @param stop selects if the wave has to stop when it arrives to the goal point
         *
         * @param directional selects if directional heuristic must be applied
         *
         * @see setInitialPoints()
         */

        virtual void computeFM
        (const bool stop = true, const bool directional = false) {
            // TODO: check if the previous steps have been done (initialization).
            int j =  0;
            int n_neighs = 0;
            bool stopWavePropagation = 0;

            while (narrow_band_.size() > 0 && stopWavePropagation ==  0) {
                int idxMin = narrow_band_.popMinIdx();
                n_neighs = grid_->getNeighbors(idxMin, neighbors);
                grid_->getCell(idxMin).setState(FMState::FROZEN);

                for (int s = 0; s < n_neighs; ++s) {
                    j = neighbors[s];
                    if ((grid_->getCell(j).getState() ==  FMState::FROZEN) || grid_->getCell(j).isOccupied()) // If Frozen or obstacle
                        continue;
                    else {
                        double new_arrival_time = solveEikonal(j);
                        double dir_time = 0;

                        if (directional ==  true)
                            dir_time = solveEikonal(j, idxMin);

                        if (grid_->getCell(j).getState() ==  FMState::NARROW) { // Updating narrow band if necessary.
                            if (new_arrival_time < grid_->getCell(j).getArrivalTime()) {
                                grid_->getCell(j).setArrivalTime(new_arrival_time);
                                narrow_band_.increase( &(grid_->getCell(j)) );
                                velocity_map_[j] = vel;
                            }

                            if (directional)
                                if (dir_time < grid_->getCell(j).getDirectionalTime())
                                    grid_->getCell(j).setDirectionalTime(dir_time);
                        }
                        else {
                            grid_->getCell(j).setState(FMState::NARROW);
                            grid_->getCell(j).setArrivalTime(new_arrival_time);
                            velocity_map_[j] = vel;

                            if (directional)
                                grid_->getCell(j).setDirectionalTime(dir_time);
                            narrow_band_.push( &(grid_->getCell(j)) );
                        } // neighbors open.
                    } // neighbors not frozen.
                    if (idxMin ==  initial_point_[0] && stop)
                        stopWavePropagation = 1;
                } // For each neighbor.
            } // while narrow band not empty
        }

        /**
         * Main Fast Marching Square Directional Function with velocity saturation. It requires to call first the setInitialPoints() function.
         *
         * @param maxDistance saturation distance (relative, where 1 means maximum distance). If this value is -1 (default) the velocities map is not saturated.
         *
         * @see setInitialPoints()
         */
        virtual void computeFM2Directional
        (const float maxDistance = -1) {
            maxDistance_ = maxDistance;

            velocity_map_.resize(grid_->size());
            if (maxDistance_ != -1)
                computeVelocitiesMap(true);
            else
                computeVelocitiesMap();

            // According to the theoretical basis the wave is expanded from the goal point to the initial point.
            std::vector <int> wave_init;
            wave_init.push_back(goal_idx_);
            setInitialPoints(wave_init, true);
            computeFM(true, true);
        }

        /**
         * Computes the path from the given index to a minimum (the one
         * gradient descent choses) and returns the velocity. According to
         * the theoretical basis the wave is expanded from the goal point
         * to the initial point. For these reasons the gradient must to be
         * applied from the initial point.
         *
         * No checks are done (points in the borders, points in obstacles...).
         *
         * The included scripts will parse the saved path.
         *
         * @param path the resulting path (output).
         *
         * @param velocity the resulting path (output).
         */
        virtual void computePath
        (path_t * p, std::vector <double> * path_velocity) {
            path_t* path_ = p;
            constexpr int ndims = grid_t::getNDims();

            GradientDescent< nDGridMap<FMDirectionalCell, ndims> > grad;
            grad.apply_directional(*grid_,initial_point_[0],*path_, velocity_map_, *path_velocity);
        }

        /**
        * For a cell with index idx, obtains the minimum value of the neigbours in dimension dim looking the
        * DirectionalTime value.
        *
        * @param idx index of the cell accessed.
        *
        * @param dim dimension in which the minimum is examinated (0: x, 1: y, 2: z, etc).
        *
        * @return the corresponding minimum value.
        * */
        double getMinValueInDimDirectional
        (const int idx, const int dim) {
            // n_neighs = 0; // How many neighbors obtained in that dimension.
            constexpr int ndims = grid_->getNDims();
            std::array<int, ndims> n;
            int n_neighs = grid_->getNumberNeighborsInDim(idx,n,dim);

            if (n_neighs ==  1)
                return grid_->getCell(n[0]).getDirectionalTime();
            else
                return (grid_->getCell(n[0]).getDirectionalTime()<grid_->getCell(n[1]).getDirectionalTime()) ? grid_->getCell(n[0]).getDirectionalTime() : grid_->getCell(n[1]).getDirectionalTime();
        }

    private:

        /**
         * Computes the velocities map of the FM2 algorithm.
         *
         * @param saturate select if the potential is saturated according to maxDistance_ .
         */

        void computeVelocitiesMap
        (bool saturate = false) {
            setInitialPoints(fmm2_sources_);
            computeFM(false);

            //Rescaling and saturating to relative velocities: [0-1]
            double maxValue = grid_->getMaxValue();
            double maxVelocity = 0;

            if (saturate)
                maxVelocity = maxDistance_ / grid_->getLeafSize();

            for (int i = 0; i < grid_->size(); i++) {
                double velocity = grid_->getCell(i).getValue() / maxValue;

                if (saturate)
                    if (velocity < maxVelocity)
                        grid_->getCell(i).setVelocity(velocity / maxVelocity);
                    else
                        grid_->getCell(i).setVelocity(1);
                else
                    grid_->getCell(i).setVelocity(velocity);

              // Restarting grid values for second wave expasion.
              grid_->getCell(i).setValue(std::numeric_limits<double>::infinity());
              grid_->getCell(i).setDirectionalTime(std::numeric_limits<double>::infinity());
              grid_->getCell(i).setState(FMState::OPEN);
            }
        }

    protected:
        using FMM<grid_t, heap_t>::grid_;
        using FMM<grid_t, heap_t>::neighbors;
        using FMM<grid_t, heap_t>::init_points_;
        using FMM<grid_t, heap_t>::Tvalues;
        using FMM<grid_t, heap_t>::TTvalues;
        using FMM<grid_t, heap_t>::narrow_band_;

    private:
        double sumT; /*!< Auxiliar value wich computes T1+T2+T3... Useful for generalizing the Eikonal solver. */
        double sumTT; /*!< Auxiliar value wich computes T1^2+T2^2+T3^2... Useful for generalizing the Eikonal solver. */
        double sumDistance; /*!< Auxiliar value wich computes euclidean distance between narrow band and goal point. Useful for generalizing the Eikonal solver with euristic. */

        double vel; /*!< Auxiliar value wich contains the velocity of the cell used on the Eikonal solver. */
        double maxDistance_; /*!< Distance value to saturate the first potential. */

        int goal_idx_; /*!< Goal point for the Fast Marching Square Star. */
        std::vector<int> fmm2_sources_;	/*!< Wave propagation sources for the Fast Marching Square Star. */
        std::vector<int> initial_point_;	/*!< Initial point for the Fast Marching Square Star. */

        std::array<int, grid_t::getNDims()-1> d_;
        std::array<int, grid_t::getNDims()> dimsize_;
        std::vector<double> velocity_map_; /*!< Auxiliar vector which contains the final velocity map of the environment. */

        int ndims_;
        double angle_wave, angle_velocity;
        double diff_angle;
};

#endif /* FM2DIR_H_*/


/*
   Copyright 2019 Ethereum Foundation

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
*/

#include <iostream>

#include <boost/date_time/posix_time/posix_time.hpp>

#include "dust_generator.hpp"
#include "keccak.hpp"
#include "memdb_bucket.hpp"
#include "miner.hpp"

using namespace silkworm;

static const auto kInitialAccounts = 10'000'000;
static const auto kNewAccountsPerBlock = 300;
static const auto kBlockTime = 15;             // sec
static const auto kBandwidth = 1'000'000 / 8;  // bytes per sec

void print_hints(const sync::Hints& hints) {
  static constexpr double kKibibyte = 1024;
  static constexpr double kMebibyte = 1024 * 1024;

  std::cout << "Hints for " << hints.num_leaves * 1e-6
            << "M dust accounts with reply size <~ "
            << hints.approx_max_reply_size / kKibibyte << " KiB:\n";
  const auto mem_depth = hints.depth_to_fit_in_memory();
  std::cout << "depth to fit in memory " << static_cast<int>(mem_depth) << " ("
            << std::setprecision(3)
            << hints.tree_size_in_bytes(mem_depth) / kMebibyte << " MiB)\n";
  const auto d1 = hints.optimal_phase1_depth();
  std::cout << "optimal phase 1 depth  " << static_cast<int>(d1) << std::endl;
  const auto d2 = hints.optimal_phase2_depth();
  std::cout << "optimal phase 2 depth  " << static_cast<int>(d2) << std::endl;
  std::cout << "overhead (∞ bandwidth) " << std::setprecision(2)
            << hints.inf_bandwidth_reply_overhead() * 100 << "%\n";
  std::cout << "RQS                    " << std::setprecision(2)
            << hints.rqs(d2) / kMebibyte << " MiB \n\n";
}

int main() {
  using namespace silkworm::lab;
  using namespace boost::posix_time;

  static const auto kStartBlock = 7212230u;
  static const auto kSeed = 3548264823u;

  sync::Hints hints;
  hints.changes_per_block = kNewAccountsPerBlock;
  print_hints(hints);

  hints.num_leaves = kInitialAccounts;
  print_hints(hints);

  const auto time0 = microsec_clock::local_time();
  MemDbBucket miner_state("miner_state");
  RNG rng(kSeed);
  DustGenerator dust_gen(rng);

  // create random dust accounts
  int account_num = 0;

  miner_state.put(
      [&dust_gen, &account_num]() -> std::optional<DbBucket::KeyVal> {
        if (account_num >= kInitialAccounts) {
          return {};
        }
        if (account_num % 1'000'000 == 0) {
          std::cout << account_num / 1'000'000 << "M accounts generated"
                    << std::endl;
        }
        ++account_num;

        Hash key = keccak(byte_view(dust_gen.random_address()));
        std::string val = to_rlp(dust_gen.random_account());
        return std::pair{byte_view(key), val};
      });

  Miner miner(miner_state, hints, kStartBlock);
  const auto time1 = microsec_clock::local_time();
  std::cout << "Dust accounts generated in " << time1 - time0 << "\n\n";

  MemDbBucket leecher_state("leecher_state");
  Node leecher(leecher_state, hints, {});
  sync::Stats stats;
  auto new_blocks = 0;
  auto generated_leaves = kInitialAccounts;

  while (true) {
    std::cout << "new block " << new_blocks << " phase "
              << (leecher.phase1_sync_done() + 1) << std::endl;

    leecher.sync(miner, stats, kBandwidth * kBlockTime);

    std::cout << "leaves received " << stats.reply_total_leaves
              << " vs generated " << generated_leaves << std::endl;

    if (leecher.sync_done()) {
      break;
    }

    miner.new_block();
    for (int i = 0; i < kNewAccountsPerBlock; ++i) {
      miner.create_account(dust_gen.random_address(),
                           dust_gen.random_account());
    }
    miner.seal_block();

    generated_leaves += kNewAccountsPerBlock;
    ++new_blocks;
  }

  std::cout << "\nSync done? " << std::boolalpha << leecher.sync_done()
            << "\n\n";

  const auto time2 = microsec_clock::local_time();
  const auto emulated_time = seconds((new_blocks + 1) * kBlockTime);

  std::cout << "CPU time            " << time2 - time1 << std::endl;
  std::cout << "Emulated time       " << emulated_time << std::endl;
  std::cout << "#new blocks         " << new_blocks << std::endl;
  std::cout << "#requests           " << stats.num_requests << std::endl;
  std::cout << "request total bytes " << stats.request_total_bytes << std::endl;
  std::cout << "#replies            " << stats.num_replies << std::endl;
  std::cout << "reply total bytes   " << stats.reply_total_bytes << std::endl;
  std::cout << "reply total leaves  " << stats.reply_total_leaves << std::endl;
  std::cout << "reply total nodes   " << stats.reply_total_nodes << std::endl;
  std::cout << "generated leaves    " << generated_leaves << std::endl;

  const auto leaf_bytes =
      static_cast<double>(generated_leaves * sync::kLeafSize);
  const auto overhead = stats.reply_total_bytes / leaf_bytes - 1;
  std::cout << "reply overhead      " << std::setprecision(2) << overhead * 100
            << "%\n\n";

  const bool same = miner_state.has_same_data(leecher_state);

  if (same) {
    std::cout << "Sync verified 😅\n";
  } else {
    std::cout << "Epic Fail 🤬\n";
  }

  // TODO multiple leechers
  // a) spawn leechers in separate threads
  // b) keep mining new blocks
  // c) wait for leechers to finish sync
}