/*===========================================================================
 NetEvo Library
 Copyright (C) 2011 Thomas E. Gorochowski <tgorochowski@me.com>
 Bristol Centre for Complexity Sciences, University of Bristol, Bristol, UK
 ----------------------------------------------------------------------------
 NetEvo is a computing framework designed to allow researchers to investigate
 evolutionary aspects of dynamical complex networks. By providing tools to
 easily integrate each of these factors in a coherent way, it is hoped a
 greater understanding can be gained of key attributes and features displayed
 by complex systems.

 NetEvo is open-source software released under the Open Source Initiative
 (OSI) approved Non-Profit Open Software License ("Non-Profit OSL") 3.0.
 Detailed information about this licence can be found in the COPYING file
 included as part of the source distribution.

 This library 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.
 ============================================================================*/

#include "simulate.h"
#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp>

using namespace boost::numeric::odeint;

namespace netevo {

   void SimulateMap::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // We are in discrete time so use integers for time
      int t = 0, tEnd = (int)tMax;
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());

      // Check to ensure that initial conditions are correct size
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateMap::simulate)" << endl;
         return;
      }

      // Vector to hold the output. Copy the initial conditions and add to the output
      State y1 = State(initial);
      State y2 = State(initial);

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Send the observer the initial conditions
      logger.newState (sys, y1);
      logger.endStep(SIM_STEP);
      logger.commit();
      obs(y1, 0.0);

      // Loop through all time steps and calculate new states
      for (t = 1; t <= tEnd; t++) {
         if (t%2 == 0) {
            // Use y1 as old and y2 as new
            // Simulate the dynamics
            sys(y1, y2, (double)t);
            // Log the state change
            logger.newState (sys, y2);
            logger.endStep(SIM_STEP);
            logger.commit();
            // Send result to the observer
            obs(y2, (double)t);
         }
         else {
            // Use y2 as old and y1 as new
            // Simulate the dynamics
            sys(y2, y1, (double)t);
            // Log the state change
            logger.newState (sys, y1);
            logger.endStep(SIM_STEP);
            logger.commit();
            // Send result to the observer
            obs(y1, (double)t);
         }
      }

      // Ensure the initial vector is updated to the final result
      initial = ( t%2 == 0 )  ? y1 : y2;
   }

   void SimulateOdeFixed::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeFixed::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta4<State> rk4_stepper_type;
      typedef adams_bashforth_moulton<5,State> adams_bash_moul_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_4:
            integrate_const(rk4_stepper_type(),
                                    Simulator(&sys), initial, 0.0, tMax, mStepSize, ObserverPassThrough(sys, obs, logger));
            break;
         case ADAM_BASH_MOUL:
            integrate_const(adams_bash_moul_stepper_type(),
                                    Simulator(&sys), initial, 0.0, tMax, mStepSize, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

   void SimulateOdeConst::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeConst::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta_cash_karp54<State> rkck54_error_stepper_type;
      typedef runge_kutta_dopri5<State> dopri5_error_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_CASH_KARP_54:
            integrate_const(make_controlled( mEpsAbs , mEpsRel , rkck54_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5:
            integrate_const(make_controlled( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5_DENSE:
            integrate_const(make_dense_output( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                    Simulator(&sys), initial, 0.0, tMax, mOutputStep, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

   void SimulateOdeAdaptive::simulate (System &sys, double tMax, State &initial, SimObserver &obs, ChangeLog &logger) {

      // Check to ensure that initial conditions are correct size
      int states = (countNodes(sys)*sys.nodeStates()) + (countArcs(sys)*sys.arcStates());
      if (initial.size() < states || initial.size() > states) {
         cerr << "Incorrect number of states for initial conditions (SimulateOdeAdaptive::simulate)" << endl;
         return;
      }

      // Check that the state IDs are correct, if not refresh
      if (!sys.validStateIDs()) { sys.refreshStateIDs(); }

      // Create the required steppers
      typedef runge_kutta_cash_karp54<State> rkck54_error_stepper_type;
      typedef runge_kutta_dopri5<State> dopri5_error_stepper_type;

      // Solve the system
      switch (mStepper) {
         case RK_CASH_KARP_54:
            integrate_adaptive(make_controlled( mEpsAbs , mEpsRel , rkck54_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5:
            integrate_adaptive(make_controlled( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         case RK_DOPRI_5_DENSE:
            integrate_adaptive(make_dense_output( mEpsAbs , mEpsRel , dopri5_error_stepper_type() ),
                                       Simulator(&sys), initial, 0.0, tMax, mInitialStep, ObserverPassThrough(sys, obs, logger));
            break;
         default:
            // Do nothing
            break;
      }
   }

} // netevo namespace


/****************************************************************************
**
** Copyright (C) 2017 TU Wien, ACIN, Vision 4 Robotics (V4R) group
** Contact: v4r.acin.tuwien.ac.at
**
** This file is part of V4R
**
** V4R is distributed under dual licenses - GPLv3 or closed source.
**
** GNU General Public License Usage
** V4R 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.
**
** V4R 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.
**
** Please review the following information to ensure the GNU General Public
** License requirements will be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
**
** Commercial License Usage
** If GPL is not suitable for your project, you must purchase a commercial
** license to use V4R. Licensees holding valid commercial V4R licenses may
** use this file in accordance with the commercial license agreement
** provided with the Software or, alternatively, in accordance with the
** terms contained in a written agreement between you and TU Wien, ACIN, V4R.
** For licensing terms and conditions please contact office<at>acin.tuwien.ac.at.
**
**
** The copyright holder additionally grants the author(s) of the file the right
** to use, copy, modify, merge, publish, distribute, sublicense, and/or
** sell copies of their contributions without any restrictions.
**
****************************************************************************/

/**
 * @file main.cpp
 * @author Johann Prankl (prankl@acin.tuwien.ac.at)
 * @date 2017
 * @brief
 *
 */

#ifndef KP_TSF_OPTIMIZE_BUNDLE_HH
#define KP_TSF_OPTIMIZE_BUNDLE_HH

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <v4r/core/macros.h>
#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <v4r/camera_tracking_and_mapping/TSFFrame.hh>
#include "opencv2/imgproc/imgproc.hpp"

namespace v4r {

/**
 * TSFOptimizeBundle
 */
class V4R_EXPORTS TSFOptimizeBundle {
 public:
  /**
   * Parameter
   */
  class Parameter {
   public:
    double depth_error_scale;

    bool use_robust_loss;
    double loss_scale;
    bool optimize_focal_length;
    bool optimize_principal_point;
    bool optimize_radial_k1;
    bool optimize_radial_k2;
    bool optimize_radial_k3;
    bool optimize_tangential_p1;
    bool optimize_tangential_p2;
    bool optimize_delta_cloud_rgb_pose_global;
    bool optimize_delta_cloud_rgb_pose;
    double px_error_scale;
    Parameter()
    : depth_error_scale(100), use_robust_loss(true), loss_scale(2.), optimize_focal_length(false),
      optimize_principal_point(false), optimize_radial_k1(false), optimize_radial_k2(false), optimize_radial_k3(false),
      optimize_tangential_p1(false), optimize_tangential_p2(false), optimize_delta_cloud_rgb_pose_global(false),
      optimize_delta_cloud_rgb_pose(false), px_error_scale(1) {}
  };

 private:
  Parameter param;

  cv::Mat_<double> dist_coeffs;
  cv::Mat_<double> intrinsic;
  std::vector<double> lm_intrinsics;

  Eigen::Matrix<double, 6, 1> delta_pose;
  std::vector<Eigen::Matrix<double, 6, 1>> poses_Rt;
  std::vector<Eigen::Matrix<double, 6, 1>> poses_Rt_RGB;
  std::vector<std::vector<Eigen::Vector3d>> points3d;

  std::vector<int> const_intrinsics;
  bool const_all_intrinsics;

  void convertPosesToRt(const std::vector<TSFFrame::Ptr> &map);
  void convertPosesFromRt(std::vector<TSFFrame::Ptr> &map);
  void convertPosesFromRtRGB(std::vector<TSFFrame::Ptr> &map);
  void convertPoints(const std::vector<TSFFrame::Ptr> &map);
  void optimizePoses(std::vector<TSFFrame::Ptr> &map);
  void optimizeCloudPosesRGBPoses(std::vector<TSFFrame::Ptr> &map);
  void optimizeCloudPosesDeltaRGBPose(std::vector<TSFFrame::Ptr> &map);

  void setCameraParameterConst();

 public:
  TSFOptimizeBundle(const Parameter &p = Parameter());
  ~TSFOptimizeBundle();

  void optimize(std::vector<TSFFrame::Ptr> &map);

  void getCameraParameter(cv::Mat &_intrinsic, cv::Mat &_dist_coeffs) const;

  inline const std::vector<std::vector<Eigen::Vector3d>> &getOptiPoints() const {
    return points3d;
  }

  void setCameraParameter(const cv::Mat &_intrinsic, const cv::Mat &_dist_coeffs);
  void setParameter(const Parameter &p = Parameter());
};

/*************************** INLINE METHODES **************************/

}  // namespace v4r

#endif


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <array>
#include <boost/functional/hash.hpp>
#include <cstddef>
#include <unordered_map>
#include <utility>

#include "DataStructures/DataBox/Tag.hpp"
#include "Domain/Structure/Direction.hpp"
#include "Domain/Structure/ElementId.hpp"
#include "Evolution/DiscontinuousGalerkin/MortarData.hpp"
#include "NumericalAlgorithms/Spectral/Mesh.hpp"
#include "NumericalAlgorithms/Spectral/Projection.hpp"  // for MortarSize
#include "Time/TimeStepId.hpp"

/// %Tags used for DG evolution scheme.
namespace evolution::dg::Tags {
/// Data on mortars, indexed by (Direction, ElementId) pairs
///
/// The `Dim` is the volume dimension, not the face dimension.
template <size_t Dim>
struct MortarData : db::SimpleTag {
  using Key = std::pair<Direction<Dim>, ElementId<Dim>>;
  using type =
      std::unordered_map<Key, evolution::dg::MortarData<Dim>, boost::hash<Key>>;
};

/// Mesh on the mortars, indexed by (Direction, ElementId) pairs
///
/// The `Dim` is the volume dimension, not the face dimension.
template <size_t Dim>
struct MortarMesh : db::SimpleTag {
  using Key = std::pair<Direction<Dim>, ElementId<Dim>>;
  using type = std::unordered_map<Key, Mesh<Dim - 1>, boost::hash<Key>>;
};

/// Size of a mortar, relative to the element face.  That is, the part
/// of the face that it covers.
///
/// The `Dim` is the volume dimension, not the face dimension.
template <size_t Dim>
struct MortarSize : db::SimpleTag {
  using Key = std::pair<Direction<Dim>, ElementId<Dim>>;
  using type =
      std::unordered_map<Key, std::array<Spectral::MortarSize, Dim - 1>,
                         boost::hash<Key>>;
};

/// The next temporal id at which to receive data on the specified mortar.
///
/// The `Dim` is the volume dimension, not the face dimension.
template <size_t Dim>
struct MortarNextTemporalId : db::SimpleTag {
  using Key = std::pair<Direction<Dim>, ElementId<Dim>>;
  using type = std::unordered_map<Key, TimeStepId, boost::hash<Key>>;
};
}  // namespace evolution::dg::Tags


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <backendsCommon/test/EndToEndTestImpl.hpp>

#include <backendsCommon/test/DetectionPostProcessTestImpl.hpp>
#include <backendsCommon/test/GatherEndToEndTestImpl.hpp>
#include <backendsCommon/test/MergerTestImpl.hpp>
#include <backendsCommon/test/ArithmeticTestImpl.hpp>

#include <boost/test/unit_test.hpp>
#include <boost/test/execution_monitor.hpp>

BOOST_AUTO_TEST_SUITE(RefEndToEnd)

std::vector<armnn::BackendId> defaultBackends = {armnn::Compute::CpuRef};

BOOST_AUTO_TEST_CASE(ConstantUsage_Ref_Float32)
{
    BOOST_TEST(ConstantUsageFloat32Test(defaultBackends));
}

BOOST_AUTO_TEST_CASE(ConstantUsage_Ref_Uint8)
{
    BOOST_TEST(ConstantUsageUint8Test(defaultBackends));
}

BOOST_AUTO_TEST_CASE(Unsigned8)
{
    using namespace armnn;

    // Create runtime in which test will run
    armnn::IRuntime::CreationOptions options;
    armnn::IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // Builds up the structure of the network.
    armnn::INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0, "input");
    IConnectableLayer* softmax = net->AddSoftmaxLayer(SoftmaxDescriptor(), "softmax");
    IConnectableLayer* output  = net->AddOutputLayer(0, "output");

    input->GetOutputSlot(0).Connect(softmax->GetInputSlot(0));
    softmax->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    // Sets the tensors in the network.
    TensorInfo inputTensorInfo(TensorShape({1, 5}), DataType::QuantisedAsymm8);
    inputTensorInfo.SetQuantizationOffset(100);
    inputTensorInfo.SetQuantizationScale(10000.0f);
    input->GetOutputSlot(0).SetTensorInfo(inputTensorInfo);

    TensorInfo outputTensorInfo(TensorShape({1, 5}), DataType::QuantisedAsymm8);
    outputTensorInfo.SetQuantizationOffset(0);
    outputTensorInfo.SetQuantizationScale(1.0f/255.0f);
    softmax->GetOutputSlot(0).SetTensorInfo(outputTensorInfo);

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, defaultBackends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    auto error = runtime->LoadNetwork(netId, std::move(optNet));
    BOOST_TEST(error == Status::Success);

    // Creates structures for input & output.
    std::vector<uint8_t> inputData
    {
        1, 10, 3, 200, 5 // Some inputs - one of which is sufficiently larger than the others to saturate softmax.
    };
    std::vector<uint8_t> outputData(5);

    armnn::InputTensors inputTensors
    {
        {0, armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())}
    };
    armnn::OutputTensors outputTensors
    {
        {0, armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results.
    BOOST_TEST(outputData[0] == 0);
    BOOST_TEST(outputData[1] == 0);
    BOOST_TEST(outputData[2] == 0);
    BOOST_TEST(outputData[3] == 255); // softmax has been saturated.
    BOOST_TEST(outputData[4] == 0);
}

BOOST_AUTO_TEST_CASE(TrivialAdd)
{
    // This test was designed to match "AddTwo" in android nn/runtime/test/TestTrivialModel.cpp.

    using namespace armnn;

    // Create runtime in which test will run
    armnn::IRuntime::CreationOptions options;
    armnn::IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // Builds up the structure of the network.
    armnn::INetworkPtr net(INetwork::Create());

    IConnectableLayer* input1 = net->AddInputLayer(0);
    IConnectableLayer* input2 = net->AddInputLayer(1);
    IConnectableLayer* add    = net->AddAdditionLayer();
    IConnectableLayer* output = net->AddOutputLayer(0);

    input1->GetOutputSlot(0).Connect(add->GetInputSlot(0));
    input2->GetOutputSlot(0).Connect(add->GetInputSlot(1));
    add->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    // Sets the tensors in the network.
    TensorInfo tensorInfo(TensorShape({3, 4}), DataType::Float32);
    input1->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    input2->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    add->GetOutputSlot(0).SetTensorInfo(tensorInfo);

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, defaultBackends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    runtime->LoadNetwork(netId, std::move(optNet));

    // Creates structures for input & output - matching android nn test.
    std::vector<float> input1Data
    {
        1.f, 2.f, 3.f, 4.f, 5.f, 6.f, 7.f, 8.f, 9.f, 10.f, 11.f, 12.f
    };
    std::vector<float> input2Data
    {
        100.f, 200.f, 300.f, 400.f, 500.f, 600.f, 700.f, 800.f, 900.f, 1000.f, 1100.f, 1200.f
    };
    std::vector<float> outputData(12);

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), input1Data.data())},
        {1,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), input2Data.data())}
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
    };

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results
    BOOST_TEST(outputData[0] == 101);
    BOOST_TEST(outputData[1] == 202);
    BOOST_TEST(outputData[2] == 303);
    BOOST_TEST(outputData[3] == 404);
    BOOST_TEST(outputData[4] == 505);
    BOOST_TEST(outputData[5] == 606);
    BOOST_TEST(outputData[6] == 707);
    BOOST_TEST(outputData[7] == 808);
    BOOST_TEST(outputData[8] == 909);
    BOOST_TEST(outputData[9] == 1010);
    BOOST_TEST(outputData[10] == 1111);
    BOOST_TEST(outputData[11] == 1212);
}

BOOST_AUTO_TEST_CASE(MultipleOutputs)
{
    using namespace armnn;

    // Create runtime in which test will run
    armnn::IRuntime::CreationOptions options;
    armnn::IRuntimePtr  runtime(armnn::IRuntime::Create(options));

    // Builds up the structure of the network.
    INetworkPtr net(INetwork::Create());

    IConnectableLayer* input = net->AddInputLayer(0);

    // ReLu1
    ActivationDescriptor activation1Descriptor;
    activation1Descriptor.m_Function = ActivationFunction::BoundedReLu;
    activation1Descriptor.m_A = 1.f;
    activation1Descriptor.m_B = -1.f;
    IConnectableLayer* activation1 = net->AddActivationLayer(activation1Descriptor);

    // ReLu6
    ActivationDescriptor activation2Descriptor;
    activation2Descriptor.m_Function = ActivationFunction::BoundedReLu;
    activation2Descriptor.m_A = 6.0f;
    IConnectableLayer* activation2 = net->AddActivationLayer(activation2Descriptor);

    // BoundedReLu(min=2, max=5)
    ActivationDescriptor activation3Descriptor;
    activation3Descriptor.m_Function = ActivationFunction::BoundedReLu;
    activation3Descriptor.m_A = 5.0f;
    activation3Descriptor.m_B = 2.0f;
    IConnectableLayer* activation3 = net->AddActivationLayer(activation3Descriptor);

    IConnectableLayer* output1 = net->AddOutputLayer(0);
    IConnectableLayer* output2 = net->AddOutputLayer(1);
    IConnectableLayer* output3 = net->AddOutputLayer(2);

    input->GetOutputSlot(0).Connect(activation1->GetInputSlot(0));
    input->GetOutputSlot(0).Connect(activation2->GetInputSlot(0));
    input->GetOutputSlot(0).Connect(activation3->GetInputSlot(0));

    activation1->GetOutputSlot(0).Connect(output1->GetInputSlot(0));
    activation2->GetOutputSlot(0).Connect(output2->GetInputSlot(0));
    activation3->GetOutputSlot(0).Connect(output3->GetInputSlot(0));

    // Sets the tensors in the network.
    TensorInfo tensorInfo(TensorShape({ 10 }), DataType::Float32);
    input->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    activation1->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    activation2->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    activation3->GetOutputSlot(0).SetTensorInfo(tensorInfo);

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, defaultBackends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    runtime->LoadNetwork(netId, std::move(optNet));

    // Creates structures for input & output.
    const std::vector<float> inputData{ 3.f, 5.f, 2.f, 3.f, 7.f, 0.f, -2.f, -1.f, 3.f, 3.f };

    std::vector<float> output1Data(inputData.size());
    std::vector<float> output2Data(inputData.size());
    std::vector<float> output3Data(inputData.size());

    InputTensors inputTensors
    {
        {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), inputData.data())}
    };
    OutputTensors outputTensors
    {
        {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), output1Data.data())},
        {1,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 1), output2Data.data())},
        {2,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 2), output3Data.data())}
    };

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results.
    BOOST_TEST(output1Data == std::vector<float>({ 1.f, 1.f, 1.f, 1.f, 1.f, 0.f, -1.f, -1.f, 1.f, 1.f })); // ReLu1
    BOOST_TEST(output2Data == std::vector<float>({ 3.f, 5.f, 2.f, 3.f, 6.f, 0.f, 0.f, 0.f, 3.f, 3.f })); // ReLu6
    BOOST_TEST(output3Data == std::vector<float>({ 3.f, 5.f, 2.f, 3.f, 5.f, 2.f, 2.f, 2.f, 3.f, 3.f })); // [2, 5]
}

BOOST_AUTO_TEST_CASE(TrivialMin)
{
    using namespace armnn;

    // Create runtime in which test will run
    armnn::IRuntime::CreationOptions options;
    armnn::IRuntimePtr runtime(armnn::IRuntime::Create(options));

    // Builds up the structure of the network.
    armnn::INetworkPtr net(INetwork::Create());

    IConnectableLayer* input1 = net->AddInputLayer(0);
    IConnectableLayer* input2 = net->AddInputLayer(1);
    IConnectableLayer* min    = net->AddMinimumLayer();
    IConnectableLayer* output = net->AddOutputLayer(0);

    input1->GetOutputSlot(0).Connect(min->GetInputSlot(0));
    input2->GetOutputSlot(0).Connect(min->GetInputSlot(1));
    min->GetOutputSlot(0).Connect(output->GetInputSlot(0));

    // Sets the tensors in the network.
    TensorInfo tensorInfo(TensorShape({1, 1, 1, 4}), DataType::Float32);
    input1->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    input2->GetOutputSlot(0).SetTensorInfo(tensorInfo);
    min->GetOutputSlot(0).SetTensorInfo(tensorInfo);

    // optimize the network
    IOptimizedNetworkPtr optNet = Optimize(*net, defaultBackends, runtime->GetDeviceSpec());

    // Loads it into the runtime.
    NetworkId netId;
    runtime->LoadNetwork(netId, std::move(optNet));

    // Creates structures for input & output - matching android nn test.
    std::vector<float> input1Data
        {
            1.0f, 2.0f, 3.0f, 4.0f
        };
    std::vector<float> input2Data
        {
            2.0f, 1.0f, 5.0f, 2.0f
        };
    std::vector<float> outputData(4);

    InputTensors inputTensors
        {
            {0,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), input1Data.data())},
            {1,armnn::ConstTensor(runtime->GetInputTensorInfo(netId, 0), input2Data.data())}
        };
    OutputTensors outputTensors
        {
            {0,armnn::Tensor(runtime->GetOutputTensorInfo(netId, 0), outputData.data())}
        };

    // Does the inference.
    runtime->EnqueueWorkload(netId, inputTensors, outputTensors);

    // Checks the results
    BOOST_TEST(outputData[0] == 1);
    BOOST_TEST(outputData[1] == 1);
    BOOST_TEST(outputData[2] == 3);
    BOOST_TEST(outputData[3] == 2);
}

BOOST_AUTO_TEST_CASE(RefEqualSimpleEndToEndTest)
{
    const std::vector<uint8_t> expectedOutput({ 1, 1, 1, 1,  0, 0, 0, 0,
                                                0, 0, 0, 0,  1, 1, 1, 1 });

    ArithmeticSimpleEndToEnd<armnn::DataType::Float32, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Equal,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefGreaterSimpleEndToEndTest)
{
    const std::vector<uint8_t> expectedOutput({ 0, 0, 0, 0,  1, 1, 1, 1,
                                                0, 0, 0, 0,  0, 0, 0, 0 });

    ArithmeticSimpleEndToEnd<armnn::DataType::Float32, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Greater,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefEqualSimpleEndToEndUint8Test)
{
    const std::vector<uint8_t> expectedOutput({ 1, 1, 1, 1,  0, 0, 0, 0,
                                                0, 0, 0, 0,  1, 1, 1, 1 });

    ArithmeticSimpleEndToEnd<armnn::DataType::QuantisedAsymm8, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Equal,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefGreaterSimpleEndToEndUint8Test)
{
    const std::vector<uint8_t> expectedOutput({ 0, 0, 0, 0,  1, 1, 1, 1,
                                                0, 0, 0, 0,  0, 0, 0, 0 });

    ArithmeticSimpleEndToEnd<armnn::DataType::QuantisedAsymm8, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Greater,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefEqualBroadcastEndToEndTest)
{
    const std::vector<uint8_t> expectedOutput({ 1, 0, 1, 1, 0, 0,
                                                0, 0, 0, 0, 0, 0 });

    ArithmeticBroadcastEndToEnd<armnn::DataType::Float32, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Equal,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefGreaterBroadcastEndToEndTest)
{
    const std::vector<uint8_t> expectedOutput({ 0, 1, 0, 0, 0, 1,
                                                1, 1, 1, 1, 1, 1 });

    ArithmeticBroadcastEndToEnd<armnn::DataType::Float32, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Greater,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefEqualBroadcastEndToEndUint8Test)
{
    const std::vector<uint8_t > expectedOutput({ 1, 0, 1, 1, 0, 0,
                                                 0, 0, 0, 0, 0, 0 });

    ArithmeticBroadcastEndToEnd<armnn::DataType::QuantisedAsymm8, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Equal,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefGreaterBroadcastEndToEndUint8Test)
{
    const std::vector<uint8_t> expectedOutput({ 0, 1, 0, 0, 0, 1,
                                                1, 1, 1, 1, 1, 1 });

    ArithmeticBroadcastEndToEnd<armnn::DataType::QuantisedAsymm8, armnn::DataType::Boolean>(defaultBackends,
                                                                                LayerType::Greater,
                                                                                expectedOutput);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim0Test)
{
    MergerDim0EndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim0Uint8Test)
{
    MergerDim0EndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim1Test)
{
    MergerDim1EndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim1Uint8Test)
{
    MergerDim1EndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim2Test)
{
    MergerDim2EndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim2Uint8Test)
{
    MergerDim2EndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim3Test)
{
    MergerDim3EndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefMergerEndToEndDim3Uint8Test)
{
    MergerDim3EndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefGatherFloatTest)
{
    GatherEndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefGatherUint8Test)
{
    GatherEndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefGatherMultiDimFloatTest)
{
    GatherMultiDimEndToEnd<armnn::DataType::Float32>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefGatherMultiDimUint8Test)
{
    GatherMultiDimEndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends);
}

BOOST_AUTO_TEST_CASE(RefDetectionPostProcessRegularNmsTest)
{
    std::vector<float> boxEncodings({
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, -1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f
    });
    std::vector<float> scores({
        0.0f, 0.9f, 0.8f,
        0.0f, 0.75f, 0.72f,
        0.0f, 0.6f, 0.5f,
        0.0f, 0.93f, 0.95f,
        0.0f, 0.5f, 0.4f,
        0.0f, 0.3f, 0.2f
    });
    std::vector<float> anchors({
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 100.5f, 1.0f, 1.0f
    });
    DetectionPostProcessRegularNmsEndToEnd<armnn::DataType::Float32>(defaultBackends, boxEncodings, scores, anchors);
}

inline void QuantizeData(uint8_t* quant, const float* dequant, const TensorInfo& info)
{
    for (size_t i = 0; i < info.GetNumElements(); i++)
    {
        quant[i] = armnn::Quantize<uint8_t>(dequant[i], info.GetQuantizationScale(), info.GetQuantizationOffset());
    }
}

BOOST_AUTO_TEST_CASE(RefDetectionPostProcessRegularNmsUint8Test)
{
    armnn::TensorInfo boxEncodingsInfo({ 1, 6, 4 }, armnn::DataType::Float32);
    armnn::TensorInfo scoresInfo({ 1, 6, 3 }, armnn::DataType::Float32);
    armnn::TensorInfo anchorsInfo({ 6, 4 }, armnn::DataType::Float32);

    boxEncodingsInfo.SetQuantizationScale(1.0f);
    boxEncodingsInfo.SetQuantizationOffset(1);
    scoresInfo.SetQuantizationScale(0.01f);
    scoresInfo.SetQuantizationOffset(0);
    anchorsInfo.SetQuantizationScale(0.5f);
    anchorsInfo.SetQuantizationOffset(0);

    std::vector<float> boxEncodings({
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, -1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f
    });
    std::vector<float> scores({
        0.0f, 0.9f, 0.8f,
        0.0f, 0.75f, 0.72f,
        0.0f, 0.6f, 0.5f,
        0.0f, 0.93f, 0.95f,
        0.0f, 0.5f, 0.4f,
        0.0f, 0.3f, 0.2f
    });
    std::vector<float> anchors({
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 100.5f, 1.0f, 1.0f
    });

    std::vector<uint8_t> qBoxEncodings(boxEncodings.size(), 0);
    std::vector<uint8_t> qScores(scores.size(), 0);
    std::vector<uint8_t> qAnchors(anchors.size(), 0);
    QuantizeData(qBoxEncodings.data(), boxEncodings.data(), boxEncodingsInfo);
    QuantizeData(qScores.data(), scores.data(), scoresInfo);
    QuantizeData(qAnchors.data(), anchors.data(), anchorsInfo);
    DetectionPostProcessRegularNmsEndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends, qBoxEncodings,
                                                                             qScores, qAnchors,
                                                                             1.0f, 1, 0.01f, 0, 0.5f, 0);
}

BOOST_AUTO_TEST_CASE(RefDetectionPostProcessFastNmsTest)
{
    std::vector<float> boxEncodings({
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, -1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f
    });
    std::vector<float> scores({
        0.0f, 0.9f, 0.8f,
        0.0f, 0.75f, 0.72f,
        0.0f, 0.6f, 0.5f,
        0.0f, 0.93f, 0.95f,
        0.0f, 0.5f, 0.4f,
        0.0f, 0.3f, 0.2f
    });
    std::vector<float> anchors({
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 100.5f, 1.0f, 1.0f
    });
    DetectionPostProcessFastNmsEndToEnd<armnn::DataType::Float32>(defaultBackends, boxEncodings, scores, anchors);
}

BOOST_AUTO_TEST_CASE(RefDetectionPostProcessFastNmsUint8Test)
{
    armnn::TensorInfo boxEncodingsInfo({ 1, 6, 4 }, armnn::DataType::Float32);
    armnn::TensorInfo scoresInfo({ 1, 6, 3 }, armnn::DataType::Float32);
    armnn::TensorInfo anchorsInfo({ 6, 4 }, armnn::DataType::Float32);

    boxEncodingsInfo.SetQuantizationScale(1.0f);
    boxEncodingsInfo.SetQuantizationOffset(1);
    scoresInfo.SetQuantizationScale(0.01f);
    scoresInfo.SetQuantizationOffset(0);
    anchorsInfo.SetQuantizationScale(0.5f);
    anchorsInfo.SetQuantizationOffset(0);

    std::vector<float> boxEncodings({
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, -1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f
    });
    std::vector<float> scores({
        0.0f, 0.9f, 0.8f,
        0.0f, 0.75f, 0.72f,
        0.0f, 0.6f, 0.5f,
        0.0f, 0.93f, 0.95f,
        0.0f, 0.5f, 0.4f,
        0.0f, 0.3f, 0.2f
    });
    std::vector<float> anchors({
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 0.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 10.5f, 1.0f, 1.0f,
        0.5f, 100.5f, 1.0f, 1.0f
    });

    std::vector<uint8_t> qBoxEncodings(boxEncodings.size(), 0);
    std::vector<uint8_t> qScores(scores.size(), 0);
    std::vector<uint8_t> qAnchors(anchors.size(), 0);
    QuantizeData(qBoxEncodings.data(), boxEncodings.data(), boxEncodingsInfo);
    QuantizeData(qScores.data(), scores.data(), scoresInfo);
    QuantizeData(qAnchors.data(), anchors.data(), anchorsInfo);
    DetectionPostProcessFastNmsEndToEnd<armnn::DataType::QuantisedAsymm8>(defaultBackends, qBoxEncodings,
                                                                          qScores, qAnchors,
                                                                          1.0f, 1, 0.01f, 0, 0.5f, 0);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "amatrix.h"

#include <complex>
#include <utility>
#include <valarray>
#include <vector>

#include "absl/types/span.h"
#include <armadillo>

namespace Visqol {
template <typename T>
inline AMatrix<T>::AMatrix(const AMatrix<T>& other) {
  matrix_ = arma::Mat<T>(other.matrix_);
}

template <typename T>
inline AMatrix<T>::AMatrix(const arma::Mat<T>& mat) {
  matrix_ = mat;
}

template <typename T>
inline AMatrix<T>::AMatrix(const std::vector<T>& col) {
  matrix_ = arma::Mat<T>(col);
}

template <typename T>
inline AMatrix<T>::AMatrix(const absl::Span<T>& col) {
  matrix_ = arma::Mat<T>(col.data(), col.size(), 1);
}

template <typename T>
inline AMatrix<T>::AMatrix(const std::valarray<T>& va) {
  std::vector<T> v;
  v.reserve(va.size());
  v.assign(std::begin(va), std::end(va));
  matrix_ = arma::Mat<T>(v);
}

template <typename T>
inline AMatrix<T>::AMatrix(const std::vector<std::vector<T>>& vecOfCols) {
  // assumes all vectors all of equal length
  matrix_ = arma::Mat<T>(vecOfCols[0].size(), vecOfCols.size());
  for (size_t i = 0; i < vecOfCols.size(); i++) {
    matrix_.col(i) = arma::Col<T>(vecOfCols[i]);
  }
}

template <typename T>
inline AMatrix<T>::AMatrix(size_t rows, size_t cols, std::vector<T>&& data) {
  matrix_ = arma::Mat<T>(&data[0], (arma::uword)rows, (arma::uword)cols, false);
}

template <typename T>
inline AMatrix<T>::AMatrix(size_t rows, size_t cols,
    const std::vector<T>& data) {
  matrix_ = arma::Mat<T>(&data[0], (arma::uword)rows, (arma::uword)cols);
}

template <typename T>
inline AMatrix<T>::AMatrix(size_t rows, size_t cols) {
  matrix_ = arma::Mat<T>((arma::uword)rows, (arma::uword)cols);
}

template <typename T>
inline AMatrix<T>::AMatrix(arma::Mat<T>&& matrix) {
  matrix_ = std::move(matrix);
}

template <typename T>
AMatrix<T> AMatrix<T>::GetSpan(size_t rowStart, size_t rowEnd, size_t colStart,
                               size_t colEnd) const {
  arma::Mat<T> m = matrix_.submat(rowStart, colStart, rowEnd, colEnd);
  return AMatrix<T>(m);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::Filled(size_t rows, size_t cols, T initialValue) {
  arma::Mat<T> m((arma::uword)rows, (arma::uword)cols);
  m.fill(initialValue);
  AMatrix<T> am(std::move(m));
  return am;
}

template <typename T>
inline T& AMatrix<T>::operator()(size_t row, size_t column) {
  return matrix_((arma::uword)row, (arma::uword)column);
}

template <typename T>
inline T AMatrix<T>::operator()(size_t row, size_t column) const {
  return matrix_((arma::uword)row, (arma::uword)column);
}

template <typename T>
inline T& AMatrix<T>::operator()(size_t elementIndex) {
  return matrix_.at((arma::uword)elementIndex);
}

template <typename T>
inline T AMatrix<T>::operator()(size_t elementIndex) const {
  return matrix_((arma::uword)elementIndex);
}

template <typename T>
inline AMatrix<T>& AMatrix<T>::operator=(const AMatrix<T>& other) {
  matrix_ = other.matrix_;
  return *this;
}

template <typename T>
inline bool AMatrix<T>::operator==(const AMatrix<T>& other) const {
  if (matrix_.n_rows != other.matrix_.n_rows) return false;
  if (matrix_.n_cols != other.matrix_.n_cols) return true;
  return std::equal(matrix_.cbegin(), matrix_.cend(), other.matrix_.cbegin());
  // Why doesn't == work when documentation says it does? matrix_ ==
  // other.matrix_;
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator+(const AMatrix<T>& other) const {
  return AMatrix<T>(matrix_ + other.matrix_);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator+(T v) const {
  return AMatrix<T>(matrix_ + v);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator*(T v) const {
  return AMatrix<T>(matrix_ * v);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator-(T v) const {
  return AMatrix<T>(matrix_ - v);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator/(T v) const {
  return AMatrix<T>(matrix_ / v);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::operator-(const AMatrix<T>& m) const {
  return AMatrix<T>(matrix_ - m.matrix_);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::PointWiseProduct(const AMatrix<T>& m) const {
  return AMatrix<T>(std::move(matrix_ % m.matrix_));
}

template <typename T>
inline AMatrix<T> AMatrix<T>::PointWiseDivide(const AMatrix<T>& m) const {
  return AMatrix<T>(std::move(matrix_ / m.matrix_));
}

template <typename T>
inline AMatrix<T> AMatrix<T>::Transpose() const {
  return AMatrix<T>{std::move(trans(matrix_))};
}

template <typename T>
inline std::vector<T> AMatrix<T>::RowSubset(size_t rowIndex,
                                            size_t startColumnIndex,
                                            size_t endColumnIndex) const {
  std::vector<T> vec(arma::conv_to<std::vector<T>>::from(matrix_.row(
    rowIndex).subvec(startColumnIndex, endColumnIndex)));
  return vec;
}

template <typename T>
inline size_t AMatrix<T>::LongestDimensionLength() const {
  return (matrix_.n_rows > matrix_.n_cols) ? matrix_.n_rows : matrix_.n_cols;
}

template <>
inline AMatrix<double> AMatrix<double>::Abs() const {
  AMatrix<double> absMatrix(matrix_.n_rows, matrix_.n_cols);
  for (size_t i = 0; i < absMatrix.NumElements(); i++) {
    absMatrix.matrix_(i) = std::abs(matrix_(i));
  }
  return absMatrix;
}

template <>
inline AMatrix<double> AMatrix<std::complex<double>>::Abs() const {
  arma::Mat<double> absMatrix;
  absMatrix.set_size(matrix_.n_rows, matrix_.n_cols);
  for (size_t i = 0; i < absMatrix.n_elem; i++) {
    absMatrix(i) = std::abs(matrix_(i));
  }
  return AMatrix<double>(std::move(absMatrix));
}

template <typename T>
inline std::vector<T> AMatrix<T>::GetRow(size_t row) const {
  std::vector<T> vec(arma::conv_to<std::vector<T>>::from(matrix_.row(row)));
  return vec;
}

template <typename T>
inline AMatrix<T> AMatrix<T>::GetRows(size_t rowStart, size_t rowEnd) const {
  return AMatrix<T>(std::move(matrix_.rows(rowStart, rowEnd)));
}

template <typename T>
inline AMatrix<T> AMatrix<T>::GetColumn(size_t column) const {
  return AMatrix<T>(matrix_.cols(column, column));
}

template <typename T>
inline AMatrix<T> AMatrix<T>::GetColumns(size_t colStart, size_t colEnd) const {
  return AMatrix<T>(matrix_.cols(colStart, colEnd));
}

template <typename T>
inline void AMatrix<T>::SetRow(size_t rowIndex, const std::vector<T>& row) {
  matrix_.row(rowIndex) = std::move(arma::Row<T>(row));;
}

template <typename T>
inline void AMatrix<T>::SetColumn(size_t colIndex, AMatrix<T>&& col) {
  matrix_.col(colIndex) = std::move(col.GetArmaMat());
}

template <typename T>
inline void AMatrix<T>::SetColumn(size_t colIndex, std::vector<T>&& col) {
  matrix_.col(colIndex) = std::move(arma::Col<T>(col));
}

template <typename T>
inline void AMatrix<T>::SetColumn(size_t colIndex, std::valarray<T>&& col) {
  std::vector<T> v;
  v.reserve(col.size());
  v.assign(std::begin(col), std::end(col));
  matrix_.col(colIndex) = std::move(arma::Col<T>(v));
}

template <typename T>
inline void AMatrix<T>::SetRow(size_t rowIndex, std::valarray<T>&& row) {
  std::vector<T> v;
  v.reserve(row.size());
  v.assign(std::begin(row), std::end(row));
  matrix_.row(rowIndex) = std::move(arma::Row<T>(v));
}

template <typename T>
void AMatrix<T>::Print() const {
  printf("\n");
  matrix_.print();
}

template <>
void AMatrix<std::complex<double>>::PrintSummary(const char* s) const {
  printf("%s\n", s);
  size_t outMatSize = matrix_.n_rows;
  printf("first five\n");
  for (size_t i = 0; i < 5; i++) {
    printf("complex[%2zu] = %9.20f , %9.20f\n", i, matrix_(i).real(),
           matrix_(i).imag());
  }
  printf("middle \n");
  for (size_t i = (outMatSize / 2) - 4; i < (outMatSize / 2) + 6; i++) {
    printf("complex[%2zu] = %9.20f , %9.20f\n", i, matrix_(i).real(),
           matrix_(i).imag());
  }
  printf("last five\n");
  for (size_t i = outMatSize - 6; i < outMatSize; i++) {
    printf("complex[%2zu] = %9.20f , %9.20f\n", i, matrix_(i).real(),
           matrix_(i).imag());
  }
}

template <>
void AMatrix<double>::PrintSummary(const char* s) const {
  printf("%s\n", s);
  size_t outMatSize = matrix_.n_rows;
  printf("first five\n");
  for (size_t i = 0; i < 5; i++) {
    printf("double[%2zu] = %9.20f\n", i, matrix_(i));
  }
  printf("middle \n");
  for (size_t i = (outMatSize / 2) - 4; i < (outMatSize / 2) + 6; i++) {
    printf("double[%2zu] = %9.20f\n", i, matrix_(i));
  }
  printf("last five\n");
  for (size_t i = outMatSize - 6; i < outMatSize; i++) {
    printf("double[%2zu] = %9.20f\n", i, matrix_(i));
  }
}

template <typename T>
inline const T* AMatrix<T>::MemPtr() const {
  return matrix_.memptr();
}

template <typename T>
inline void AMatrix<T>::Resize(const size_t rows, const size_t cols) {
  matrix_.resize(rows, cols);
}

template <typename T>
inline AMatrix<T> AMatrix<T>::JoinVertically(const AMatrix<T>& other) const {
  // Having a separate line for return is needed to cast to Mat
  arma::Mat<T> m = arma::join_vert(matrix_, other.GetArmaMat());
  return m;
}

template <typename T>
inline AMatrix<T> AMatrix<T>::FlipUpDown() const {
  return AMatrix<T>(arma::flipud(matrix_));
}

template <typename T>
inline AMatrix<T> AMatrix<T>::Mean(kDimension dim) const {
  return AMatrix<T>(arma::mean(matrix_, static_cast<int>(dim)));
}

template <typename T>
inline const arma::Mat<T>& AMatrix<T>::GetArmaMat() const {
  return matrix_;
}

template <typename T>
inline std::vector<T> AMatrix<T>::ToVector() const {
  return arma::conv_to<std::vector<T>>::from(matrix_.col(0));
}

template <typename T>
inline std::valarray<T> AMatrix<T>::ToValArray() const {
  std::vector<T> v = arma::conv_to<std::vector<T>>::from(matrix_.col(0));
  return std::valarray<T>(v.data(), v.size());
}

template <>
inline std::vector<std::complex<double>>
    AMatrix<std::complex<double>>::ToVector() const {
  return arma::conv_to<std::vector<std::complex<double>>>::from(matrix_.col(0));
}

template <typename T>
inline size_t AMatrix<T>::NumRows() const {
  return matrix_.n_rows;
}
template <typename T>
inline size_t AMatrix<T>::NumCols() const {
  return matrix_.n_cols;
}
template <typename T>
inline size_t AMatrix<T>::NumElements() const {
  return matrix_.n_elem;
}

template <typename T>
inline typename AMatrix<T>::iterator AMatrix<T>::begin() {
  return matrix_.begin();
}
template <typename T>
inline typename AMatrix<T>::iterator AMatrix<T>::end() {
  return matrix_.end();
}
template <typename T>
inline typename AMatrix<T>::const_iterator AMatrix<T>::cbegin() const {
  return matrix_.cbegin();
}
template <typename T>
inline typename AMatrix<T>::const_iterator AMatrix<T>::cend() const {
  return matrix_.cend();
}

template <typename T>
inline const T* AMatrix<T>::data() const {
  return matrix_.mem;
}
template <typename T>
inline T* AMatrix<T>::mutData() const {
  return const_cast<T*>(matrix_.mem);
}

template class AMatrix<double>;
template class AMatrix<std::complex<double>>;

}  // namespace Visqol


// Copyright Abel Sinkovics (abel@sinkovics.hu) 2013.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/instantiate.hpp>
#include <mpllibs/metamonad/tmp_value.hpp>
#include <mpllibs/metamonad/apply.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>
#include <boost/mpl/assert.hpp>

#include <boost/type_traits.hpp>

using mpllibs::metamonad::tmp_value;

namespace
{
  template <class A, class B>
  struct some_template : tmp_value<some_template<A, B> > {};
}

BOOST_AUTO_TEST_CASE(test_instantiate)
{
  using mpllibs::metamonad::instantiate2;
  using mpllibs::metamonad::apply;

  using boost::mpl::vector;
  using boost::mpl::na;

  using boost::is_same;

  // test_instantiation
  BOOST_MPL_ASSERT((
    is_same<
      some_template<int, double>,
      apply<instantiate2<some_template>, vector<int, double> >::type
    >
  ));

  // test_instantiation_with_na
  BOOST_MPL_ASSERT((
    is_same<
      some_template<int, na>,
      apply<instantiate2<some_template>, vector<int> >::type
    >
  ));
}


#include "util/indexed_data.hpp"
#include "common/temporary_file.hpp"
#include "util/exception.hpp"

#include <boost/test/unit_test.hpp>

#include <iomanip>
#include <iostream>
#include <sstream>
#include <typeinfo>
#include <vector>

BOOST_AUTO_TEST_SUITE(indexed_data)

using namespace osrm;
using namespace osrm::util;

BOOST_AUTO_TEST_CASE(check_variable_group_block_bitops)
{
    VariableGroupBlock<16> variable_group_block;
    BOOST_CHECK_EQUAL(variable_group_block.sum2bits(0xe4), 6);
    BOOST_CHECK_EQUAL(variable_group_block.sum2bits(0x11111111), 8);
    BOOST_CHECK_EQUAL(variable_group_block.sum2bits(0x55555555), 16);
    BOOST_CHECK_EQUAL(variable_group_block.sum2bits(0xffffffff), 48);

    BOOST_CHECK_EQUAL(variable_group_block.log256(0), 0);
    BOOST_CHECK_EQUAL(variable_group_block.log256(1), 1);
    BOOST_CHECK_EQUAL(variable_group_block.log256(255), 1);
    BOOST_CHECK_EQUAL(variable_group_block.log256(256), 2);
    BOOST_CHECK_EQUAL(variable_group_block.log256(1024), 2);
    BOOST_CHECK_EQUAL(variable_group_block.log256(16777215), 3);
}

template <typename IndexedData, typename Offsets, typename Data>
void test_rw(const Offsets &offsets, const Data &data)
{
    IndexedData indexed_data(offsets.begin(), offsets.end(), data.begin());

    for (std::size_t index = 0; index < offsets.size() - 1; ++index)
    {
        typename IndexedData::ResultType expected_result(&data[offsets[index]],
                                                         &data[offsets[index + 1]]);
        BOOST_CHECK_EQUAL(expected_result, indexed_data.at(index));
    }
}

BOOST_AUTO_TEST_CASE(check_group_blocks_with_different_sizes)
{

    std::vector<std::string> str = {
        "",     "A", "bb", "ccc", "dDDd", "E", "ff", "ggg", "hhhh", "I", "jj", "",  "kkk",
        "llll", "M", "nn", "ooo", "pppp", "q", "r",  "S",   "T",    "",  "u",  "V", "W",
        "X",    "Y", "Z",  "",    "",     "",  "",   "",    "",     "",  "0",  ""};

    std::vector<unsigned char> name_char_data;
    std::vector<std::uint32_t> name_offsets;
    for (auto s : str)
    {
        name_offsets.push_back(name_char_data.size());
        std::copy(s.begin(), s.end(), std::back_inserter(name_char_data));
    }
    name_offsets.push_back(name_char_data.size());

    test_rw<IndexedData<VariableGroupBlock<0, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<VariableGroupBlock<1, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<VariableGroupBlock<16, std::string>>>(name_offsets, name_char_data);

    test_rw<IndexedData<FixedGroupBlock<0, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<FixedGroupBlock<1, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<FixedGroupBlock<16, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<FixedGroupBlock<32, std::string>>>(name_offsets, name_char_data);
    test_rw<IndexedData<FixedGroupBlock<128, std::string>>>(name_offsets, name_char_data);
}

BOOST_AUTO_TEST_CASE(check_1001_pandas)
{
    std::vector<unsigned char> name_char_data;
    std::vector<std::uint32_t> name_offsets;

    const std::string panda = "🐼";
    name_offsets.push_back(0);
    for (std::size_t i = 0; i < 1000; ++i)
        std::copy(panda.begin(), panda.end(), std::back_inserter(name_char_data));
    name_offsets.push_back(name_char_data.size());
    std::copy(panda.begin(), panda.end(), std::back_inserter(name_char_data));
    name_offsets.push_back(name_char_data.size());

    test_rw<IndexedData<VariableGroupBlock<16, std::string>>>(name_offsets, name_char_data);
}

BOOST_AUTO_TEST_CASE(check_different_sizes)
{
    for (std::size_t num_strings = 0; num_strings < 256; ++num_strings)
    {
        std::vector<unsigned char> name_char_data;
        std::vector<std::uint32_t> name_offsets;

        const std::string canoe = "🛶";
        name_offsets.push_back(0);
        for (std::size_t i = 0; i < num_strings; ++i)
        {
            std::copy(canoe.begin(), canoe.end(), std::back_inserter(name_char_data));
            name_offsets.push_back(name_char_data.size());
        }

        test_rw<IndexedData<VariableGroupBlock<16, std::string>>>(name_offsets, name_char_data);
        test_rw<IndexedData<FixedGroupBlock<16, std::string>>>(name_offsets, name_char_data);
    }
}

BOOST_AUTO_TEST_CASE(check_max_size)
{
    std::vector<unsigned char> name_data(0x1000000, '#');
    std::vector<std::uint32_t> name_offsets;

    auto test_variable = [&name_offsets, &name_data]() {
        test_rw<IndexedData<VariableGroupBlock<16, std::string>>>(name_offsets, name_data);
    };
    auto test_fixed = [&name_offsets, &name_data]() {
        test_rw<IndexedData<FixedGroupBlock<16, std::string>>>(name_offsets, name_data);
    };

    name_offsets = {0, 0x1000000};
    BOOST_CHECK_THROW(test_variable(), osrm::util::exception);
    name_offsets = {0, 0x1000000 - 1};
    test_variable();

    name_offsets = {0, 256};
    BOOST_CHECK_THROW(test_fixed(), osrm::util::exception);
    name_offsets = {0, 255};
    BOOST_CHECK_NO_THROW(test_fixed());
}

BOOST_AUTO_TEST_CASE(check_string_view)
{
    std::string name_data = "hellostringview";
    std::vector<std::uint32_t> name_offsets = {0, 5, 11, 15};

    IndexedData<VariableGroupBlock<16, StringView>> indexed_data(
        name_offsets.begin(), name_offsets.end(), name_data.begin());

    BOOST_CHECK_EQUAL(indexed_data.at(0), "hello");
    BOOST_CHECK_EQUAL(indexed_data.at(1), "string");
    BOOST_CHECK_EQUAL(indexed_data.at(2), "view");
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/hana/fwd/sum.hpp>


#include <boost/units/physical_dimensions/wavenumber.hpp>


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file pbes_test.cpp
/// \brief Add your file description here.

#define MCRL2_DEBUG_ENUMERATE_QUANTIFIERS_BUILDER

#include <sstream>
#include <iostream>
#include <iterator>
#include <utility>
#include <set>
#include <cstdio>
#include <boost/test/minimal.hpp>
#include <boost/algorithm/string.hpp>
#include "mcrl2/atermpp/aterm_io.h"
#include "mcrl2/data/detail/print_utility.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/modal_formula/parse.h"
#include "mcrl2/pbes/find.h"
#include "mcrl2/pbes/is_bes.h"
#include "mcrl2/pbes/pbes.h"
#include "mcrl2/pbes/parse.h"
#include "mcrl2/pbes/lps2pbes.h"
#include "mcrl2/pbes/txt2pbes.h"
#include "mcrl2/pbes/io.h"
#include "mcrl2/pbes/complement.h"
#include "mcrl2/pbes/detail/instantiate_global_variables.h"

using namespace mcrl2;
using atermpp::make_vector;
using core::identifier_string;
using data::data_expression;
using data::variable;
using data::basic_sort;
using data::multiset_identifier_generator;
using state_formulas::state_formula;
using lps::linearise;
using lps::specification;
using pbes_system::pbes;
using pbes_system::pbes_expression;
using pbes_system::pbes_equation;
using pbes_system::lps2pbes;
using pbes_system::propositional_variable_instantiation;

const std::string SPECIFICATION =
  "act a:Nat;                               \n"
  "                                         \n"
  "map smaller: Nat#Nat -> Bool;            \n"
  "                                         \n"
  "var x,y : Nat;                           \n"
  "                                         \n"
  "eqn smaller(x,y) = x < y;                \n"
  "                                         \n"
  "proc P(n:Nat) = sum m: Nat. a(m). P(m);  \n"
  "                                         \n"
  "init P(0);                               \n";

const std::string TRIVIAL_FORMULA  = "[true*]<true*>true";

const std::string MPSU_SPECIFICATION =
  "% This file describes a controller for a simplified Movable Patient                   \n"
  "% Support Unit. It is described in Fokkink, Groote and Reniers,                       \n"
  "% Modelling reactive systems.                                                         \n"
  "%                                                                                \n"
  "% Jan Friso Groote, September, 2006.                                                  \n"
  "                                                                                \n"
  "sort Mode = struct Normal | Emergency ;                                               \n"
  "     MotorStatus = struct turnleft | turnright | stopped ;                            \n"
  "                                                                                \n"
  "act pressStop, pressResume,                                                           \n"
  "    pressUndock, pressLeft,                                                           \n"
  "    pressRight, motorLeft,                                                            \n"
  "    motorRight, motorOff,                                                             \n"
  "    applyBrake, releaseBrake,                                                         \n"
  "    isDocked, unlockDock,                                                             \n"
  "    atInnermost, atOutermost;                                                         \n"
  "                                                                                \n"
  "proc Controller(m:Mode,docked,rightmost,leftmost:Bool,ms:MotorStatus)=                \n"
  "       pressStop.Controller(Emergency,docked,rightmost,leftmost,ms)+                  \n"
  "       pressResume.Controller(Normal,docked,rightmost,leftmost,ms)+                   \n"
  "       pressUndock.                                                                   \n"
  "         (docked && rightmost)                                                        \n"
  "                -> applyBrake.unlockDock.Controller(m,false,rightmost,leftmost,ms)    \n"
  "                <> Controller(m,docked,rightmost,leftmost,ms)+                        \n"
  "       pressLeft.                                                                     \n"
  "          (docked && ms!=turnleft && !leftmost && m==Normal)                          \n"
  "                -> releaseBrake.motorLeft.                                            \n"
  "                     Controller(m,docked,false,leftmost,turnleft)                     \n"
  "                <> Controller(m,docked,rightmost,leftmost,ms)+                        \n"
  "       pressRight.                                                                    \n"
  "          (docked && ms!=turnright && !rightmost && m==Normal)                        \n"
  "                -> releaseBrake.motorRight.                                           \n"
  "                     Controller(m,docked,rightmost,false,turnright)                   \n"
  "                <> Controller(m,docked,rightmost,leftmost,ms)+                        \n"
  "       isDocked.Controller(m,true,rightmost,leftmost,ms)+                             \n"
  "       atInnermost.motorOff.applyBrake.Controller(m,docked,true,false,stopped)+       \n"
  "       atOutermost.motorOff.applyBrake.Controller(m,docked,false,true,stopped);       \n"
  "                                                                                \n"
  "                                                                                \n"
  "                                                                                \n"
  "init Controller(Normal,true,false,false,stopped);                                     \n"
  ;

const std::string FORMULA  = "nu X(n:Nat = 1). [forall m:Nat. a(m)](val(n < 10)  && X(n+2))";
const std::string FORMULA2 = "forall m:Nat. [a(m)]false";

const std::string MPSU_FORMULA =
  "% This file describes the modal formulas for property 5 used in section \n"
  "% 5.3 of Designing and understanding the behaviour of systems           \n"
  "% by J.F. Groote and M.A. Reniers.                                      \n"
  "                                                                        \n"
  "nu X(b1:Bool=false, b2:Bool=true,b3:Bool=true,b4:Bool=true).            \n"
  "        val(b1 && b2 && b3 && b4) => ([pressLeft]                       \n"
  "              (mu Y.[!motorLeft &&                                      \n"
  "                    !unlockDock &&                                      \n"
  "                    !pressStop &&                                       \n"
  "                    !atInnermost]Y))                                    \n";

void test_pbes()
{
  specification spec=remove_stochastic_operators(linearise(SPECIFICATION));
  state_formula formula = state_formulas::parse_state_formula(FORMULA2, spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  pbes_expression e = p.equations().front().formula();

  BOOST_CHECK(!is_bes(p));
  BOOST_CHECK(!is_bes(e));

  try
  {
    load_pbes(p, "non-existing file");
    BOOST_CHECK(false); // loading is expected to fail
  }
  catch (mcrl2::runtime_error e)
  {
  }

  std::string filename = "write_term_to_text_stream.pbes";
  try
  {
    atermpp::aterm t = atermpp::read_term_from_string("f(x)");
    std::ofstream os;
    os.open(filename.c_str());
    atermpp::write_term_to_text_stream(t, os);
    os.close();
    load_pbes(p, filename);
    BOOST_CHECK(false); // loading is expected to fail
  }
  catch (mcrl2::runtime_error e)
  {
    remove(filename.c_str());
  }
  filename = "pbes_test_file.pbes";
  save_pbes(p, filename);
  load_pbes(p, filename);
  remove(filename.c_str());
}

void test_global_variables()
{
  std::string TEXT =
    "glob k, m, n:Nat;                        \n"
    "                                         \n"
    "pbes                                     \n"
    "   mu X1(n1, m1:Nat) = X2(n1) || X2(m1); \n"
    "   mu X2(n2:Nat)     = X1(n2, n);        \n"
    "                                         \n"
    "init                                     \n"
    "   X1(m, n);                             \n"
    ;

  pbes p;
  std::stringstream s(TEXT);
  s >> p;
  std::set<variable> freevars = p.global_variables();
  BOOST_CHECK(freevars.size() == 3);  // The global variable k does not occur in the specification,
  // but occurs in the global variables list.
}

void test_complement_method_builder()
{
  using namespace pbes_system;
  namespace d = data::sort_bool;

  variable X("x", data::sort_bool::bool_());
  variable Y("y", data::sort_bool::bool_());

  pbes_expression p = or_(and_(X,Y), and_(Y,X));
  pbes_expression q = and_(or_(d::not_(X), d::not_(Y)), or_(d::not_(Y),d::not_(X)));
  std::cout << "p             = " << mcrl2::pbes_system::pp(p) << std::endl;
  std::cout << "q             = " << mcrl2::pbes_system::pp(q) << std::endl;
  std::cout << "complement(p) = " << mcrl2::pbes_system::pp(complement(p)) << std::endl;
  BOOST_CHECK(complement(p) == q);
}

void test_pbes_expression()
{
  variable x1("x1", basic_sort("X"));
  pbes_expression e = x1;
}

void test_trivial()
{
  specification spec=remove_stochastic_operators(linearise(lps::detail::ABP_SPECIFICATION()));
  state_formula formula = state_formulas::parse_state_formula(TRIVIAL_FORMULA, spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  BOOST_CHECK(p.is_well_typed());
}

void test_instantiate_global_variables()
{
  std::string spec_text =
    "act a,b:Nat;             \n"
    "    d;                   \n"
    "proc P(n:Nat)=a(n).delta;\n"
    "init d.P(1);             \n"
    ;
  std::string formula_text = "([true*.a(1)]  (mu X.([!a(1)]X && <true> true)))";
  specification spec=remove_stochastic_operators(linearise(spec_text));
  state_formula formula = state_formulas::parse_state_formula(formula_text, spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  std::cout << "<before>" << mcrl2::pbes_system::pp(p) << std::endl;
  std::cout << "<lps>" << lps::pp(spec) << std::endl;
  pbes_system::detail::instantiate_global_variables(p);
  std::cout << "<after>" << pbes_system::pp(p) << std::endl;
}

void test_find_sort_expressions()
{
  using data::sort_expression;

  specification spec=remove_stochastic_operators(linearise(lps::detail::ABP_SPECIFICATION()));
  state_formula formula = state_formulas::parse_state_formula(TRIVIAL_FORMULA, spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  std::set<sort_expression> s;
  pbes_system::find_sort_expressions(p, std::inserter(s, s.end()));
  std::cout << core::detail::print_set(s) << std::endl;
}

void test_io()
{
  using namespace pbes_system;

  std::string PBES_SPEC =
    "pbes nu X1 = X2; \n"
    "     mu X2 = X2; \n"
    "                 \n"
    "init X1;         \n"
    ;
  pbes p = txt2pbes(PBES_SPEC);
  save_pbes(p, "pbes.pbes",  pbes_format_internal());
  load_pbes(p, "pbes.pbes",  pbes_format_internal());
  save_pbes(p, "pbes.aterm", pbes_format_internal_text());
  load_pbes(p, "pbes.aterm", pbes_format_internal_text());
  save_pbes(p, "pbes.txt",   pbes_format_text());
  load_pbes(p, "pbes.txt",   pbes_format_text());
}

void test_is_bes()
{
  // found with random testing 14-1-2011
  using namespace pbes_system;
  std::string text =
    "pbes nu X =       \n"
    "       val(true); \n"
    "                  \n"
    "init X;           \n"
    ;
  pbes p = txt2pbes(text);
  BOOST_CHECK(is_bes(p));
}

int test_main(int argc, char** argv)
{
  test_trivial();
  test_pbes();
  test_global_variables();
  test_complement_method_builder();
  test_pbes_expression();
  test_instantiate_global_variables();
  test_find_sort_expressions();
  test_is_bes();
  test_io();

  return 0;
}


/*=============================================================================

  SKSURGERYPCLCPP: Image-guided surgery functions, in C++, using PCL.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "sksFeatureMatchWrapper.h"
#include "sksExceptionMacro.h"
#include "sksConversionUtils.h"
#include <sksFeatureMatch.h>
#include <boost/python/extract.hpp>

namespace sks
{

//-----------------------------------------------------------------------------
void FeatureMatchWrapper(const np::ndarray& source,
                         const np::ndarray& target,
                         float siftNormalSearchRadius,
                         float siftMinScale,
                         unsigned int siftNumOctaves,
                         unsigned int siftNumScalesPerOctave,
                         float siftMinContrast,
                         unsigned int siftKSearch,
                         float ransacInlierThreshold,
                         unsigned int ransacMaximumIterations,
                         np::ndarray& result,
                         np::ndarray& transformedSource
                        )
{
  CheckInputsForRegistration(source, target, transformedSource, result);

  pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud = ConvertInputToPointCloud(source);
  pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud = ConvertInputToPointCloud(target);
  pcl::PointCloud<pcl::PointXYZ>::Ptr transformedSourceCloud = ConvertInputToPointCloud(transformedSource);

  Eigen::Matrix4f finalTransform;

  sks::FeatureMatch(sourceCloud,
                    targetCloud,
                    siftNormalSearchRadius,
                    siftMinScale,
                    siftNumOctaves,
                    siftNumScalesPerOctave,
                    siftMinContrast,
                    siftKSearch,
                    ransacInlierThreshold,
                    ransacMaximumIterations,
                    finalTransform,
                    transformedSourceCloud);

  ConvertEigenToNumpy(finalTransform, result);
  ConvertPointCloudToNumpy(transformedSourceCloud, transformedSource);
}

} // end namespace


/*
 *  Copyright (C) 2018 Naomasa Matsubayashi
 *  Licensed under MIT license, see file LICENSE in this source tree.
 */
#ifndef UWG_TRANSPORT_HPP
#define UWG_TRANSPORT_HPP
#include <iterator>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/qi.hpp>
#include <uwg/hash.hpp>
#include <uwg/mac.hpp>
#include <uwg/kdf.hpp>
#include <uwg/dh.hpp>
#include <uwg/aead.hpp>
#include <uwg/timestamp.hpp>
#include <uwg/initial_key.hpp>
#include <uwg/clear_key.hpp>
#include <uwg/types.hpp>
#include <uwg/dump.hpp>
namespace uwg {
  template< typename Out, typename In >
  void encrypt_data( Out &packet, const key_state &key, In &&plain ) {
    const auto packet_size = plain.size()  / 16 * 16 + ( plain.size() % 16 ? 16 : 0 );
    packet.clear();
    packet.resize( packet_size + wg_aead_hash_len + wg_counter_len + wg_spi_len + wg_header_len, 0u );
    packet[ 0 ] = 0x04;
    namespace karma = boost::spirit::karma;
    karma::generate( std::next( packet.begin(), wg_transport_remote_spi_offset ), karma::little_dword, key.remote_kxid );
    karma::generate( std::next( packet.begin(), wg_transport_counter_offset ), karma::little_qword, key.tx_count );
    plain.resize( packet_size, 0u );
    wg_key_type empty;
    auto encrypted = make_svv( packet.data(), wg_transport_packet_offset, packet_size + wg_aead_hash_len );
    aead_enc( encrypted, key.send_key, key.tx_count, plain, empty );
  }
  template< typename Out, typename In >
  bool decrypt_data( Out &plain, key_state &key, const In &packet ) {
    if( packet.size() < wg_transport_packet_offset + wg_aead_hash_len )
      return false;
    if( packet[ 0 ] != 0x04 )
      return false;
    namespace qi = boost::spirit::qi;
    uint64_t counter = 0;
    qi::parse( std::next( packet.begin(), wg_transport_counter_offset ), std::next( packet.begin(), wg_transport_counter_offset + wg_counter_len ), qi::little_qword, counter );
    wg_key_type empty;
    auto encrypted = make_svv( packet.data(), wg_transport_packet_offset, packet.size() - wg_transport_packet_offset );
    if( !aead_dec( plain, key.receive_key, counter, encrypted, empty ) )
      return false;
    if( plain.size() >= 4u ) {
      uint16_t length = 0;
      qi::parse( std::next( plain.begin(), 2 ), std::next( plain.begin(), 4 ), qi::big_word, length );
      if( plain.size() < length )
        return false;
      plain.resize( length );
    }
    return true;
  }
}

#endif


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

#include <nt2/core/functions/sub2ind.hpp>
#include <nt2/sdk/parameters.hpp>
#include <nt2/core/container/dsl.hpp>
#include <nt2/core/utility/as_index.hpp>
#include <nt2/include/functions/plus.hpp>
#include <nt2/include/functions/minus.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/touint.hpp>
#include <nt2/include/functions/as_size.hpp>
#include <boost/fusion/include/make_vector.hpp>
#include <boost/preprocessor/arithmetic/inc.hpp>
#include <boost/preprocessor/repetition/repeat.hpp>
#include <boost/preprocessor/iteration/iterate.hpp>
#include <boost/preprocessor/repetition/enum.hpp>
#include <boost/preprocessor/repetition/enum_params.hpp>

namespace nt2 { namespace ext
{
   #define BOOST_PP_FILENAME_1 "nt2/core/functions/generic/sub2ind.hpp"
   #define BOOST_PP_ITERATION_LIMITS (2, BOOST_PP_INC(NT2_MAX_DIMENSIONS))
   #include BOOST_PP_ITERATE()
} }

#endif

#else

  #define N BOOST_PP_ITERATION()

  #define M0(z,n,t) (t<arithmetic_<A1> >)
  #define M1(z,n,t) nt2::touint(a##n - one)

  /// INTERNAL ONLY - sub2ind(ettent,D1,D2)
  BOOST_DISPATCH_IMPLEMENT  ( sub2ind_, tag::cpu_
                            , (SZ)(A1)
                            , (fusion_sequence_<SZ>)
                              BOOST_PP_REPEAT(N,M0,generic_)
                            )
  {
    typedef A1 result_type;

    BOOST_FORCEINLINE
    result_type operator()(SZ const& sz, BOOST_PP_ENUM_PARAMS(N,A1 const& a) ) const
    {
      A1 one  = One<A1>();
      A1 that = one
              + splat<A1> ( as_index ( sz, boost::fusion::make_vector
                                        ( BOOST_PP_ENUM(N, M1, ~) )
                                    )
                          );
      return that;
    }
  };

  /// INTERNAL ONLY - ind2sub(size,IND)
  BOOST_DISPATCH_IMPLEMENT  ( sub2ind_, tag::cpu_
                            , (SZ)(A1)
                            , ((ast_<SZ,nt2::container::domain>))
                              BOOST_PP_REPEAT(N,M0,scalar_)
                            )
  {
    BOOST_DISPATCH_RETURNS( BOOST_PP_INC(N)
                          , (SZ const& sz, BOOST_PP_ENUM_PARAMS(N,A1 a) )
                          , ( nt2::sub2ind( nt2::as_size(sz)
                                          , BOOST_PP_ENUM_PARAMS(N,a)
                                          )
                            )
                          )
  };

  #undef M1
  #undef M0
  #undef N

#endif


// Copyright 2020 Peter Dimov
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt

#if defined(_MSC_VER) && _MSC_VER == 1900
# pragma warning(disable: 4552) // '<<': operator has no effect; expected operator with side-effect
#endif

#include <boost/lambda2.hpp>
#include <boost/core/lightweight_test.hpp>

#define TEST_UNARY(Op) \
    BOOST_TEST_EQ( (Op _1)( 0 ), Op 0 ); \
    BOOST_TEST_EQ( (Op _1)( 1 ), Op 1 ); \
    BOOST_TEST_EQ( (Op _1)( 5 ), Op 5 ); \
    BOOST_TEST_EQ( (Op (Op _1))( 5 ), Op (Op 5) ); \
    BOOST_TEST_EQ( ((Op _1) + (Op _2))( 1, 2 ), (Op 1) + (Op 2) ); \
    BOOST_TEST_EQ( ((Op _1) && (Op _2))( 1, 2 ), (Op 1) && (Op 2) );

#define TEST_BINARY(Op) \
    BOOST_TEST_EQ( (_1 Op _2)( 0, 1 ), 0 Op 1 ); \
    BOOST_TEST_EQ( (_1 Op _2)( 3, 5 ), 3 Op 5 ); \
    BOOST_TEST_EQ( (_1 Op 2)( 1, 2 ), 1 Op 2 ); \
    BOOST_TEST_EQ( (1 Op _2)( 1, 2 ), 1 Op 2 ); \
    BOOST_TEST_EQ( ((_1 Op _2) + (_3 Op _4))( 1, 2, 3, 4 ), (1 Op 2) + (3 Op 4) );

int main()
{
    using namespace boost::lambda2;

    TEST_BINARY(+)
    TEST_BINARY(-)
    TEST_BINARY(*)
    TEST_BINARY(/)
    TEST_BINARY(%)
    TEST_UNARY(-)

    TEST_BINARY(==)
    TEST_BINARY(!=)
    TEST_BINARY(>)
    TEST_BINARY(<)
    TEST_BINARY(>=)
    TEST_BINARY(<=)

    TEST_BINARY(&&)
    TEST_BINARY(||)
    TEST_UNARY(!)

    TEST_BINARY(&)
    TEST_BINARY(|)
    TEST_BINARY(^)
    TEST_UNARY(~)

#if defined(_MSC_VER) && _MSC_VER == 1900 && !defined(_DEBUG)

    // prevents crash in TEST_BINARY(/)
    // no idea why

    BOOST_TEST_EQ( (_1 / _2)( 0, 1 ), 0 / 1 );

#endif

    TEST_BINARY(<<)
    TEST_BINARY(>>)

    TEST_UNARY(+)

    return boost::report_errors();
}


// -*- C++ -*-
//
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/AbstractSQ.h"
#include "mccomponents/kernels/sample/SQ/GridSQ.h"

namespace wrap_mccomponents {

  using namespace mccomponents::sample;

  double fx_get( const fx & fx, const double & x)
  {
    return fx(x);
  }

  fx * new_fx( double xbegin, double xend, double xstep,
	       std::vector<double> & v )
  {
    return new fx( xbegin, xend, xstep, v.begin() );
  }

  void wrap_GridSQ()
  {
    using namespace boost::python;

    typedef GridSQ w_t;

    class_<w_t, bases<mccomponents::sample::AbstractSQ>, boost::noncopyable >
      ("GridSQ",
       init<const fx &>()
       [with_custodian_and_ward<1,2> () ]
       )
      ;

    class_<fx>
      ("fx", no_init)
      .def( "__call__", fx_get)
      ;

    def( "new_fx", new_fx,
         with_custodian_and_ward_postcall<0,4,
         return_value_policy< manage_new_object > > () )
      ;

  }

}

// End of file


#include <iostream>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <Eigen/Geometry>

#include "Common.h"
#include "camera/Camera.hpp"
#include "camera/RealCamera.hpp"
#include "calibration/CameraPoseCalculator.h"
#include "mask/OFForegroundMaskCalculator.h"
#include "object/MultiObjectSelector.h"
#include "optical_flow/OpticalFlowCalculator.h"
#include "optical_flow/ReportOpticalFlowCalculator.h"
#include "Triangulator.h"
#include "Visualization.h"

using namespace cv;
using namespace std;

std::vector<std::vector<Mat>> getMasks(std::vector<Ptr<Camera>> &cameras,
                                       std::vector<Ptr<ForegroundMaskCalculator>> &maskCalculators) {

    std::vector<std::vector<Mat>> selected(2);

    selected[0].resize(2);
    selected[1].resize(2);

#pragma omp parallel for
    for (int i = 0; i < 2; i++) {
        // std::cout << "CAP THREAD: " << omp_get_thread_num() << std::endl;
        Mat image, mask;
        cameras[i]->read(image); // readUndistorted
        mask = maskCalculators[i]->calculate(image);

        Mat gray, equal;
        cvtColor(image, gray, COLOR_BGR2GRAY);
        equalizeHist(gray, equal);

        selected[0][i] = equal;
        selected[1][i] = mask;
    }

    return selected;
}

enum VIS_TYPE {
    PIXELS, DEPTH, CONTOURS
};

int main(int argc, char **argv) {

    vector<Ptr<Camera>> camera(2);
    camera[Camera::LEFT] = Ptr<Camera>(
            new RealCamera(Camera::LEFT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_left.yml"));
    camera[Camera::RIGHT] = Ptr<Camera>(
            new RealCamera(Camera::RIGHT, "/media/balint/Data/Linux/diploma/src/final/intrinsics_right.yml"));

    vector<CameraPose> cameraPose(2);
    cameraPose[Camera::LEFT].load("/media/balint/Data/Linux/diploma/src/final/pose_left.yml");
    cameraPose[Camera::RIGHT].load("/media/balint/Data/Linux/diploma/src/final/pose_right.yml");

    std::vector<Ptr<ForegroundMaskCalculator>> maskCalculators(2);
    maskCalculators[Camera::LEFT] = Ptr<ForegroundMaskCalculator>(new OFForegroundMaskCalculator());
    maskCalculators[Camera::RIGHT] = Ptr<ForegroundMaskCalculator>(new OFForegroundMaskCalculator());

    int focus = 80;
    static_cast<RealCamera *>(camera[Camera::LEFT].get())->focus(focus);
    static_cast<RealCamera *>(camera[Camera::RIGHT].get())->focus(focus);

    FileStorage fs;
    fs.open("/media/balint/Data/Linux/diploma/F.yml", FileStorage::READ);
    Mat F;
    fs["myF"] >> F;

    Matcher matcher = Matcher(camera[Camera::LEFT], camera[Camera::RIGHT], F);
    MultiObjectSelector objSelector(matcher);

//    long frame = 0;
//
//    while (true) {
//        //double t0 = getTickCount();
//
//        frame++;
//
//        std::vector<std::vector<Mat>> frames = getMasks(camera, maskCalculators);
//
//        imshow("frame1", frames[0][0]);
//        imshow("frame2", frames[0][1]);
//        imshow("mask1", frames[1][0]);
//        imshow("mask2", frames[1][1]);
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (frame % 20 == 0) {
//            imwrite("/media/balint/Data/Linux/multi_left.png", frames[0][0]);
//            imwrite("/media/balint/Data/Linux/multi_right.png", frames[0][1]);
//
//            imwrite("/media/balint/Data/Linux/multi_left_mask.png", frames[1][0]);
//            imwrite("/media/balint/Data/Linux/multi_right_mask.png", frames[1][1]);
//        }
//    }

//    std::vector<Mat> frames(2);
//    frames[0] = imread("/media/balint/Data/Linux/frame_left_230.png");//, IMREAD_GRAYSCALE);
//    frames[1] = imread("/media/balint/Data/Linux/frame_right_230.png");//, IMREAD_GRAYSCALE);
//
//    std::vector<Mat> masks(2);
//    masks[0] = imread("/media/balint/Data/Linux/mask_left_230.png", IMREAD_GRAYSCALE);
//    masks[1] = imread("/media/balint/Data/Linux/mask_right_230.png", IMREAD_GRAYSCALE);

//    std::vector<Mat> frames(2);
//    frames[0] = imread("/media/balint/Data/Linux/__MULTI/_multi_left.png", IMREAD_GRAYSCALE);
//    frames[1] = imread("/media/balint/Data/Linux/__MULTI/_multi_right.png", IMREAD_GRAYSCALE);
//
//    std::vector<Mat> masks(2);
//    masks[0] = imread("/media/balint/Data/Linux/__MULTI/_multi_left_mask.png", IMREAD_GRAYSCALE);
//    masks[1] = imread("/media/balint/Data/Linux/__MULTI/_multi_right_mask.png", IMREAD_GRAYSCALE);

    std::vector<Mat> frames(2);
    frames[0] = imread("/media/balint/Data/Linux/for_report/120_1left_img.png");//, IMREAD_GRAYSCALE);
    frames[1] = imread("/media/balint/Data/Linux/for_report/120_3right_img.png");//, IMREAD_GRAYSCALE);

    std::vector<Mat> masks(2);
    masks[0] = imread("/media/balint/Data/Linux/for_report/120_2left_mask.png", IMREAD_GRAYSCALE);
    masks[1] = imread("/media/balint/Data/Linux/for_report/120_4right_mask.png", IMREAD_GRAYSCALE);

    //while (true) {
    std::vector<Object> objects = objSelector.selectObjects(frames, masks);

    // tiny display.
    Mat leftResult(480, 640, CV_8UC3, Scalar(0, 0, 0));
    Mat rightResult(480, 640, CV_8UC3, Scalar(0, 0, 0));

    RNG rng;
    for (int i = 0; i < objects.size(); i++) {
        const Object &obj = objects[i];

        int icolor = (unsigned) rng;
        Scalar color(icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255);
        Mat objMat(480, 640, CV_8UC3, color);

        objMat.copyTo(leftResult, obj.masks[0]);
        objMat.copyTo(rightResult, obj.masks[1]);
    }

    Mat merged = mergeImages(leftResult, rightResult);

    for (int i = 0; i < objects.size(); i++) {
        const Object &obj = objects[i];

        for (int j = 0; j < obj.matches.size(); j++) {
            int icolor = (unsigned) rng;
            Scalar color(icolor & 255, (icolor >> 8) & 255, (icolor >> 16) & 255);

            circle(merged, obj.matches[j].first, 1, color, 1);
            circle(merged, obj.matches[j].second + Point2f(640, 0), 2, color, 1);
            line(merged, obj.matches[j].first, obj.matches[j].second + Point2f(640, 0), color, 1);
        }
    }

    imshow("objects", merged);

    OpticalFlowCalculator calculator;

    std::vector<CloudPoint> finalResult;
    std::vector<Point2f> totalPoints;

    for (int i = 0; i < objects.size(); i++) {
        imshow("OriginalFrame1", frames[0]);
        imshow("OriginalFrame2", frames[1]);

        std::pair<std::vector<Point2f>, std::vector<Point2f>> matches = calculator.calcDenseMatches(frames, objects[i]);

        Triangulator triangulator(camera[Camera::LEFT], camera[Camera::RIGHT],
                                  cameraPose[Camera::LEFT], cameraPose[Camera::RIGHT]);

        std::vector<CloudPoint> cvPointcloud;
        triangulator.triangulateCv(matches.first, matches.second, cvPointcloud);

        totalPoints.insert(totalPoints.end(), matches.first.begin(), matches.first.end());
        finalResult.insert(finalResult.end(), cvPointcloud.begin(), cvPointcloud.end());

//        Visualization matVis(cameraPose[Camera::LEFT], camera[Camera::LEFT]->cameraMatrix);
//        matVis.renderWithDepth(cvPointcloud);
//        imshow("Result", matVis.getResult());
//        waitKey();
    }

    Visualization matVis(cameraPose[Camera::LEFT], camera[Camera::LEFT]->cameraMatrix);
    matVis.renderWithDepth(finalResult);
    imshow("FinalResult", matVis.getResult());
    writeCloudPoints(finalResult);

//    namedWindow("Result", 1);
//
//    //imshow("magic", matVis.getResult());
//
//    CameraPose virtualPose;
//    Mat virtualCameraMatrix = (Mat_<double>(3, 3) << 540, 0, 320, 0, 540, 240, 0, 0, 1);
//    cameraPose[Camera::LEFT].copyTo(virtualPose);
//
//    int pos = 0;
//    char const *xTrackbar = "Váltás kamerák között";
//    createTrackbar(xTrackbar, "Result", &pos, 100);
//
//    VIS_TYPE type = VIS_TYPE::DEPTH;
//
//    while (true) {
//        // calculate the relative position.
//        double ratio = ((double) pos) / 100;
//        virtualPose.tvec = (1 - ratio) * cameraPose[Camera::LEFT].tvec + ratio * cameraPose[Camera::RIGHT].tvec;
//        virtualPose.rvec = slerp(cameraPose[Camera::LEFT].rvec, cameraPose[Camera::RIGHT].rvec, ratio);
//
//        Visualization matVis2(virtualPose, virtualCameraMatrix);
//        if (type == VIS_TYPE::DEPTH) {
//            matVis2.renderWithDepth(finalResult);
//        } else if (type == VIS_TYPE::PIXELS) {
//            matVis2.renderWithGrayscale(finalResult, totalPoints, frames[0]);
//        } else {
//            matVis2.renderWithContours(finalResult);
//        }
//        imshow("Result", matVis2.getResult());
//
//        char ch = (char) waitKey(33);
//        if (ch == 27) {
//            break;
//        } else if (ch == 'd') {
//            cameraPose[Camera::RIGHT].copyTo(virtualPose);
//            pos = 100;
//            setTrackbarPos(xTrackbar, "Result", pos);
//        } else if (ch == 'a') {
//            cameraPose[Camera::LEFT].copyTo(virtualPose);
//            pos = 0;
//            setTrackbarPos(xTrackbar, "Result", pos);
//        } else if (ch == '1') {
//            type = VIS_TYPE::DEPTH;
//        } else if (ch == '2') {
//            type = VIS_TYPE::PIXELS;
//        } else if (ch == '3') {
//            type = VIS_TYPE::CONTORUS;
//        } else if (ch == 'f') {
//            imwrite("/media/balint/Data/Linux/diploma/visualization.png",
//                    matVis2.getResult()(Rect(165, 145, 315, 315)));
//        }
//    }

//    while (true) {
    char ch = (char) waitKey();
//        if (ch == 27) {
//            break;
//        }
//    }

    return 0;
}


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-bitwise
    This function object returns the high part of the entry
    as an unsigned integer of the size of
    the input.

    @par Header <boost/simd/function/hi.hpp>

    @see  lo

    @par Example:

      @snippet hi.cpp hi

    @par Possible output:

      @snippet hi.txt hi

  **/
  as_integer_t<Value, unsigned> hi(Value const& x);
} }
#endif

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

#endif


// Copyright (c) 2018-2019, CNRS
// Authors: Sanghyun Kim <ggory15@snu.ac.kr>

#ifndef __hpp_serialization_spatial_hpp__
#define __hpp_serialization_spatial_hpp__

#include <pinocchio/spatial/se3.hpp>
#include <boost/serialization/split_free.hpp>
#include <boost/serialization/vector.hpp>

namespace boost{

  namespace serialization{

    template <class Archive, typename _Scalar, int _Options>
    void save(Archive & ar, const se3::SE3Tpl<_Scalar,_Options> & M, const unsigned int /*version*/)
    {
      ar & make_nvp("translation",make_array(M.translation().data(),3));
      ar & make_nvp("rotation",make_array(M.rotation().data(),9));
    }

    template <class Archive, typename _Scalar, int _Options>
    void load(Archive & ar, se3::SE3Tpl<_Scalar,_Options> & M, const unsigned int /*version*/)
    {
      ar >> make_nvp("translation",make_array(M.translation().data(),3));
      ar >> make_nvp("rotation",make_array(M.rotation().data(),9));
    }

    template <class Archive, typename _Scalar, int _Options>
    void serialize(Archive & ar, se3::SE3Tpl<_Scalar,_Options> & M, const unsigned int version)
    {
      split_free(ar,M,version);
    }

  }

}

#endif // ifndef __hpp_serialization_spatial_hpp__