/*ckwg +29
 * Copyright 2015 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * \file
 * \brief core homography template implementations
 */

#include "homography.h"

#include <cmath>

#include <maptk/exceptions/math.h>
#include <maptk/logging_macros.h>

#include <Eigen/LU>

namespace maptk
{

namespace //anonymous
{

/// Private helper method for point transformation via homography matrix
template <typename T>
Eigen::Matrix<T,2,1>
h_map_point( Eigen::Matrix<T,3,3> const &h, Eigen::Matrix<T,2,1> const &p )
{
  Eigen::Matrix<T,3,1> out_pt = h * Eigen::Matrix<T,3,1>(p[0], p[1], 1.0);
  if( fabs(out_pt[2]) <= Eigen::NumTraits<T>::dummy_precision() )
  {
    throw point_maps_to_infinity();
  }
  return Eigen::Matrix<T,2,1>( out_pt[0] / out_pt[2], out_pt[1] / out_pt[2] );
}

} // end anonymous namespace

/// Construct an identity homography
template <typename T>
homography_<T>
::homography_()
  : h_( matrix_t::Identity() )
{
}

/// Construct from a provided transformation matrix
template <typename T>
homography_<T>
::homography_( Eigen::Matrix<T,3,3> const &mat )
  : h_( mat )
{
}

/// Conversion Copy constructor -- float specialization
template <>
template <>
homography_<float>
::homography_( homography_<float> const &other )
  : h_( other.get_matrix() )
{
}

/// Conversion Copy constructor -- double specialization
template <>
template <>
homography_<double>
::homography_( homography_<double> const &other )
  : h_( other.get_matrix() )
{
}

/// Construct from a generic homography
template <typename T>
homography_<T>
::homography_( homography const &base )
  : h_( base.matrix().template cast<T>() )
{
}

/// Construct from a generic homography -- double specialization
template <>
homography_<double>
::homography_( homography const &base )
  : h_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template <typename T>
homography_sptr
homography_<T>
::clone() const
{
  return homography_sptr( new homography_<T>( *this ) );
}

/// Get a double-typed copy of the underlying matrix transformation
template <typename T>
Eigen::Matrix<double,3,3>
homography_<T>
::matrix() const
{
  return this->h_.template cast<double>();
}

/// Specialization for homographies with native double type
template <>
Eigen::Matrix<double,3,3>
homography_<double>
::matrix() const
{
  return this->h_;
}

/// Normalize homography transformation in-place
template <typename T>
homography_sptr
homography_<T>
::normalize() const
{
  matrix_t norm = this->get_matrix();
  if( fabs(norm(2,2)) >= Eigen::NumTraits<T>::dummy_precision() )
  {
    norm /= norm(2,2);
  }
  return homography_sptr( new homography_<T>( norm ) );
}

/// Inverse the homography transformation returning a new transformation
template <typename T>
homography_sptr
homography_<T>
::inverse() const
{
  matrix_t inv;
  bool isvalid;
  this->h_.computeInverseWithCheck( inv, isvalid );
  if( !isvalid )
  {
    throw non_invertible_matrix();
  }
  return homography_sptr( new homography_<T>( inv ) );
}

/// Map a 2D double-type point using this homography
template <typename T>
Eigen::Matrix<double,2,1>
homography_<T>
::map( Eigen::Matrix<double,2,1> const &p ) const
{
  // Explicitly refer to templated version of method so as to not infinitely
  // recurse.
  Eigen::Matrix<double,3,3> m = h_.template cast<double>();
  return h_map_point( m, p );
}

/// Map a 2D double-type point using this homography -- double specialization
template <>
Eigen::Matrix<double,2,1>
homography_<double>
::map( Eigen::Matrix<double,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Get the underlying matrix transformation
template <typename T>
typename homography_<T>::matrix_t&
homography_<T>
::get_matrix()
{
  return this->h_;
}

/// Get a const new copy of the underlying matrix transformation.
template <typename T>
typename homography_<T>::matrix_t const&
homography_<T>
::get_matrix() const
{
  return this->h_;
}

/// Map a 2D point using this homography -- generic version
template <typename T>
Eigen::Matrix<T,2,1>
homography_<T>
::map_point( Eigen::Matrix<T,2,1> const &p ) const
{
  return h_map_point<T>( h_.template cast<T>(), p );
}

/// Map a 2D point using this homography -- float specialization
template <>
Eigen::Matrix<float,2,1>
homography_<float>
::map_point( Eigen::Matrix<float,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Map a 2D point using this homography -- double specialization
template <>
Eigen::Matrix<double,2,1>
homography_<double>
::map_point( Eigen::Matrix<double,2,1> const &p ) const
{
  return h_map_point( h_, p );
}

/// Custom f2f_homography multiplication operator.
template <typename T>
homography_<T>
homography_<T>
::operator*( homography_<T> const &rhs )
{
  return homography_<T>( h_ * rhs.h_ );
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// homography_<T> output stream operator
template <typename T>
std::ostream&
operator<<( std::ostream &s, homography_<T> const &h )
{
  s << h.get_matrix();
  return s;
}

/// Output stream operator for \p homography instances
std::ostream&
operator<<( std::ostream &s, homography const &h )
{
  s << h.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_HOMOGRAPHY(T) \
  template class homography_<T>; \
  template std::ostream& operator<<( std::ostream &, \
                                     homography_<T> const & )

INSTANTIATE_HOMOGRAPHY(float);
INSTANTIATE_HOMOGRAPHY(double);
#undef INSTANTIATE_HOMOGRAPHY
/// \endcond

} // end maptk namespace


/*
 * Copyright (c) 2018 Samsung Electronics Co., Ltd. All Rights Reserved
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

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

#include <iostream>
#include <vector>
#include <cassert>

#include <Eigen/Core>
#include <gemmlowp.h>

#include "types.h"
#include "common.h"
#include "optimized_ops.h"
#include "OperationUtils.h"

#include <arm_compute/graph.h>

#include <arm_compute/runtime/CL/CLFunctions.h>
#include <arm_compute/runtime/CL/functions/CLConvolution.h>

#include "io_accessor.h"
#include "util/environment.h"

static constexpr int kStaticBufferSize = 1605632;
static char static_scratch_buffer[kStaticBufferSize];

#define ANDROID_NN_CONV_PARAMETERS(Type)                                        \
    uint32_t height       = getSizeOfDimension(inputShape, 1);                  \
    uint32_t width        = getSizeOfDimension(inputShape, 2);                  \
    uint32_t filterHeight = getSizeOfDimension(filterShape, 1);                 \
    uint32_t filterWidth  = getSizeOfDimension(filterShape, 2);                 \
    uint32_t outHeight    = getSizeOfDimension(outputShape, 1);                 \
    uint32_t outWidth     = getSizeOfDimension(outputShape, 2);                 \
    uint32_t inDepth      = getSizeOfDimension(inputShape, 3);                  \
                                                                                \
    uint32_t paddingHeight = (uint32_t)padding_top;                             \
    uint32_t paddingWidth = (uint32_t)padding_left;                             \
                                                                                \
    Dims<4> im2colDim;                                                          \
    im2colDim.sizes[3] = (int)getSizeOfDimension(outputShape, 0);               \
    im2colDim.sizes[2] = (int)getSizeOfDimension(outputShape, 1);               \
    im2colDim.sizes[1] = (int)getSizeOfDimension(outputShape, 2);               \
    im2colDim.sizes[0] = (int)inDepth * filterHeight * filterWidth;             \
                                                                                \
    im2colDim.strides[0] = 1;                                                   \
    for (int i=1; i<4; i++) {                                                   \
        im2colDim.strides[i] = im2colDim.strides[i-1] * im2colDim.sizes[i-1];   \
    }                                                                           \
                                                                                \
    Type* im2colData = nullptr;                                                 \
    int im2colByteSize = sizeof(Type);                                          \
    for (int i=0; i<4; i++) {                                                   \
        im2colByteSize *= im2colDim.sizes[i];                                   \
    }                                                                           \
    if (im2colByteSize <= kStaticBufferSize) {                                  \
        im2colData = reinterpret_cast<Type *>(static_scratch_buffer);           \
    } else {                                                                    \
        im2colData = new (std::nothrow) Type[im2colByteSize / sizeof(Type)];    \
    }

bool convFloat32(const float* inputData, const Shape& inputShape,
                 const float* filterData, const Shape& filterShape,
                 const float* biasData, const Shape& biasShape,
                 int32_t padding_left, int32_t padding_right,
                 int32_t padding_top, int32_t padding_bottom,
                 int32_t stride_width, int32_t stride_height,
                 int32_t activation,
                 float* outputData, const Shape& outputShape) {

    ANDROID_NN_CONV_PARAMETERS(float)

    #define ANDROID_NN_CONV(activation)                                        \
        Conv<FusedActivationFunctionType::activation>(                         \
            inputData, convertShapeToDims(inputShape),                         \
            filterData, convertShapeToDims(filterShape),                       \
            biasData, convertShapeToDims(biasShape),                           \
            stride_width, stride_height, paddingWidth, paddingHeight,          \
            outputData, convertShapeToDims(outputShape),                       \
            im2colData, im2colDim)

    ANDROID_NN_MACRO_DISPATCH(ANDROID_NN_CONV)

    #undef ANDROID_NN_CONV

    if (im2colByteSize > kStaticBufferSize) {
        delete[] im2colData;
    }
    return true;
}

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

using arm_compute::DataType;
using arm_compute::graph::Target;
using arm_compute::graph::TensorDescriptor;
using arm_compute::TensorShape;
using arm_compute::graph::frontend::InputLayer;
using arm_compute::graph::frontend::OutputLayer;

namespace acl_graph {

bool convFloat32(const float* inputData, const Shape& inputShape,
                 const float* filterData, const Shape& filterShape,
                 const float* biasData, const Shape& biasShape,
                 int32_t padding_left, int32_t padding_right,
                 int32_t padding_top, int32_t padding_bottom,
                 int32_t stride_width, int32_t stride_height,
                 int32_t activation,
                 float* outputData, const Shape& outputShape)
{
  // Try with simple build-run with ACL Layer
  arm_compute::graph::frontend::Stream graph{0, "ACL_CONV2D_TEST"};

  Target target_hint = nnfw::util::get_env_int("NNFW_ACL_USENEON")
                           ? Target::NEON : Target::CL;

  // Not sure about which index is which value
  uint32_t tsi_c = getSizeOfDimension(inputShape, 0);
  uint32_t tsi_h = getSizeOfDimension(inputShape, 1);
  uint32_t tsi_w = getSizeOfDimension(inputShape, 2);
  uint32_t tsi_n = getSizeOfDimension(inputShape, 3);

  uint32_t tsk_h = getSizeOfDimension(filterShape, 1);
  uint32_t tsk_w = getSizeOfDimension(filterShape, 2);
  uint32_t tsk_n = getSizeOfDimension(filterShape, 3);

  graph << target_hint
        << InputLayer(TensorDescriptor(TensorShape(tsi_w, tsi_h, tsi_c, tsi_n), DataType::F32),
                  std::unique_ptr<InputAccessor>(new InputAccessor(inputData, inputShape)))
        << arm_compute::graph::frontend::ConvolutionLayer(
              tsk_w, tsk_h, tsk_n,
              std::unique_ptr<WeightAccessor>(new WeightAccessor(filterData, filterShape)),
              std::unique_ptr<BiasAccessor>(new BiasAccessor(biasData, biasShape)),
              arm_compute::PadStrideInfo(stride_width, stride_height, padding_top, padding_bottom))
        ;
  if (activation != static_cast<int32_t>(FusedActivationFunc::NONE)) {
    arm_compute::ActivationLayerInfo::ActivationFunction actFunc =
        arm_compute::ActivationLayerInfo::ActivationFunction::RELU;

    graph << arm_compute::graph::frontend::ActivationLayer(arm_compute::ActivationLayerInfo(actFunc));
    // Activation does not provide output Tensor and makes next layer fail to add to graph
    // when it's the last(output) layer. To solve this, need to add a dummy layer.
    uint32_t tso_c = getSizeOfDimension(outputShape, 0);
    uint32_t tso_h = getSizeOfDimension(outputShape, 1);
    uint32_t tso_w = getSizeOfDimension(outputShape, 2);
    uint32_t tso_n = getSizeOfDimension(outputShape, 3);
    graph << arm_compute::graph::frontend::ReshapeLayer(TensorShape(tso_w, tso_h, tso_c, tso_n));
  }
  graph << OutputLayer(std::unique_ptr<OutputAccessor>(new OutputAccessor(outputData, outputShape)))
        ;

  graph.run();

  return true;
}

} // namespace acl_graph

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

using arm_compute::TensorInfo;

namespace acl_runtime {

TensorShape calculate_convolution_layer_output_shape(
    const arm_compute::TensorShape &input_shape,
    const arm_compute::TensorShape &weights_shape,
    const arm_compute::PadStrideInfo &conv_info)
{
    unsigned int output_width  = 0;
    unsigned int output_height = 0;

    // Get output width and height
    std::tie(output_width, output_height) =
        arm_compute::scaled_dimensions(
            input_shape.x(), input_shape.y(),
            weights_shape.x(), weights_shape.y(),
            conv_info);

    // Create output shape
    TensorShape output_shape = input_shape;
    output_shape.set(0, output_width);
    output_shape.set(1, output_height);
    output_shape.set(2, weights_shape[3]);

    return output_shape;
}

bool convFloat32(const float* inputData, const Shape& inputShape,
                 const float* filterData, const Shape& filterShape,
                 const float* biasData, const Shape& biasShape,
                 int32_t padding_left, int32_t padding_right,
                 int32_t padding_top, int32_t padding_bottom,
                 int32_t stride_width, int32_t stride_height,
                 int32_t activation,
                 float* outputData, const Shape& outputShape)
{
  arm_compute::CLScheduler::get().default_init();

  uint32_t tsi_c = getSizeOfDimension(inputShape, 0);
  uint32_t tsi_h = getSizeOfDimension(inputShape, 1);
  uint32_t tsi_w = getSizeOfDimension(inputShape, 2);
  uint32_t tsi_n = getSizeOfDimension(inputShape, 3);

  uint32_t tsk_h = getSizeOfDimension(filterShape, 1);
  uint32_t tsk_w = getSizeOfDimension(filterShape, 2);
  uint32_t tsk_n = getSizeOfDimension(filterShape, 3);

  TensorShape input_shape = TensorShape(tsi_w, tsi_h, tsi_c, tsi_n);
  TensorShape filter_shape = TensorShape(tsi_w, tsi_h, tsi_c, tsi_n);
  arm_compute::PadStrideInfo conv_info =
      arm_compute::PadStrideInfo(stride_width, stride_height, padding_top, padding_bottom);

  TensorShape output_shape = calculate_convolution_layer_output_shape(
                                input_shape, filter_shape, conv_info);

  uint32_t tso_c = output_shape[0];
  uint32_t tso_w = output_shape[1];
  uint32_t tso_h = output_shape[2];
  uint32_t tso_n = output_shape[3];

  arm_compute::CLTensor input, output, bias, filter;

  input.allocator()->init(TensorInfo(tsi_w, tsi_h, arm_compute::Format::F32));
  output.allocator()->init(TensorInfo(tso_w, tso_h, arm_compute::Format::F32));
  bias.allocator()->init(TensorInfo(tso_w, tso_h, arm_compute::Format::F32));
  filter.allocator()->init(TensorInfo(tsk_w, tsk_h, arm_compute::Format::F32));

  input.allocator()->allocate();
  output.allocator()->allocate();
  bias.allocator()->allocate();
  filter.allocator()->allocate();

  input.map();
  InputAccessor ia(inputData, inputShape);
  ia.access_tensor(input);
  input.unmap();

  bias.map();
  BiasAccessor ba(biasData, biasShape);
  ba.access_tensor(bias);
  bias.unmap();

  filter.map();
  WeightAccessor fa(filterData, filterShape);
  fa.access_tensor(filter);
  filter.unmap();

  arm_compute::CLConvolutionLayer conv_f;
  conv_f.configure(&input, &filter, &bias, &output, conv_info);

  arm_compute::CLScheduler::get().sync();

  conv_f.run();

  output.map();
  OutputAccessor oa(outputData, outputShape);
  oa.access_tensor(output);
  output.unmap();

  return true;
}

} // namespace acl_runtime

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

enum COMPUTE_TYPE {
  COMPUTE_DEFAULT = 0,
  COMPUTE_ACLGRAPH,
  COMPUTE_ACLRT
};

bool convFloat32(const float* inputData, const Shape& inputShape,
                 const float* filterData, const Shape& filterShape,
                 const float* biasData, const Shape& biasShape,
                 int32_t padding_left, int32_t padding_right,
                 int32_t padding_top, int32_t padding_bottom,
                 int32_t stride_width, int32_t stride_height,
                 int32_t activation,
                 float* outputData, const Shape& outputShape,
                 COMPUTE_TYPE compType) {

  switch (compType)
  {
  case COMPUTE_DEFAULT :
    return convFloat32(inputData, inputShape, filterData, filterShape,
                       biasData, biasShape, padding_left, padding_right,
                       padding_top, padding_bottom, stride_width, stride_height,
                       activation, outputData, outputShape);

  case COMPUTE_ACLGRAPH :
    return acl_graph::convFloat32(inputData, inputShape, filterData, filterShape,
                       biasData, biasShape, padding_left, padding_right,
                       padding_top, padding_bottom, stride_width, stride_height,
                       activation, outputData, outputShape);

  case COMPUTE_ACLRT :
    return acl_runtime::convFloat32(inputData, inputShape, filterData, filterShape,
                       biasData, biasShape, padding_left, padding_right,
                       padding_top, padding_bottom, stride_width, stride_height,
                       activation, outputData, outputShape);
  }
  return false;
}

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

void dumpData(const char* name, const float* data, const Shape& shape)
{
  uint32_t height = getSizeOfDimension(shape, 1);
  uint32_t width  = getSizeOfDimension(shape, 2);

  std::cout << "---" << name << "---" << std::endl;
  for (int h = 0; h < height; h++) {
    std::cout << "H=" << h << " | ";
    for (int w = 0; w < width; w++) {
      std::cout << data[h * width + w] << ",";
    }
    std::cout << std::endl;
  }
}

void initData(float* outputData, int num, float value)
{
  for (int i = 0; i < num; i++) {
    *(outputData + i) = value;
  }
}

void initDataSeq(float* outputData, int num, float value)
{
  for (int i = 0; i < num; i++) {
    *(outputData + i) = value;
    value += 1.0;
  }
}

// compareData
// return true if result == expected with the shape info,
// otherwise false
bool compareData(const float* result, const float* expected, const Shape& shape)
{
  NN_CHECK_EQ(shape.dimensions.size(), 4);

  uint32_t height = getSizeOfDimension(shape, 1);
  uint32_t width  = getSizeOfDimension(shape, 2);
  uint32_t numitems = height * width;
  for (int item = 0; item < numitems; item++) {
    if (*(result + item) != *(expected + item)) {
      LOG(ERROR) << "compareData failed: result " << *(result + item)
                 << ", expected " << *(expected + item) << std::endl;
      return false;
    }
  }
  return true;
}

int test_3x3_1x1_one(COMPUTE_TYPE comptype)
{
  float inputData[9];
  const Shape inputShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float filterData[9];
  const Shape filterShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float biasData[1] = { 1.0 };
  const Shape biasShape = { OperandType::FLOAT32, {1,1,1,1}, 1.0, 0 };
  int32_t padding_left = 0;
  int32_t padding_right = 0;
  int32_t padding_top = 0;
  int32_t padding_bottom = 0;
  int32_t stride_width = 1;
  int32_t stride_height = 1;
  int32_t activation = static_cast<int32_t>(FusedActivationFunc::RELU);
  float* outputData = new float[9];
  const Shape outputShape = { OperandType::FLOAT32, {1,1,1,1}, 1.0, 0 };
  float* expectData = new float[9];
  bool bret;

  initData(inputData, sizeof(inputData) / sizeof(inputData[0]), 1.0);
  initData(filterData, sizeof(filterData) / sizeof(filterData[0]), 1.0);
  initData(outputData, sizeof(outputData) / sizeof(outputData[0]), 0.0);
  initData(expectData, sizeof(expectData) / sizeof(expectData[0]), 0.0);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     expectData, outputShape,
                     COMPUTE_DEFAULT);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     outputData, outputShape,
                     comptype);

  dumpData("Input  ", inputData, inputShape);
  dumpData("Filter ", filterData, filterShape);
  dumpData("Bias   ", biasData, biasShape);
  dumpData("Output ", outputData, outputShape);
  std::cout << std::endl;

  bret = compareData(outputData, expectData, outputShape);

  delete outputData;
  delete expectData;

  if (!bret)
  {
    LOG(ERROR) << "TEST FAILED " << __FUNCTION__ << std::endl;
    return -1;
  }
  return 0;
}

int test_3x3_3x3_one(void)
{
  float inputData[9];
  const Shape inputShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float filterData[9];
  const Shape filterShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float biasData[1] = { 1.0 };
  const Shape biasShape = { OperandType::FLOAT32, {1,1,1,1}, 1.0, 0 };
  int32_t padding_left = 1;
  int32_t padding_right = 1;
  int32_t padding_top = 1;
  int32_t padding_bottom = 1;
  int32_t stride_width = 1;
  int32_t stride_height = 1;
  int32_t activation = static_cast<int32_t>(FusedActivationFunc::RELU);
  float* outputData = new float[9];
  const Shape outputShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float* expectData = new float[9];
  bool bret;

  initData(inputData, sizeof(inputData) / sizeof(inputData[0]), 1.0);
  initData(filterData, sizeof(filterData) / sizeof(filterData[0]), 1.0);
  initData(outputData, sizeof(outputData) / sizeof(outputData[0]), 0.0);
  initData(expectData, sizeof(expectData) / sizeof(expectData[0]), 0.0);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     expectData, outputShape,
                     COMPUTE_DEFAULT);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     outputData, outputShape,
                     COMPUTE_ACLGRAPH);

  dumpData("Input  ", inputData, inputShape);
  dumpData("Filter ", filterData, filterShape);
  dumpData("Bias   ", biasData, biasShape);
  dumpData("Output ", outputData, outputShape);
  std::cout << std::endl;

  bret = compareData(outputData, expectData, outputShape);

  delete outputData;
  delete expectData;

  if (!bret)
  {
    LOG(ERROR) << "TEST FAILED " << __FUNCTION__ << std::endl;
    return -1;
  }
  return 0;
}

int test_3x3_3x3_seq(void)
{
  float inputData[9];
  const Shape inputShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float filterData[9];
  const Shape filterShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float biasData[1] = { 1.0 };
  const Shape biasShape = { OperandType::FLOAT32, {1,1,1,1}, 1.0, 0 };
  int32_t padding_left = 1;
  int32_t padding_right = 1;
  int32_t padding_top = 1;
  int32_t padding_bottom = 1;
  int32_t stride_width = 1;
  int32_t stride_height = 1;
  int32_t activation = static_cast<int32_t>(FusedActivationFunc::RELU);
  float* outputData = new float[9];
  const Shape outputShape = { OperandType::FLOAT32, {1,3,3,1}, 1.0, 0 };
  float* expectData = new float[9];
  bool bret;

  initDataSeq(inputData, sizeof(inputData) / sizeof(inputData[0]), 1.0);
  initDataSeq(filterData, sizeof(filterData) / sizeof(filterData[0]), 1.0);
  initDataSeq(outputData, sizeof(outputData) / sizeof(outputData[0]), 0.0);
  initData(expectData, sizeof(expectData) / sizeof(expectData[0]), 0.0);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     expectData, outputShape,
                     COMPUTE_DEFAULT);

  bret = convFloat32(inputData, inputShape,
                     filterData, filterShape,
                     biasData, biasShape,
                     padding_left, padding_right,
                     padding_top, padding_bottom,
                     stride_width, stride_height,
                     activation,
                     outputData, outputShape,
                     COMPUTE_ACLGRAPH);

  dumpData("Input  ", inputData, inputShape);
  dumpData("Filter ", filterData, filterShape);
  dumpData("Bias   ", biasData, biasShape);
  dumpData("Output ", outputData, outputShape);
  std::cout << std::endl;

  bret = compareData(outputData, expectData, outputShape);

  delete outputData;
  delete expectData;

  if (!bret)
  {
    LOG(ERROR) << "TEST FAILED " << __FUNCTION__ << std::endl;
    return -1;
  }
  return 0;
}

int main(int argc, char* argv[])
{
  int result;

  // input 3x3, output 1x1, all data 1.0
  result = test_3x3_1x1_one(COMPUTE_ACLGRAPH);
  if (result) return result;
  result = test_3x3_1x1_one(COMPUTE_ACLRT);
  if (result) return result;

  // input 3x3, output 3x3, all data 1.0
  result = test_3x3_3x3_one();
  if (result) return result;

  result = test_3x3_3x3_seq();
  if (result) return result;

  return result;
}


//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_CPP_INT_CHECKED_HPP
#define BOOST_MP_CPP_INT_CHECKED_HPP

#include <climits>
#include <limits>
#include <type_traits>
#include <stdexcept>
#include <string>
#include <boost/multiprecision/detail/standalone_config.hpp>
#include <boost/multiprecision/detail/no_exceptions_support.hpp>

namespace boost { namespace multiprecision { namespace backends { namespace detail {

//
// Simple routines for performing checked arithmetic with a builtin arithmetic type.
// Note that this is not a complete header, it must be included as part of boost/multiprecision/cpp_int.hpp.
//

template <typename T>
inline constexpr T type_max() noexcept
{
   return
   #ifdef BOOST_HAS_INT128
   std::is_same<T, boost::multiprecision::int128_type>::value ? INT128_MAX :
   std::is_same<T, boost::multiprecision::uint128_type>::value ? UINT128_MAX :
   #endif
   (std::numeric_limits<T>::max)();
}

template <typename T>
inline constexpr T type_min() noexcept
{
   return
   #ifdef BOOST_HAS_INT128
   std::is_same<T, boost::multiprecision::int128_type>::value ? INT128_MIN :
   std::is_same<T, boost::multiprecision::uint128_type>::value ? T(0) :
   #endif
   (std::numeric_limits<T>::min)();
}

inline void raise_overflow(std::string op)
{
   BOOST_MP_THROW_EXCEPTION(std::overflow_error("overflow in " + op));
}
inline void raise_add_overflow()
{
   raise_overflow("addition");
}
inline void raise_subtract_overflow()
{
   BOOST_MP_THROW_EXCEPTION(std::range_error("Subtraction resulted in a negative value, but the type is unsigned"));
}
inline void raise_mul_overflow()
{
   raise_overflow("multiplication");
}
inline void raise_div_overflow()
{
   raise_overflow("division");
}

template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_add_imp(A a, A b, const std::integral_constant<bool, true>&)
{
   if (a > 0)
   {
      if ((b > 0) && ((type_max<A>() - b) < a))
         raise_add_overflow();
   }
   else
   {
      if ((b < 0) && ((type_min<A>() - b) > a))
         raise_add_overflow();
   }
   return a + b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_add_imp(A a, A b, const std::integral_constant<bool, false>&)
{
   if ((type_max<A>() - b) < a)
      raise_add_overflow();
   return a + b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_add(A a, A b, const std::integral_constant<int, checked>&)
{
   return checked_add_imp(a, b, std::integral_constant<bool, boost::multiprecision::detail::is_signed<A>::value && boost::multiprecision::detail::is_integral<A>::value > ());
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_add(A a, A b, const std::integral_constant<int, unchecked>&)
{
   return a + b;
}

template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_subtract_imp(A a, A b, const std::integral_constant<bool, true>&)
{
   if (a > 0)
   {
      if ((b < 0) && ((type_max<A>() + b) < a))
         raise_subtract_overflow();
   }
   else
   {
      if ((b > 0) && ((type_min<A>() + b) > a))
         raise_subtract_overflow();
   }
   return a - b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_subtract_imp(A a, A b, const std::integral_constant<bool, false>&)
{
   if (a < b)
      raise_subtract_overflow();
   return a - b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_subtract(A a, A b, const std::integral_constant<int, checked>&)
{
   return checked_subtract_imp(a, b, std::integral_constant<bool, boost::multiprecision::detail::is_signed<A>::value && boost::multiprecision::detail::is_integral<A>::value>());
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_subtract(A a, A b, const std::integral_constant<int, unchecked>&)
{
   return a - b;
}

template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_multiply(A a, A b, const std::integral_constant<int, checked>&)
{
   BOOST_MP_USING_ABS
   if (a && (type_max<A>() / abs(a) < abs(b)))
      raise_mul_overflow();
   return a * b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_multiply(A a, A b, const std::integral_constant<int, unchecked>&)
{
   return a * b;
}

template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_divide(A a, A b, const std::integral_constant<int, checked>&)
{
   if (b == 0)
      raise_div_overflow();
   return a / b;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_divide(A a, A b, const std::integral_constant<int, unchecked>&)
{
   return a / b;
}

template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_left_shift(A a, unsigned long long shift, const std::integral_constant<int, checked>&)
{
   if (a && shift)
   {
      if ((shift > sizeof(A) * CHAR_BIT) || (a >> (sizeof(A) * CHAR_BIT - shift)))
         BOOST_MP_THROW_EXCEPTION(std::overflow_error("Shift out of range"));
   }
   return a << shift;
}
template <class A>
inline BOOST_MP_CXX14_CONSTEXPR A checked_left_shift(A a, unsigned long long shift, const std::integral_constant<int, unchecked>&)
{
   return (shift >= sizeof(A) * CHAR_BIT) ? 0 : a << shift;
}

}}}} // namespace boost::multiprecision::backends::detail

#endif


/*! \class EikonalSolver
    \brief Abstract class that serves as interface for the actual EikonalSolvers implemented.
    It requires (at least) the computeInternal method to be implemented,

    It uses as a main container the nDGridMap class. The nDGridMap template paramenter
    has to be an FMCell or something inherited from it.

    Copyright (C) 2015 Javier V. Gomez
    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 EIKONALSOLVER_H_
#define EIKONALSOLVER_H_

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

#include <boost/concept_check.hpp>

#include <fast_methods/fm/solver.hpp>
#include <fast_methods/console/console.h>

template <class grid_t>
class EikonalSolver : public Solver<grid_t>{

    public:
        EikonalSolver() : Solver<grid_t>("EikonalSolver") {}
        EikonalSolver(const std::string& name) : Solver<grid_t>(name) {}

        /** \brief Solves nD Eikonal equation for cell idx. If heuristics are activated, it will add
            the estimated travel time to goal with current velocity. */
        virtual double solveEikonal(const int & idx)
        {
            unsigned int a = grid_t::getNDims(); // a parameter of the Eikonal equation.
            Tvalues_.clear();

            for (unsigned int dim = 0; dim < grid_t::getNDims(); ++dim) {
                double minTInDim = grid_->getMinValueInDim(idx, dim);
                if (!std::isinf(minTInDim) && minTInDim < grid_->getCell(idx).getArrivalTime())
                    Tvalues_.push_back(minTInDim);
                else
                    a -=1;
            }

            if (a == 0)
                return std::numeric_limits<double>::infinity();

            // Sort the neighbor values to make easy the following code.
            /// \todo given that this sorts a small vector, a n^2 methods could be better. Test it.
            std::sort(Tvalues_.begin(), Tvalues_.end());
            double updatedT;
            for (unsigned i = 1; i <= a; ++i) {
                updatedT = solveEikonalNDims(idx, i);
                // If no more dimensions or increasing one dimension will not improve time.
                if (i == a || (updatedT - Tvalues_[i]) < utils::COMP_MARGIN)
                    break;
            }
            return updatedT;
        }

    protected:
        /** \brief Solves the Eikonal equation assuming that Tvalues_
            is sorted. */
        double solveEikonalNDims
        (unsigned int idx, unsigned int dim) {
            // Solve for 1 dimension.
            if (dim == 1)
                return Tvalues_[0] + grid_->getLeafSize() / grid_->getCell(idx).getVelocity();

            // Solve for any number > 1 of dimensions.
            double sumT = 0;
            double sumTT = 0;
            for (unsigned i = 0; i < dim; ++i) {
                sumT += Tvalues_[i];
                sumTT += Tvalues_[i]*Tvalues_[i];
            }

            // These a,b,c values are simplified since leafsize^2, which should be present in the three
            // terms but they are cancelled out when solving the quadratic function.
            double a = dim;
            double b = -2*sumT;
            double c = sumTT - grid_->getLeafSize() * grid_->getLeafSize() / (grid_->getCell(idx).getVelocity()*grid_->getCell(idx).getVelocity());
            double quad_term = b*b - 4*a*c;

            if (quad_term < 0)
                return std::numeric_limits<double>::infinity();
            else
                return (-b + sqrt(quad_term))/(2*a);
        }

        /** \brief Auxiliar vector with values T0,T1...Tn-1 variables in the Discretized Eikonal Equation. */
        std::vector<double>          Tvalues_;

        /** \brief Auxiliar array which stores the neighbor of each iteration of the computeFM() function. */
        std::array <unsigned int, 2*grid_t::getNDims()> neighbors_;

        using Solver<grid_t>::grid_;
};

#endif /* EIKONALSOLVER_H_*/


/*
 * This file is part of the statismo library.
 *
 * Author: Marcel Luethi (marcel.luethi@unibas.ch)
 *
 * Copyright (c) 2011 University of Basel
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __PosteriorModelBuilder_hxx
#define __PosteriorModelBuilder_hxx

#include "PosteriorModelBuilder.h"

#include <iostream>

#include <Eigen/SVD>

#include "CommonTypes.h"
#include "PCAModelBuilder.h"

namespace statismo {

//
// PosteriorModelBuilder
//
//

template <typename T>
PosteriorModelBuilder<T>::PosteriorModelBuilder()
    : Superclass() {
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    double noiseVariance) const {
    return BuildNewModel(sampleDataList, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues, pointValuesNoiseVariance), noiseVariance);
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueListType& pointValues,
    double pointValuesNoiseVariance,
    bool computeScores) const {

    return BuildNewModelFromModel(inputModel, TrivialPointValueWithCovarianceListWithUniformNoise(pointValues,pointValuesNoiseVariance), computeScores);

}

template <typename T>
typename PosteriorModelBuilder<T>::PointValueWithCovarianceListType
PosteriorModelBuilder<T>::TrivialPointValueWithCovarianceListWithUniformNoise(
    const PointValueListType& pointValues, double pointValueNoiseVariance) const {

    const MatrixType pointCovarianceMatrix = pointValueNoiseVariance * MatrixType::Identity(3,3);
    PointValueWithCovarianceListType pvcList;//(pointValues.size());

    for (typename PointValueListType::const_iterator it = pointValues.begin(); it != pointValues.end(); ++it) {
        pvcList.push_back(PointValueWithCovariancePairType(*it,pointCovarianceMatrix));
    }

    return pvcList;

}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModel(
    const DataItemListType& sampleDataList,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    double noiseVariance) const {
    typedef PCAModelBuilder<T> PCAModelBuilderType;
    PCAModelBuilderType* modelBuilder = PCAModelBuilderType::Create();
    StatisticalModelType* model = modelBuilder->BuildNewModel(sampleDataList, noiseVariance);
    StatisticalModelType* PosteriorModel = BuildNewModelFromModel(model, pointValuesWithCovariance, noiseVariance);
    delete modelBuilder;
    delete model;
    return PosteriorModel;
}

template <typename T>
typename PosteriorModelBuilder<T>::StatisticalModelType*
PosteriorModelBuilder<T>::BuildNewModelFromModel(
    const StatisticalModelType* inputModel,
    const PointValueWithCovarianceListType& pointValuesWithCovariance,
    bool computeScores) const {

    typedef statismo::Representer<T> RepresenterType;

    const RepresenterType* representer = inputModel->GetRepresenter();

    // The naming of the variables correspond to those used in the paper
    // Posterior Shape Models,
    // Thomas Albrecht, Marcel Luethi, Thomas Gerig, Thomas Vetter
    //
    const MatrixType& Q =  inputModel->GetPCABasisMatrix();
    const VectorType& mu = inputModel->GetMeanVector();

    // this method only makes sense for a proper PPCA model (e.g. the noise term is properly defined)
    // if the model has zero noise, we assume a small amount of noise
    double rho2 = std::max((double) inputModel->GetNoiseVariance(), (double) Superclass::TOLERANCE);

    unsigned dim = representer->GetDimensions();

    // build the part matrices with , considering only the points that are fixed
    //
    unsigned numPrincipalComponents = inputModel->GetNumberOfPrincipalComponents();
    MatrixType Q_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);
    VectorType mu_g(pointValuesWithCovariance.size() * dim);
    VectorType s_g(pointValuesWithCovariance.size() * dim);

    MatrixType LQ_g(pointValuesWithCovariance.size()* dim, numPrincipalComponents);

    unsigned i = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);

        // In the formulas, we actually need the precision matrix, which is the inverse of the covariance.
        const MatrixType pointPrecisionMatrix = it->second.inverse();

        // Get the three rows pertaining to this point:
        const MatrixType Qrows_for_pt_id = Q.block(pt_id * dim, 0, dim, numPrincipalComponents);

        Q_g.block(i * dim, 0, dim, numPrincipalComponents) = Qrows_for_pt_id;
        mu_g.block(i * dim, 0, dim, 1) = mu.block(pt_id * dim, 0, dim, 1);
        s_g.block(i * dim, 0, dim, 1) = val;

        LQ_g.block(i * dim, 0, dim, numPrincipalComponents) = pointPrecisionMatrix * Qrows_for_pt_id;
        i++;
    }

    VectorType D2 = inputModel->GetPCAVarianceVector().array();

    const MatrixType& Q_gT = Q_g.transpose();

    MatrixType M = Q_gT * LQ_g;
    M.diagonal() += VectorType::Ones(Q_g.cols());

    MatrixTypeDoublePrecision Minv = M.cast<double>().inverse();

    // the MAP solution for the latent variables (coefficients)
    VectorType coeffs = Minv.cast<ScalarType>() * LQ_g.transpose() * (s_g - mu_g);

    // the MAP solution in the sample space
    VectorType mu_c = inputModel->GetRepresenter()->SampleToSampleVector(inputModel->DrawSample(coeffs));

    const VectorType& pcaVariance = inputModel->GetPCAVarianceVector();
    VectorTypeDoublePrecision pcaSdev = pcaVariance.cast<double>().array().sqrt();

    VectorType D2MinusRho = D2 - VectorType::Ones(D2.rows()) * rho2;
    // the values of D2 can be negative. We need to be careful when taking the root
    for (unsigned i = 0; i < D2MinusRho.rows(); i++) {
        D2MinusRho(i) = std::max((ScalarType) 0, D2(i));
    }
    VectorType D2MinusRhoSqrt = D2MinusRho.array().sqrt();

    typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDType;
    MatrixTypeDoublePrecision innerMatrix = D2MinusRhoSqrt.cast<double>().asDiagonal() * Minv * D2MinusRhoSqrt.cast<double>().asDiagonal();
    SVDType svd(innerMatrix, Eigen::ComputeThinU);

    // SVD of the inner matrix
    VectorType D_c = svd.singularValues().cast<ScalarType>();

    // Todo: Maybe it is possible to do this with Q, so that we don"t need to get U as well.
    MatrixType U_c = inputModel->GetOrthonormalPCABasisMatrix() * svd.matrixU().cast<ScalarType>();

    StatisticalModelType* PosteriorModel = StatisticalModelType::Create(representer , mu_c, U_c, D_c, rho2);

    // Write the parameters used to build the models into the builderInfo

    typename ModelInfo::BuilderInfoList builderInfoList = inputModel->GetModelInfo().GetBuilderInfoList();

    BuilderInfo::ParameterInfoList bi;
    bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(rho2)));
    bi.push_back(BuilderInfo::KeyValuePair("FixedPointsVariance ", Utils::toString(0.2)));
//
    BuilderInfo::DataInfoList di;

    unsigned pt_no = 0;
    for (typename PointValueWithCovarianceListType::const_iterator it = pointValuesWithCovariance.begin(); it != pointValuesWithCovariance.end(); ++it) {
        VectorType val = representer->PointSampleToPointSampleVector(it->first.second);

        // TODO we looked up the PointId for the same point before. Having it here again is inefficient.
        unsigned pt_id = representer->GetPointIdForPoint(it->first.first);
        std::ostringstream keySStream;
        keySStream << "Point constraint " << pt_no;
        std::ostringstream valueSStream;
        valueSStream << "(" << pt_id << ", (";

        for (unsigned d = 0; d < dim - 1; d++) {
            valueSStream << val[d] << ",";
        }
        valueSStream << val[dim -1];
        valueSStream << "))";
        di.push_back(BuilderInfo::KeyValuePair(keySStream.str(), valueSStream.str()));
        pt_no++;
    }

    BuilderInfo builderInfo("PosteriorModelBuilder", di, bi);
    builderInfoList.push_back(builderInfo);

    MatrixType inputScores = inputModel->GetModelInfo().GetScoresMatrix();
    MatrixType scores = MatrixType::Zero(inputScores.rows(), inputScores.cols());

    if (computeScores == true) {

        // get the scores from the input model
        for (unsigned i = 0; i < inputScores.cols(); i++) {
            // reconstruct the sample from the input model and project it back into the model
            typename RepresenterType::DatasetPointerType ds = inputModel->DrawSample(inputScores.col(i));
            scores.col(i) = PosteriorModel->ComputeCoefficientsForDataset(ds);
            representer->DeleteDataset(ds);
        }
    }
    ModelInfo info(scores, builderInfoList);
    PosteriorModel->SetModelInfo(info);

    return PosteriorModel;

}

} // namespace statismo

#endif


#pragma once
#define UNIT_TEST_ENV
#include "../golos.publication/types.h"
#include "../golos.publication/config.hpp"
#include <eosio/chain/types.hpp>
#include <boost/test/unit_test.hpp>
#include <string>
#include <vector>
#include <list>
#include <map>

namespace eosio { namespace testing {

using namespace eosio::chain;

struct balance_data {
    double tokenamount = 0.0;
    double vestamount = 0.0;
    double paymentsamount = 0.0;
    bool tokenclosed = false;
    bool vestclosed = false;
    //vesting delegation isn't covered by these tests
};

struct pool_data {
    double msgs = 0.0;
    double funds = 0.0;
    double rshares = 0.0;
    double rsharesfn = 0.0;
};

struct message_data {
    double netshares = 0.0;
    double voteshares = 0.0;
    double sumcuratorsw = 0.0;
};

// positive values are max absolute delta, and negatives represent max relative delta (1.0 is 100%)
constexpr struct {
    balance_data balance {0.1, 1, 0.005};       // this absolute values are divided to PRECISION_DIV, scale if change
    pool_data pool {0, 0.1, 0.1, 0.1};          // 0.015 value is divided to PRECISION_DIV (=15 if PRECISION_DIV=1.0)
    message_data message {-0.001, -0.001, -0.001};
} delta;

constexpr double balance_delta = 0.01;
constexpr double funds_delta = 0.01;
constexpr double pool_rshares_delta = 0.01;
constexpr double pool_rsharesfn_delta = 0.01;

inline double get_prop(int64_t arg) {
    return static_cast<double>(arg) / static_cast<double>(golos::config::_100percent);
}

struct mssgid {
    eosio::chain::name author;
    std::string permlink;

    auto get_unique_key() const {
        return permlink;
    }

    bool operator ==(const mssgid& rhs) const {
        return author == rhs.author && permlink == rhs.permlink;
    }
};

struct permlink_info {
    mssgid msg;
    mssgid parent;
    uint16_t level;
    uint32_t childcount;
};

struct aprox_val_t {
    double val;
    double delta;
    bool operator ==(const aprox_val_t& rhs) const {
        static constexpr double eps = 1.e-5;
        BOOST_REQUIRE_MESSAGE((rhs.delta >= 0.0 && delta >= 0.0) || (rhs.delta <= 0.0 && delta <= 0.0),
            "aprox_val_t operator ==(): wrong comparison mode");
        if ((rhs.val > eps && val < -eps) || (rhs.val < -eps && val > eps))
            return false;
        double d = std::min(double(std::abs(rhs.delta)), std::abs(double(delta)));
        if (delta < 0.0) {
            double a = std::min(std::abs(rhs.val), std::abs(val));
            double b = std::max(std::abs(rhs.val), std::abs(val));
            return b < eps || (a / b) > (1.0 - d);
        } else {
            return (val - d) <= rhs.val && rhs.val <= (val + d);
        }
    }

    aprox_val_t& operator = (const double& arg) { val = arg; return *this; }
    aprox_val_t& operator +=(const double& arg) { val += arg; return *this; }
};

inline std::ostream& operator<< (std::ostream& os, const aprox_val_t& rhs) {
    os << std::to_string(rhs.val) << " (" << std::to_string(double(rhs.delta)) << ") ";
    return os;
}

struct statemap : public std::map<std::string, aprox_val_t> {
    static std::string get_pool_str(uint64_t id) {
        return "pool #" +  std::to_string(id) + ": ";
    }
    static std::string get_balance_str(account_name acc) {
        return "balance of " +  acc.to_string() + ": ";
    }
    static std::string get_message_str(const mssgid& msg) {
        return "message of " + msg.author.to_string() + " #" + msg.permlink + ": ";
    }
    static std::string get_vote_str(account_name voter, const mssgid& msg) {
        return "vote of " + voter.to_string() + " for message of " + msg.author.to_string() + " #" + msg.permlink + ": ";
    }
    void set_pool(uint64_t id, const pool_data& data = {}) {
        auto prefix = get_pool_str(id);
        operator[](prefix + "msgs") = { data.msgs, delta.pool.msgs };
        operator[](prefix + "funds") = { data.funds, delta.pool.funds };
        operator[](prefix + "rshares") = { data.rshares, delta.pool.rshares };
        operator[](prefix + "rsharesfn") = { data.rsharesfn, delta.pool.rsharesfn };
    }
    void set_balance(account_name acc, const balance_data data = {}) {
        auto prefix = get_balance_str(acc);
        operator[](prefix + "tokenamount") = { data.tokenamount, delta.balance.tokenamount };
        operator[](prefix + "vestamount") = { data.vestamount, delta.balance.vestamount };
        operator[](prefix + "paymentsamount") = { data.paymentsamount, delta.balance.paymentsamount };
    }

    void set_balance_token(account_name acc, double val) {
        auto prefix = get_balance_str(acc);
        operator[](prefix + "tokenamount") = { val, delta.balance.tokenamount };
    }

    void set_balance_payments(account_name acc, double val) {
        auto prefix = get_balance_str(acc);
        operator[](prefix + "paymentsamount") = { val, delta.balance.paymentsamount };
    }

    void set_balance_vesting(account_name acc, double val) {
        auto prefix = get_balance_str(acc);
        operator[](prefix + "vestamount") = { val, delta.balance.vestamount };
    }

    void set_message(const mssgid& msg, const message_data& data = {}) {

        auto prefix = get_message_str(msg);
        operator[](prefix + "netshares") = { data.netshares, delta.message.netshares };
        operator[](prefix + "voteshares") = { data.voteshares, delta.message.voteshares };
        operator[](prefix + "sumcuratorsw") = { data.sumcuratorsw, delta.message.sumcuratorsw };
    }

    pool_data get_pool(uint64_t id) {
        auto prefix = get_pool_str(id);
        return {
            operator[](prefix + "msgs").val,
            operator[](prefix + "funds").val,
            operator[](prefix + "rshares").val,
            operator[](prefix + "rsharesfn").val
        };
    }

    void remove_same(const statemap& rhs) {
        for (auto itr_rhs = rhs.begin(); itr_rhs != rhs.end(); itr_rhs++) {
            auto itr = find(itr_rhs->first);
            if (itr != end() && itr->second == itr_rhs->second)
                erase(itr);
        }
    }
};

inline std::ostream& operator<< (std::ostream& os, const statemap& rhs) {
    for (auto itr = rhs.begin(); itr != rhs.end(); ++itr)
        os << itr->first << " = " << itr->second << "\n";
    return os;
}

struct delegation {
    account_name delegator;
    account_name delegatee;
    uint16_t interest_rate;
    double amount;
};

struct delegate_balance {
    double delegated;
    double received;
};

struct vote {
    account_name voter;
    double weight;
    double vesting;
    double created;         // time
    int64_t charge = 0;     // charge value at vote time (in percent units 0-10000)
    double revote_diff = 0.0;
    double revote_vesting = 0.0;
    double revote_weight() const { return weight + revote_diff; };

    double weight_charge(double weight) const {
        auto current_power = std::min(golos::config::_100percent - charge, int64_t(golos::config::_100percent));
        int64_t abs_w = abs(weight) * current_power;
        auto used_charge = (abs_w + 200 - 1) / 200;     // TODO: `200` should depend on limit
        return static_cast<double>(used_charge) / golos::config::_100percent;
    };

    double rshares() const {
        auto weight_rshares = revote_diff ? revote_weight() : weight;
        auto abs_rshares = weight_charge(weight_rshares) * (revote_diff ? revote_vesting : vesting);
        return (weight_rshares < 0) ? -abs_rshares : abs_rshares;
    };

    double voteshares() const {
        return weight > 0.0 ? weight_charge(weight) * vesting : 0.0;
    };
};

struct beneficiary {
    account_name account;
    uint16_t weight;
};

struct message {
    mssgid key;
    double tokenprop;
    std::list<vote> votes;
    double created;         // time
    std::vector<beneficiary> beneficiaries;
    double reward_weight;
    double curators_prcnt;
    double max_payout;

    double get_rshares_sum() const {
        double ret = 0.0;
        for (auto& v : votes)
            ret += v.rshares();
        return ret;
    };

    message(mssgid k, double tokenprop_, double created_, const std::vector<beneficiary>& beneficiaries_, double reward_weight_, double curators_prcnt_, double max_payout_) :
        key(k), tokenprop(tokenprop_), created(created_), beneficiaries(beneficiaries_), reward_weight(reward_weight_), curators_prcnt(curators_prcnt_), max_payout(max_payout_) {};
};

class cutted_func {
    std::function<double(double)> _fn;
    double _maxarg;
public:
    cutted_func(std::function<double(double)>&& f, double maxarg) : _fn(std::move(f)), _maxarg(maxarg) {};
    double operator()(double arg)const {return _fn(std::min(std::max(0.0, arg), _maxarg));};
};

struct rewardrules {
    cutted_func mainfunc;
    cutted_func curationfunc;
    cutted_func timepenalty;
    rewardrules(
        cutted_func&& mainfunc_,
        cutted_func&& curationfunc_,
        cutted_func&& timepenalty_
    ) :
        mainfunc(std::move(mainfunc_)),
        curationfunc(std::move(curationfunc_)),
        timepenalty(std::move(timepenalty_)) {}
};

struct chargeinfo {
    uint64_t lastupdate;
    double data;
};
using usercharges = std::vector<chargeinfo>;

struct limits {
    using func_t = std::function<double(double, double, double)>;
    enum kind_t: enum_t {POST, COMM, VOTE, POSTBW, UNDEF};
    std::vector<func_t> restorers; //(funcs of: prev_charge (p), vesting (v), elapsed_seconds (t))
    std::vector<limitedact> limitedacts;
    std::vector<double> vestingprices;//disabled if < 0
    std::vector<double> minvestings;

    const limitedact& get_limited_act(kind_t kind) const {
        return limitedacts.at(static_cast<size_t>(kind));
    }

    const func_t* get_restorer(kind_t kind) const {
        uint8_t num = get_limited_act(kind).restorernum;
        return num == disabled_restorer ? nullptr : &(restorers.at(num));
    }

    double get_vesting_price(kind_t kind) const {
        auto k = static_cast<size_t>(kind);
        return k < vestingprices.size() ? vestingprices[k] : -1.0;
    }

    double get_min_vesting_for(kind_t kind) const {
        auto pos = static_cast<size_t>(kind);
        return pos < minvestings.size() ? minvestings.at(pos) : 0;
    }

    size_t get_charges_num() const {
        size_t ret = 0;
        for (auto& act : limitedacts)
            ret = std::max(ret, static_cast<size_t>(act.chargenum) + 1);
        return ret;
    }

    bool check() const {
        if (vestingprices.size() >= 3 || minvestings.size() != 3)
            return false;
        for (auto& act : limitedacts)
            if (!(act.restorernum < restorers.size()) || act.restorernum == disabled_restorer)
                return false;
        return true;
    }

    void update_charge(chargeinfo& chg, const func_t* restorer, uint64_t cur_time, double vesting, double price) const {
        using namespace golos::config;    // for limit_restorer_domain
        if (restorer != nullptr) {
            auto prev = std::min(chg.data, static_cast<double>(limit_restorer_domain::max_prev));
            auto v_corr = std::min(vesting, static_cast<double>(limit_restorer_domain::max_vesting));
            auto elapsed_seconds = std::min(static_cast<double>((cur_time - chg.lastupdate) / fc::seconds(1).count()),
                static_cast<double>(limit_restorer_domain::max_elapsed_seconds));
            auto restored = (*restorer)(prev, v_corr, elapsed_seconds);
            if (restored > prev)
                chg.data = 0.0;
            else
                chg.data = std::min(chg.data - restored, static_cast<double>(limit_restorer_domain::max_res));
        }
        if (price > 0.0) {
            auto added = std::max(price, static_cast<double>(std::numeric_limits<fixed_point_utils::fixp_t>::min()));
            chg.data = std::min(chg.data + added, static_cast<double>(limit_restorer_domain::max_res));
        }
        chg.lastupdate = cur_time;
    }
    // charges are not const here
    double calc_power(kind_t kind, usercharges& charges, uint64_t cur_time, double vesting, double w) const {
        auto& lim_act = get_limited_act(kind);
        auto restorer = get_restorer(kind);
        auto& chg = charges[lim_act.chargenum];
        update_charge(chg, restorer, cur_time, vesting, lim_act.chargeprice * w);
        return (chg.data > lim_act.cutoffval) ? lim_act.cutoffval / chg.data : 1.0;
    }

    // w - vote-battery only multiplier with 1.0 base (100% = 1.0)
    double apply(kind_t kind, usercharges& charges, double& vesting_ref, uint64_t cur_time, double w) const {
        auto use_vest = vesting_ref;
        if (kind == limits::VOTE && w != 0) {
            use_vest *= w;
        }
        if (use_vest < get_min_vesting_for(kind))
            return -1.0;

        auto charges_prev = charges;
        if (charges.empty())
            charges.resize(get_charges_num(), {cur_time, 0});
        auto power = calc_power(kind, charges, cur_time, vesting_ref, kind == limits::VOTE ? w : 1.0);
        if (power != 1.0)
            charges = charges_prev;

        if (kind == limits::VOTE)
            return power == 1.0 ? 1.0 : -1.0;

        if (power != 1.0 && w == 0.0)
            return -1.0;

        if (power != 1.0) {//=> w != 0
            auto price = get_vesting_price(kind);
            if (price <= 0 || price > vesting_ref)
                return -1.0;
            vesting_ref -= price;
        }

        if (kind == limits::POST) {
            power = calc_power(limits::POSTBW, charges, cur_time, vesting_ref, 1.0);
            return power*power;
        }

        return 1.0;
    }
};

struct rewardpool {
    uint64_t id;
    double funds;
    rewardrules rules;
    limits lims;
    std::list<message> messages;
    std::map<account_name, usercharges> charges;
    rewardpool(uint64_t id_, rewardrules&& rules_, limits&& lims_)
    :   id(id_), funds(0.0), rules(std::move(rules_)), lims(std::move(lims_)) {};

    double get_rshares_sum()const {
        double ret = 0.0;
        for (auto& m : messages)
            ret += m.get_rshares_sum();
        return ret;
    }

    //yup, it's dumb. but we are testing here, we don't need performance, we prefer straightforward logic
    double get_rsharesfn_sum() const {
        double ret = 0.0;
        for (auto& m : messages)
            ret += rules.mainfunc(m.get_rshares_sum());
        return ret;
    }

    double get_ratio() const {
        BOOST_REQUIRE_MESSAGE(funds >= 0.0, "rewardpool: wrong payment mode");
        auto r = get_rshares_sum();
        return r > 0 ? funds / r : std::numeric_limits<double>::max();
    }
};

struct state {
    std::map<account_name, balance_data> balances;
    std::vector<rewardpool> pools;
    std::vector<delegation> delegators;
    std::map<account_name, delegate_balance> dlg_balances;
    void clear() { balances.clear(); pools.clear(); };
};

}} // eosio::testing

FC_REFLECT(eosio::testing::beneficiary, (account)(weight))
FC_REFLECT(eosio::testing::mssgid, (author)(permlink))
FC_REFLECT(eosio::testing::permlink_info, (msg)(parent)(level)(childcount))


#ifndef FORWARD_MODEL_HPP
#define FORWARD_MODEL_HPP

#include "2d.hpp"
#include "boundaries.hpp"
#include "density-filter.hpp"
#include "ersatz_stiffness.hpp"
#include "read_mesh.hpp"

#include <Eigen/Sparse>

#include <memory>

template <class Mesh>
struct ModelInfo
{
    using element_t = decltype(Elasticity::TwoD::instantiate_element(std::declval<const Mesh &>(), 0));
    using el_stiffness_matrix_t =
        decltype(Elasticity::TwoD::element_stiffness_matrix(std::declval<element_t>(), 0, 0));
    using stiffness_matrix_t = Elasticity::TwoD::StiffnessType<Mesh>;

    // We need the mesh data structure around.
    Mesh mesh;

    // Interpolation for the stiffness.
    ErsatzStiffness interp;

    // Density filter.
    std::vector<DensityFilter> filter;

    // Reference values of the element stiffness matrices.
    std::vector<el_stiffness_matrix_t> ref_stiffness_matrices;

    // Segments of the boundary with a homogeneous Dirichlet BC.
    std::vector<size_t> homogeneous_boundaries;

    // Store filtered values of the density variable.
    Eigen::VectorXd rho_filt;

    // Storage for the forward stiffness matrix.
    stiffness_matrix_t forward_stiffness;
    Eigen::SparseMatrix<double> forward_stiffness_eigen;
    // Factorization of the stiffness.
    Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> factorized;

    // The right hand side and displacement vectors.
    Eigen::VectorXd nodal_forcing;
    Eigen::VectorXd displacement;

    ModelInfo(
        Mesh &&m, ErsatzStiffness i, std::vector<DensityFilter> &&filt, std::vector<size_t> &&hom,
        Eigen::VectorXd &&forcing)
        : mesh(std::move(m)), interp(i), filter(std::move(filt)), homogeneous_boundaries(std::move(hom)),
          forward_stiffness(2 * mesh.num_nodes()),
          forward_stiffness_eigen(2 * mesh.num_nodes(), 2 * mesh.num_nodes()),
          nodal_forcing(std::move(forcing)), displacement(Eigen::VectorXd::Zero(nodal_forcing.size()))
    {
        assert(static_cast<unsigned long>(displacement.size()) == 2 * mesh.num_nodes());
    }

    ModelInfo(ModelInfo &&other) = default;
    // ModelInfo(const ModelInfo &other) = default;
};

template <int order>
using ModelInfoType = ModelInfo<MeshType<order>>;

using ModelInfoVariant =
    std::variant<ModelInfoType<1>, ModelInfoType<2>, ModelInfoType<3>, ModelInfoType<4>>;

// This is the main function you want to use from here.
std::unique_ptr<ModelInfoVariant> construct_model_info(
    const std::string &mesh_file, int order, Eigen::Vector2d force, ErsatzStiffness interp,
    double filt_radius, double lambda, double mu);

// And this one.
void update_model_info(ModelInfoVariant &minfo, const double *rho);

std::size_t ndofs(const ModelInfoVariant &minfo);
std::size_t num_elements(const ModelInfoVariant &minfo);

template <class Mesh>
void update_forward_stiffness(ModelInfo<Mesh> &minfo, std::size_t eli, bool init = false);

template <class Mesh>
void construct_eigen_stiffness(
    Eigen::SparseMatrix<double> &Ke, const typename ModelInfo<Mesh>::stiffness_matrix_t &K);

template <class Mesh>
void initialize_model_info(ModelInfo<Mesh> &minfo, double lambda, double mu)
{
    const auto &mesh = minfo.mesh;
    minfo.ref_stiffness_matrices.reserve(mesh.num_elements());
    minfo.rho_filt.resize(mesh.num_elements(), 1.0);

    // Compute the reference stiffness matrices, and also the structure of the
    // sparse global stiffness matrix.
    for (std::size_t eli = 0; eli < mesh.num_elements(); ++eli)
    {
        minfo.ref_stiffness_matrices.emplace_back(Elasticity::TwoD::element_stiffness_matrix(
            Elasticity::TwoD::instantiate_element(mesh, eli), lambda, mu));

        update_forward_stiffness(minfo, eli, true);
    }

    for (std::size_t which : minfo.homogeneous_boundaries)
    {
        Elasticity::TwoD::impose_homogeneous_condition(mesh, minfo.forward_stiffness, which);
    }
    construct_eigen_stiffness<Mesh>(minfo.forward_stiffness_eigen, minfo.forward_stiffness);
    minfo.factorized.analyzePattern(minfo.forward_stiffness_eigen);
}

template <class Mesh, class Vec>
void update_model_info(ModelInfo<Mesh> &minfo, const Vec &rho)
{
    filter_densities(minfo.rho_filt, rho, minfo.filter);
    minfo.forward_stiffness.reset();
    for (std::size_t eli = 0; eli < minfo.mesh.num_elements(); ++eli)
    {
        update_forward_stiffness(minfo, eli);
    }
    for (std::size_t which : minfo.homogeneous_boundaries)
    {
        Elasticity::TwoD::impose_homogeneous_condition(minfo.mesh, minfo.forward_stiffness, which);
    }
    construct_eigen_stiffness<Mesh>(minfo.forward_stiffness_eigen, minfo.forward_stiffness);
    minfo.factorized.factorize(minfo.forward_stiffness_eigen);
}

template <class Mesh>
void update_forward_stiffness(ModelInfo<Mesh> &minfo, std::size_t eli, bool init)
{
    const auto &nn = minfo.mesh.element(eli).node_numbers();
    const auto &Kel = minfo.ref_stiffness_matrices.at(eli);
    assert(nn.size() * 2 == static_cast<unsigned>(Kel.rows()));
    assert(Kel.rows() == Kel.cols());

    double scaling = init ? 1.0 : minfo.interp(minfo.rho_filt[eli]);
    auto &K = minfo.forward_stiffness;
    for (std::size_t i = 0; i < nn.size(); ++i)
    {
        for (std::size_t j = i; j < nn.size(); ++j)
        {
            K.insert_entry(2 * nn[i], 2 * nn[j], scaling * Kel(2 * i, 2 * j));
            K.insert_entry(2 * nn[i], 2 * nn[j] + 1, scaling * Kel(2 * i, 2 * j + 1));
            if (i != j)
            {
                K.insert_entry(2 * nn[i] + 1, 2 * nn[j], scaling * Kel(2 * i + 1, 2 * j));
            }
            K.insert_entry(2 * nn[i] + 1, 2 * nn[j] + 1, scaling * Kel(2 * i + 1, 2 * j + 1));
        }
    }
}

template <class Mesh>
void construct_eigen_stiffness(
    Eigen::SparseMatrix<double> &Ke, const typename ModelInfo<Mesh>::stiffness_matrix_t &K)
{
    std::vector<Eigen::Triplet<double>> triplets;
    for (std::size_t i = 0; i < K.num_rows(); ++i)
    {
        for (auto [j, v] : K.row(i))
        {
            triplets.emplace_back(i, j, v);
            if (j != i)
            {
                triplets.emplace_back(j, i, v);
            }
        }
    }
    Ke.setFromTriplets(triplets.begin(), triplets.end());
    Ke.makeCompressed();
}

template <class Mesh>
std::vector<std::size_t> get_dirichlet_boundaries(const Mesh &mesh)
{
    std::vector<std::size_t> ebounds;
    for (std::size_t i = 0; i < mesh.num_boundaries(); ++i)
    {
        if (mesh.boundary_tags(i).size() == 1 && mesh.boundary_tags(i)[0] == "ESSENTIAL")
        {
            fmt::print("Boundary {} is an essential boundary\n", i);
            ebounds.push_back(i);
        }
    }
    return ebounds;
}

template <class Mesh>
std::size_t get_forcing_boundary(const Mesh &mesh)
{
    for (std::size_t i = 0; i < mesh.num_boundaries(); ++i)
    {
        if (mesh.boundary_tags(i).size() == 1 && mesh.boundary_tags(i)[0] == "FORCE")
        {
            return i;
        }
    }
    return -1;
}

template <int Order>
auto build_vector_mass_matrix(const LineMesh<Order> &mesh)
{
    auto [elements, dofs] = mesh.get_elements_and_dofs();
    constexpr auto mass_form = [](const auto &u, const auto &v) { return u * v; };
    const auto num_dofs = dofs.back().back() + 1;

    using Triplet = Eigen::Triplet<double>;
    std::vector<Triplet> triplets;
    for (std::size_t el = 0; el < elements.size(); ++el)
    {
        const auto element = elements[el];
        const auto el_dofs = dofs[el];

        const auto local_M = element.form_matrix(mass_form);
        for (std::size_t i = 0; i < el_dofs.size(); ++i)
        {
            for (std::size_t j = i; j < el_dofs.size(); ++j)
            {
                triplets.emplace_back(2 * el_dofs[i], 2 * el_dofs[j], local_M(i, j));
                triplets.emplace_back(2 * el_dofs[i] + 1, 2 * el_dofs[j] + 1, local_M(i, j));
                if (j != i)
                {
                    triplets.emplace_back(2 * el_dofs[j], 2 * el_dofs[i], local_M(i, j));
                    triplets.emplace_back(2 * el_dofs[j] + 1, 2 * el_dofs[i] + 1, local_M(i, j));
                }
            }
        }
    }
    Eigen::SparseMatrix<double> M(num_dofs * 2, num_dofs * 2);
    M.setFromTriplets(triplets.begin(), triplets.end());
    return M;
}

template <class Mesh>
void compute_pressure_force(
    Eigen::VectorXd &nodal_forcing, const Mesh &mesh, std::size_t fbound, Eigen::Vector2d f)
{
    Eigen::SparseMatrix<double> M = build_vector_mass_matrix(build_boundary_mesh(mesh, fbound));
    const auto &boundary = mesh.boundary(fbound);
    Eigen::VectorXd work(M.rows());
    for (int i = 0; i < M.rows(); i += 2)
    {
        work.segment(i, 2) = f;
    }

    work = (M * work).eval();

    for (std::size_t i = 0; i < boundary.nodes.size(); ++i)
    {
        std::size_t node = boundary.nodes[i];
        nodal_forcing.segment(node * 2, 2) += work.segment(i * 2, 2);
    }
}

#endif // FORWARD_MODEL_HPP


#include <catch.hpp>

#include "board.hpp"

#include <numeric_range.hpp>

#include <boost/optional/optional_io.hpp>

using Aronda::Board;

TEST_CASE("Testing basic board properties", "[board]")
{
    SECTION("Clean Board Properties")
    {
        Board g;
        for(const auto square_index : range(Aronda::c_number_of_squares))
        {
            const auto state = g.squareState(Aronda::Square(square_index));
            CHECK(state.placed_stones[0] == 0);
            CHECK(state.placed_stones[1] == 0);
            CHECK(state.player_locked == boost::none);
        }
    }

    SECTION("Max number of stones on a square")
    {
        for(const auto i : range(Aronda::c_number_of_squares))
        {
            const auto max_nb_stones = Board::maxNumberOfStonesOnASquare(Aronda::Square(i));
            if(i == 17 || i == 19 || i == 21 || i == 23)
                CHECK(max_nb_stones == 2);
            else if(i == 0 || i == 9 || i == 11 || i == 13 || i == 15)
                CHECK(max_nb_stones == 4);
            else
                CHECK(max_nb_stones == 3);
        }
    }
}


// kate: replace-tabs off; indent-width 4; indent-mode normal
// vim: ts=4:sw=4:noexpandtab
/*

Copyright (c) 2010--2018,
François Pomerleau and Stephane Magnenat, ASL, ETHZ, Switzerland
You can contact the authors at <f dot pomerleau at gmail dot com> and
<stephane at magnenat dot net>

All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the <organization> nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL ETH-ASL BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/
#include "SurfaceNormal.h"

// Eigenvalues
#include "Eigen/QR"
#include "Eigen/Eigenvalues"

#include "PointMatcherPrivate.h"
#include "IO.h"
#include "MatchersImpl.h"

#include <boost/format.hpp>

#include "DataPointsFilters/utils/utils.h"

// SurfaceNormalDataPointsFilter
// Constructor
template<typename T>
SurfaceNormalDataPointsFilter<T>::SurfaceNormalDataPointsFilter(const Parameters& params):
	PointMatcher<T>::DataPointsFilter("SurfaceNormalDataPointsFilter",
		SurfaceNormalDataPointsFilter::availableParameters(), params),
	knn(Parametrizable::get<int>("knn")),
	maxDist(Parametrizable::get<T>("maxDist")),
	epsilon(Parametrizable::get<T>("epsilon")),
	keepNormals(Parametrizable::get<bool>("keepNormals")),
	keepDensities(Parametrizable::get<bool>("keepDensities")),
	keepEigenValues(Parametrizable::get<bool>("keepEigenValues")),
	keepEigenVectors(Parametrizable::get<bool>("keepEigenVectors")),
	keepMatchedIds(Parametrizable::get<bool>("keepMatchedIds")),
	keepMeanDist(Parametrizable::get<bool>("keepMeanDist")),
	sortEigen(Parametrizable::get<bool>("sortEigen")),
	smoothNormals(Parametrizable::get<bool>("smoothNormals"))
{
}

// Compute
template<typename T>
typename PointMatcher<T>::DataPoints
SurfaceNormalDataPointsFilter<T>::filter(
	const DataPoints& input)
{
	DataPoints output(input);
	inPlaceFilter(output);
	return output;
}

// In-place filter
template<typename T>
void SurfaceNormalDataPointsFilter<T>::inPlaceFilter(
	DataPoints& cloud)
{
	typedef typename DataPoints::View View;
	typedef typename DataPoints::Label Label;
	typedef typename DataPoints::Labels Labels;
	typedef typename MatchersImpl<T>::KDTreeMatcher KDTreeMatcher;
	typedef typename PointMatcher<T>::Matches Matches;

	using namespace PointMatcherSupport;

	const int pointsCount(cloud.features.cols());
	const int featDim(cloud.features.rows());
	const int descDim(cloud.descriptors.rows());
	const unsigned int labelDim(cloud.descriptorLabels.size());

	// Validate descriptors and labels
	int insertDim(0);
	for(unsigned int i = 0; i < labelDim ; ++i)
		insertDim += cloud.descriptorLabels[i].span;
	if (insertDim != descDim)
		throw InvalidField("SurfaceNormalDataPointsFilter: Error, descriptor labels do not match descriptor data");

	// Reserve memory for new descriptors
	const int dimNormals(featDim-1);
	const int dimDensities(1);
	const int dimEigValues(featDim-1);
	const int dimEigVectors((featDim-1)*(featDim-1));
	//const int dimMatchedIds(knn);
	const int dimMeanDist(1);

	boost::optional<View> normals;
	boost::optional<View> densities;
	boost::optional<View> eigenValues;
	boost::optional<View> eigenVectors;
	boost::optional<View> matchedValues;
	boost::optional<View> matchIds;
	boost::optional<View> meanDists;

	Labels cloudLabels;
	if (keepNormals)
		cloudLabels.push_back(Label("normals", dimNormals));
	if (keepDensities)
		cloudLabels.push_back(Label("densities", dimDensities));
	if (keepEigenValues)
		cloudLabels.push_back(Label("eigValues", dimEigValues));
	if (keepEigenVectors)
		cloudLabels.push_back(Label("eigVectors", dimEigVectors));
	if (keepMatchedIds)
		cloudLabels.push_back(Label("matchedIds", knn));
	if (keepMeanDist)
		cloudLabels.push_back(Label("meanDists", dimMeanDist));

	// Reserve memory
	cloud.allocateDescriptors(cloudLabels);

	if (keepNormals)
		normals = cloud.getDescriptorViewByName("normals");
	if (keepDensities)
		densities = cloud.getDescriptorViewByName("densities");
	if (keepEigenValues)
		eigenValues = cloud.getDescriptorViewByName("eigValues");
	if (keepEigenVectors)
		eigenVectors = cloud.getDescriptorViewByName("eigVectors");
	if (keepMatchedIds)
		matchIds = cloud.getDescriptorViewByName("matchedIds");
	if (keepMeanDist)
		meanDists = cloud.getDescriptorViewByName("meanDists");

	using namespace PointMatcherSupport;
	// Build kd-tree
	Parametrizable::Parameters param;
	boost::assign::insert(param) ( "knn", toParam(knn) );
	boost::assign::insert(param) ( "epsilon", toParam(epsilon) );
	boost::assign::insert(param) ( "maxDist", toParam(maxDist) );

	KDTreeMatcher matcher(param);
	matcher.init(cloud);

	Matches matches(typename Matches::Dists(knn, pointsCount), typename Matches::Ids(knn, pointsCount));
	matches = matcher.findClosests(cloud);

	// Search for surrounding points and compute descriptors
	int degenerateCount(0);
	for (int i = 0; i < pointsCount; ++i)
	{
		bool isDegenerate = false;
		// Mean of nearest neighbors (NN)
		Matrix d(featDim-1, knn);
		int realKnn = 0;

		for(int j = 0; j < int(knn); ++j)
		{
			if (matches.dists(j,i) != Matches::InvalidDist)
			{
				const int refIndex(matches.ids(j,i));
				d.col(realKnn) = cloud.features.block(0, refIndex, featDim-1, 1);
				++realKnn;
			}
		}
		d.conservativeResize(Eigen::NoChange, realKnn);

		const Vector mean = d.rowwise().sum() / T(realKnn);
		const Matrix NN = d.colwise() - mean;

		const Matrix C((NN * NN.transpose()) / T(realKnn));
		Vector eigenVa = Vector::Zero(featDim-1, 1);
		Matrix eigenVe = Matrix::Zero(featDim-1, featDim-1);
		// Ensure that the matrix is suited for eigenvalues calculation
		if(keepNormals || keepEigenValues || keepEigenVectors)
		{
			if(C.fullPivHouseholderQr().rank()+1 >= featDim-1)
			{
				const Eigen::EigenSolver<Matrix> solver(C);
				eigenVa = solver.eigenvalues().real();
				eigenVe = solver.eigenvectors().real();

				if(sortEigen)
				{
					const std::vector<size_t> idx = sortIndexes<T>(eigenVa);
					const size_t idxSize = idx.size();
					Vector tmp_eigenVa = eigenVa;
					Matrix tmp_eigenVe = eigenVe;
					for(size_t i=0; i<idxSize; ++i)
					{
						eigenVa(i,0) = tmp_eigenVa(idx[i], 0);
						eigenVe.col(i) = tmp_eigenVe.col(idx[i]);
					}
				}
			}
			else
			{
				//std::cout << "WARNING: Matrix C needed for eigen decomposition is degenerated. Expected cause: no noise in data" << std::endl;
				++degenerateCount;
				isDegenerate = true;
			}
		}

		if(keepNormals)
		{
			if(sortEigen)
				normals->col(i) = eigenVe.col(0);
			else
				normals->col(i) = computeNormal<T>(eigenVa, eigenVe);

			// clamp normals to [-1,1] to handle approximation errors
			normals->col(i) = normals->col(i).cwiseMax(-1.0).cwiseMin(1.0);
		}
		if(keepDensities)
		{
			if(isDegenerate)
				(*densities)(0, i) = 0.;
			else
				(*densities)(0, i) = computeDensity<T>(NN);
		}
		if(keepEigenValues)
			eigenValues->col(i) = eigenVa;
		if(keepEigenVectors)
			eigenVectors->col(i) = serializeEigVec<T>(eigenVe);
		if(keepMeanDist)
		{
			if(isDegenerate)
				(*meanDists)(0, i) = std::numeric_limits<std::size_t>::max();
			else
			{
				const Vector point = cloud.features.block(0, i, featDim-1, 1);
				(*meanDists)(0, i) = (point - mean).norm();
			}
		}

	}

	if(keepMatchedIds)
	{
		matchIds.get() = matches.ids.template cast<T>();
	}

	if(smoothNormals)
	{
		for (int i = 0; i < pointsCount; ++i)
		{
			const Vector currentNormal = normals->col(i);
			Vector mean = Vector::Zero(featDim-1);
			int n=0;
			for(int j = 0; j < int(knn); ++j)
			{
				if (matches.dists(j,i) != Matches::InvalidDist)
				{
					const int refIndex(matches.ids(j,i));
					const Vector normal = normals->col(refIndex);
					if(currentNormal.dot(normal) > 0.)
						mean += normal;
					else // flip normal vector
						mean -= normal;

					++n;
				}
			}

			normals->col(i) = mean / T(n);
		}
	}

	if (degenerateCount)
	{
		LOG_WARNING_STREAM("WARNING: Matrix C needed for eigen decomposition was degenerated in " << degenerateCount << " points over " << pointsCount << " (" << float(degenerateCount)*100.f/float(pointsCount) << " %)");
	}

}

template struct SurfaceNormalDataPointsFilter<float>;
template struct SurfaceNormalDataPointsFilter<double>;


#include "b18TrafficDijkstra.h"

#include <boost/graph/dijkstra_shortest_paths.hpp>

#define ROUTE_DEBUG 0
#define PATH_DEBUG 0

namespace LC {

/////////////////////////////
template <typename UniquePairAssociativeContainer>
class default_assignment_associative_property_map : public boost::put_get_helper <
    typename UniquePairAssociativeContainer::value_type::second_type &,
    default_assignment_associative_property_map<UniquePairAssociativeContainer> > {
  typedef UniquePairAssociativeContainer C;
 public:
  typedef typename C::key_type key_type;
  typedef typename C::value_type::second_type value_type;
  typedef value_type &reference;
  typedef boost::lvalue_property_map_tag category;
  default_assignment_associative_property_map() : m_c(0) { }
  default_assignment_associative_property_map(C &c) : m_c(&c) { }

  reference operator[](const key_type &k) const {
    if (m_c->find(k) == m_c->end()) {
      (*m_c)[k] = FLT_MAX;
    }

    return (*m_c)[k];
  }
 private:
  C *m_c;
};

class dijkstra_finish : public std::exception {
};

typedef std::map<int, float> DistanceMap;
typedef default_assignment_associative_property_map<DistanceMap> DistanceMapInternal;

typedef std::map<LC::RoadGraph::roadGraphVertexDesc_BI, LC::RoadGraph::roadGraphVertexDesc_BI> PredecessorMap;
typedef default_assignment_associative_property_map<PredecessorMap> PredecessorMapInternal;

void B18TrafficDijstra::calculateSeveralPeopleRoute(
  LC::RoadGraph::roadBGLGraph_BI &roadGraph,
  std::vector<B18TrafficPerson> &trafficPersonVec,
  std::vector<uint> &indexPathVec,
  uint &currIndexPath,
  std::vector<uint> &peopleStartInInter,
  std::map<RoadGraph::roadGraphEdgeDesc_BI, uint> &edgeDescToLaneMapNum) {

  LC::RoadGraph::roadGraphVertexDesc_BI srcvertex = trafficPersonVec[peopleStartInInter[0]].init_intersection;
  //check not already at the destination
  int n = boost::num_vertices(roadGraph);

  for (int p = 0; p < peopleStartInInter.size(); p++) {
    LC::RoadGraph::roadGraphVertexDesc_BI tgtvertex =
      trafficPersonVec[peopleStartInInter[p]].end_intersection;

    if (tgtvertex == srcvertex || srcvertex < 0 || srcvertex >= n || tgtvertex < 0 || tgtvertex >= n) { //source same than target (we have arrived)
      trafficPersonVec[peopleStartInInter[p]].indexPathInit = 0; // that index points to -1
      peopleStartInInter.erase(peopleStartInInter.begin() + p);
      p--;
    }
  }

  if (peopleStartInInter.size() <= 0) { // all processed
    return;
  }

  // run dijkstra
  //printf("Run dijktra\n");
  PredecessorMap mapPredecessor;
  boost::associative_property_map<PredecessorMap> pmPredecessor(mapPredecessor);

  DistanceMap mapDistance;
  DistanceMapInternal pmDistance(mapDistance);
  mapDistance[srcvertex] = 0.0;

  typedef LC::RoadGraph::roadGraphVertexDesc_BI VertexDescriptor;
  typedef std::map<VertexDescriptor, size_t>            VertexIndexMap;
  typedef std::map<VertexDescriptor, boost::default_color_type> ColorMap;
  VertexIndexMap mapVertexIndex;
  ColorMap mapColor;
  boost::associative_property_map<VertexIndexMap> pmVertexIndex(mapVertexIndex);
  boost::associative_property_map<ColorMap> pmColor(mapColor);

  boost::dijkstra_shortest_paths_no_init(roadGraph,
                                         srcvertex,
                                         pmPredecessor,
                                         pmDistance,
                                         boost::get(&LC::RoadGraphEdge::edge_weight, roadGraph),
                                         pmVertexIndex,
                                         std::less<float>(),
                                         boost::closed_plus<float>(),
                                         float(),
                                         boost::default_dijkstra_visitor(),
                                         pmColor);

  // if not map predecessor all failed
  if (mapPredecessor.size() <= 0) {
    for (int p = 0; p < peopleStartInInter.size(); p++) {
      trafficPersonVec[peopleStartInInter[p]].indexPathInit = 0; // that index points to -1
    }
    return;
  }

  // for each person find the path using the map predecersor
  for (int p = 0; p < peopleStartInInter.size(); p++) {
    // Set person index to path.
    trafficPersonVec[peopleStartInInter[p]].indexPathInit = currIndexPath;
    // create path
    uint vertex = trafficPersonVec[peopleStartInInter[p]].end_intersection;
    //printf("s %d d %d\n",srcvertex,vertex);
    std::vector<uint> path;

    while (vertex != srcvertex) {
      if (ROUTE_DEBUG == true) {
        printf("%d<- ", vertex);
      }

      path.push_back(vertex);
      vertex = mapPredecessor[vertex];

      if (path[path.size() - 1] == vertex) {
        break;  //twice the same vertex
      }
    }

    path.push_back(srcvertex);

    if (ROUTE_DEBUG == true) {
      printf("%d\n", srcvertex);
    }

    //printf("path %d\n",path.size());
    // put path lanes in nextEdgeM
    //trafficPersonVec[peopleStartInInter[p]].nextPathEdge=nextEdgeM.size();//the first path edge will be in that possition in nextEdge
    for (int pa = path.size() - 1; pa > 0; pa--) {
      std::pair<RoadGraph::roadGraphEdgeDesc_BI, bool> edge_pair = boost::edge(
            path[pa], path[pa - 1], roadGraph);

      if (edge_pair.second == false) {
        if (path[pa] != 0 && path[pa - 1] != 0) {
          printf("N Error: Edge %d %d should exists\n", path[pa], path[pa - 1]);
        }
        break;
      } else {
        //std::map<RoadGraph::roadGraphEdgeDesc_BI,uint> edgeDescToLaneMapNum
        if (edgeDescToLaneMapNum.find(edge_pair.first) == edgeDescToLaneMapNum.end()) {
          if (roadGraph[edge_pair.first].numberOfLanes > 0) { //not found and >0 --> error
            printf("****Unknown edge\n");
          }
          break;
        }

        uint lane = edgeDescToLaneMapNum[edge_pair.first];
        if (indexPathVec.size() < (currIndexPath-2)) {  // almost full
          printf("*****indexPathVec too small -> resize: %d\n", indexPathVec.size());
          indexPathVec.resize((int) (indexPathVec.size()*1.2f));// Expand 20%.
        }
        indexPathVec[currIndexPath++] = lane;
      }
    }
    indexPathVec[currIndexPath++] = -1; // end path with -1
  }

  ////////////////////
  // Debug
  const bool kDebugFirstPerson = false;
  if (trafficPersonVec.size()>0 && kDebugFirstPerson) {
    int currIndex = 0;
    while (true) {
      uint laneMap = indexPathVec[trafficPersonVec[0].indexPathInit + currIndex];
      if (laneMap != -1) {
        printf("-> %u ", laneMap);
        currIndex++;
      } else {
        break;
      }
    }
    printf("\n");
  }

}//

////////////////

void B18TrafficDijstra::generateRoutesMulti(
  LC::RoadGraph::roadBGLGraph_BI &roadGraph,
  std::vector<B18TrafficPerson> &trafficPersonVec,
  std::vector<uint>& indexPathVec,
  std::map<RoadGraph::roadGraphEdgeDesc_BI, uint> &edgeDescToLaneMapNum,
  int weigthMode,
  float sample) {

  uint currIndexPath = 0; // counter to keep track where to put more
  std::vector<uint> oldIndexPathVec = std::move(indexPathVec); // avoid copying
  indexPathVec = std::vector<uint>();
  indexPathVec.resize(trafficPersonVec.size() * 140); // initial allocation (so we do not add)
  indexPathVec[currIndexPath++] = -1; // first path is empty
  // Just using the lane length and number of edges
  if (weigthMode == 0 || weigthMode == 1) {
    if (PATH_DEBUG) {
      printf(">> generateRoutesMulti\n");
    }

    QTime timer;
    timer.start();
    RoadGraph::roadGraphEdgeIter_BI ei, eiEnd;

    // 1. Update weight edges

    int numEdges = 0;
    float minTravelTime = FLT_MAX;
    float maxTravelTime = -FLT_MAX;
    float minLength = FLT_MAX;
    float maxLength = -FLT_MAX;
    float minSpeed = FLT_MAX;
    float maxSpeed = -FLT_MAX;

    for (boost::tie(ei, eiEnd) = boost::edges(roadGraph); ei != eiEnd; ++ei) {
      numEdges++;
      if ((edgeDescToLaneMapNum.size() > 20) && (numEdges % (edgeDescToLaneMapNum.size() / 20)) == 0) {
        printf("Edge %d of %d (%2.0f%%)\n", numEdges, edgeDescToLaneMapNum.size(), (100.0f * numEdges) / edgeDescToLaneMapNum.size());
      }
      if (roadGraph[*ei].numberOfLanes > 0) {
        // Use the original info, length and max speed
        float speed;
        if (weigthMode == 0 || roadGraph[*ei].averageSpeed.size() <= 0) {
          speed = roadGraph[*ei].maxSpeedMperSec;// *sqrt(roadGraph[*ei].numberOfLanes));
        } else {
          // Use the avarage speed sampled in a former step
          float avOfAv = 0;
          for (int avOfAvN = 0; avOfAvN < roadGraph[*ei].averageSpeed.size(); avOfAvN++) {
            avOfAv += roadGraph[*ei].averageSpeed[avOfAvN];
          }
          speed = avOfAv / roadGraph[*ei].averageSpeed.size();
        }
        float travelTime = roadGraph[*ei].edgeLength / speed;
        roadGraph[*ei].edge_weight = travelTime;

        minTravelTime = minTravelTime>travelTime ? travelTime : minTravelTime;
        maxTravelTime = maxTravelTime < travelTime ? travelTime : maxTravelTime;

        minLength = minLength > roadGraph[*ei].edgeLength ? roadGraph[*ei].edgeLength : minLength;
        maxLength = maxLength < roadGraph[*ei].edgeLength ? roadGraph[*ei].edgeLength : maxLength;

        minSpeed = minSpeed > speed ? speed : minSpeed;
        maxSpeed = maxSpeed < speed ? speed : maxSpeed;
      } else {
        roadGraph[*ei].edge_weight =
          100000000.0; //FLT_MAX;// if it has not lines, then the weight is inf
      }
    }
    printf("Travel time Min: %f Max: %f\n", minTravelTime, maxTravelTime);
    printf("Length Min: %f Max: %f\n", minLength, maxLength);
    printf("Speed Min: %f Max: %f\n", minSpeed, maxSpeed);

    //2. Generate hash with init intersectios nad people
    printf("Start Dikstra\n");
    QHash<uint, std::vector<uint>> intersectionToPeople;

    for (int p = 0; p < trafficPersonVec.size(); p++) {

      // Some people do not change route.
      if (sample != 1.0f) {
        if (sample > (((float)qrand()) / RAND_MAX)) { // not recalculate
          // Copy route directly
          uint oldIndex = trafficPersonVec[p].indexPathInit;
          trafficPersonVec[p].indexPathInit = currIndexPath;
          uint index = 0;
          while (oldIndexPathVec[oldIndex + index] != -1) {
            indexPathVec[currIndexPath++] = oldIndexPathVec[oldIndex + index];
            index++;
          }
          indexPathVec[currIndexPath++] = -1;
          continue;
        }
      }

      uint iI = trafficPersonVec[p].init_intersection;
      intersectionToPeople[iI].push_back(p);
    }

    QHash<uint, std::vector<uint>>::const_iterator i = intersectionToPeople.constBegin();

    int proccP = 0;
    while (i != intersectionToPeople.constEnd()) {
      if (i.value().size() <= 0) {
        continue;
      }

      std::vector<uint> peopleStartInInter = i.value();
      proccP += peopleStartInInter.size();
      //printf("peopleStartInInter.size() %d\n",peopleStartInInter.size());
      calculateSeveralPeopleRoute(
        roadGraph,
        trafficPersonVec,
        indexPathVec,
        currIndexPath,
        peopleStartInInter,
        edgeDescToLaneMapNum);
      ++i;

      if ((trafficPersonVec.size() > 100) && ((proccP % int(trafficPersonVec.size()/100) == 0))) {
        printf("Dikstra: %d of %d in %d ms\n", proccP, trafficPersonVec.size(), timer.elapsed());
      }
    }

    // resize tight indexPathVec and set everyone to the first edge (maybe do in sim?)
    printf("Final Path Size %u\n", currIndexPath);
    indexPathVec.resize(currIndexPath);
    printf("<< generateRoutesMulti in %d ms\n", timer.elapsed());
    return;
  }

  printf("ERROR: Unknow generateRoutesMulti %d\n", weigthMode);
  exit(0);

}//
}