// This file is part of PoseEstimation.
// Copyright (c) 2021, Eijiro Shibusawa <phd_kimberlite@yahoo.co.jp>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice, this
//    list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 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 P3P_UTIL_HPP_
#define P3P_UTIL_HPP_

#include <Eigen/Dense>

#include <random>
#include <vector>

namespace P3P
{
template <typename FloatType, typename RNG>
void getRandomPose(RNG &rng, FloatType *R, FloatType *t)
{
	std::uniform_real_distribution<FloatType> ufd(-1, 1);
	// compute random rotation
	Eigen::Matrix<FloatType, 3, 3> mR1;
	for (int k = 0; k < 3; k++)
	{
		for (int l = 0; l < 3; l++)
		{
			mR1(k, l) = ufd(rng);
		}
	}
	Eigen::JacobiSVD<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > svd(mR1, Eigen::ComputeFullU | Eigen::ComputeFullV);
	mR1 = svd.matrixU() * svd.matrixV().transpose();
	if (mR1.determinant() < 0)
	{
		mR1.col(0) = -mR1.col(0);
	}
	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > mR(R);
	mR = mR1;

	// compute random translation
	Eigen::Map<Eigen::Matrix<FloatType, 3, 1> > mt(t);
	mt[0] = 0.5 * ufd(rng);
	mt[1] = 0.5 * ufd(rng);
	mt[2] = 6 + 0.5 * ufd(rng);
}

template <typename FloatType, typename RNG>
void getRandomPoints(RNG &rng, int n, std::vector<FloatType> &p)
{
	std::uniform_real_distribution<FloatType> ufd(-1, 1);
	// compute random point
	p.reserve(3 * n);
	p.resize(0);
	for (int k = 0; k < n; k++)
	{
		p.push_back(2 * ufd(rng));
		p.push_back(2 * ufd(rng));
		p.push_back(2 * ufd(rng));
	}
}

template <typename FloatType>
void getRandomProjections(int n, FloatType *R, FloatType *t, std::vector<FloatType> &p2d, std::vector<FloatType> &p3d)
{
	// x = R*X + t
	std::random_device rd;
	std::mt19937 rng(rd());
	getRandomPose(rng, R, t);
	getRandomPoints(rng, n, p3d);

	Eigen::Map<Eigen::Matrix<FloatType, 3, 3, Eigen::RowMajor> > mR(R);
	Eigen::Map<Eigen::Matrix<FloatType, 3, 1> > mt(t);
	Eigen::Map<Eigen::Matrix<FloatType, 3, Eigen::Dynamic> > mX(&(p3d[0]), 3, n);
	// project 3D point
	Eigen::Matrix<FloatType, 3, Eigen::Dynamic> mTmp(3, n);
	mTmp = (mR*mX).colwise() + mt;
	// get 2D Point
	p2d.resize(2 * n);
	Eigen::Map<Eigen::Matrix<FloatType, 2, Eigen::Dynamic> > mx(&(p2d[0]), 2, n);
	// perspective projection
	mx.row(0) = mTmp.row(0).array() / mTmp.row(2).array();
	mx.row(1) = mTmp.row(1).array() / mTmp.row(2).array();
}
}

#endif // P3P_UTIL_HPP_


#pragma once

// ROS
#include <std_msgs/Float64MultiArray.h>
#include <std_msgs/MultiArrayDimension.h>

// EIGEN
#include <Eigen/Dense>
#include <Eigen/Geometry>

// CPLEX
#include <ilcplex/ilocplex.h>

// Others
#include <mission.hpp>
#include <param.hpp>

ILOSTLBEGIN

namespace SwarmPlanning {
    class SCPPlanner {
    public:
        std_msgs::Float64MultiArray msgs_traj_info;
        std_msgs::Float64MultiArray msgs_traj_input;

        SCPPlanner(Mission _mission,
                   Param _param)
                : mission(_mission),
                  param(_param) {
            h = param.time_step;
            T = 34;
            K = round(T / h) + 1;         // number of segments
            N = mission.qn;
            outdim = 3;                 // number of outputs (x,y,z)

            p_max = 5;
            v_max = 10;
            a_max = 10;
            j_max = 10;
            epsilon = 0.01;

            u_prev = Eigen::MatrixXd::Zero(outdim * N * K, 1);
        }

        bool update(bool log) {
            IloEnv env;
            Timer timer;

            u.resize(outdim * N * K);
            try {
                timer.reset();
                buildConstMtx();
                timer.stop();
                ROS_INFO_STREAM("Constraint Matrix runtime: " << timer.elapsedSeconds());

                timer.reset();
                solveQP(env, log);
                timer.stop();
                ROS_INFO_STREAM("QP runtime: " << timer.elapsedSeconds());
            }
            catch (IloException &e) {
                ROS_ERROR_STREAM("Concert exception caught: " << e);
                return false;
            }
            catch (...) {
                ROS_ERROR("Unknown exception caught");
                return false;
            }
            env.end();

            createMsg();
            return true;
        }

    private:
        Mission mission;
        Param param;

        double h, T, p_max, v_max, a_max, j_max, epsilon;
        int K, N, outdim;
        IloNum count_x, count_eq, count_lq;

        Eigen::MatrixXd Q, A_eq, b_eq, A_ineq, b_ineq,
                P, V, A, J, p_start, p_goal,
                A_ineq_dynamics, b_ineq_dynamics, u_prev;
        std::vector<double> u;

        void buildConstMtx() {
            build_Q();
            build_mapping_mtx();
            build_eq_const();
            build_ineq_const();
        }

        void solveQP(const IloEnv &env, bool log) {
            Timer timer;
            IloNum cost_total, cost_prev;

            IloCplex cplex(env);
//        cplex.setParam(IloCplex::Param::TimeLimit, 0.04);

            cost_total = SP_INFINITY;
            cost_prev = 0;
            int iter = 0;

            timer.reset();
            while (abs(cost_total - cost_prev) > epsilon * cost_total) {
                IloModel model(env);
                IloNumVarArray var(env);
                IloRangeArray con(env);

                populatebyrow(model, var, con);

                cplex.extract(model);

                if (log) {
                    cplex.exportModel("/home/jungwon/QPmodel.lp");
                } else {
                    cplex.setOut(env.getNullStream());
                }

                // Optimize the problem and obtain solution.
                if (!cplex.solve()) {
                    env.error() << "Failed to optimize QP, Check ~/.ros/QPresult.txt" << endl;
                    throw (-1);
                }

                IloNumArray vals(env);
                cost_prev = cost_total;
                cost_total = cplex.getObjValue();
                cplex.getValues(vals, var);

                // update input
                for (int dim = 0; dim < outdim; dim++) {
                    for (int qi = 0; qi < N; qi++) {
                        for (int k = 0; k < K; k++) {
                            u[dim * N * K + qi * K + k] = vals[dim * N * K + qi * K + k];
                            u_prev(dim * N * K + qi * K + k, 0) = vals[dim * N * K + qi * K + k];
                        }
                    }
                }

                timer.stop();
                ROS_INFO_STREAM("QP iteration " << iter << " : " << "total_cost : " << cost_total);
                ROS_INFO_STREAM("QP iteration " << iter << " : " << "iteration time : " << timer.elapsedSeconds());
                ROS_INFO_STREAM("QP iteration " << iter << " : " << "x size: " << count_x);
                ROS_INFO_STREAM("QP iteration " << iter << " : " << "eq const size: " << count_eq);
                ROS_INFO_STREAM("QP iteration " << iter << " : " << "ineq const size: " << count_lq);
                iter++;
                timer.reset();

                // update inequality constraints
                update_ineq_const();
            }

            ROS_INFO_STREAM("QP total cost = " << cost_total);
        }

        void createMsg() {
            std::vector<double> traj_info;
            traj_info.emplace_back(N);
            traj_info.emplace_back(K);
            traj_info.emplace_back(h);
            msgs_traj_info.data = traj_info;
            msgs_traj_input.data = u;
        }

        // Cost matrix Q
        void build_Q() {
            Q = Eigen::MatrixXd::Identity(outdim * N * K, outdim * N * K);
        }

        void build_mapping_mtx() {
            P = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // position matrix p = Pu + p_start
            V = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // velocity matrix v = Vu, assume v_start = 0
            A = Eigen::MatrixXd::Identity(outdim * N * K, outdim * N * K); // accelation matrix a = Au
            J = Eigen::MatrixXd::Zero(outdim * N * K, outdim * N * K); // accelation matrix a = Au

            p_start = Eigen::MatrixXd::Zero(outdim * N * K, 1);
            p_goal = Eigen::MatrixXd::Zero(outdim * N, 1);

            for (int dim = 0; dim < outdim; dim++) {
                for (int qi = 0; qi < N; qi++) {
                    int offset = dim * N * K + qi * K;
                    for (int k = 0; k < K; k++) {
                        for (int j = 0; j < k; j++) {
                            P(offset + k, offset + j) = 0.5 * h * h * (2 * (k - j) - 1);
                            V(offset + k, offset + j) = h;
                        }
                        if (k != 0) {
                            J(offset + k, offset + k) = 1 / h;
                            J(offset + k, offset + k - 1) = -1 / h;
                        }

                        p_start(offset + k, 0) = mission.startState[qi][dim];
                    }
                    p_goal(dim * N + qi, 0) = mission.goalState[qi][dim];
                }
            }
        }

        void build_eq_const() {
            A_eq = Eigen::MatrixXd::Zero(4 * outdim * N, outdim * N * K);
            b_eq = Eigen::MatrixXd::Zero(4 * outdim * N, 1);

            Eigen::MatrixXd A_init = Eigen::MatrixXd::Zero(outdim * N, outdim * N * K);
            Eigen::MatrixXd A_final = Eigen::MatrixXd::Zero(outdim * N, outdim * N * K);

            for (int dim = 0; dim < outdim; dim++) {
                for (int qi = 0; qi < N; qi++) {
                    int offset = dim * N * K + qi * K;
                    A_init(dim * N + qi, offset) = 1;
                    A_final(dim * N + qi, offset + K - 1) = 1;
                }
            }

            A_eq.block(0 * outdim * N, 0, outdim * N, outdim * N * K) = A_init;
            A_eq.block(1 * outdim * N, 0, outdim * N, outdim * N * K) = A_final * P;
            A_eq.block(2 * outdim * N, 0, outdim * N, outdim * N * K) = A_final * V;
            A_eq.block(3 * outdim * N, 0, outdim * N, outdim * N * K) = A_final;

            b_eq.block(outdim * N, 0, outdim * N, 1) = p_goal - A_final * p_start;
        }

        void build_ineq_const() {
            A_ineq_dynamics = Eigen::MatrixXd::Zero(8 * outdim * N * K, outdim * N * K);
            A_ineq_dynamics << P,
                    -P,
                    V,
                    -V,
                    A,
                    -A,
                    J,
                    -J;

            b_ineq_dynamics = Eigen::MatrixXd::Zero(8 * outdim * N * K, 1);
            Eigen::MatrixXd b_ineq_base = Eigen::MatrixXd::Ones(outdim * N * K, 1);
            b_ineq_dynamics << b_ineq_base * p_max - p_start,
                    b_ineq_base * p_max + p_start,
                    b_ineq_base * v_max,
                    b_ineq_base * v_max,
                    b_ineq_base * a_max,
                    b_ineq_base * a_max,
                    b_ineq_base * j_max,
                    b_ineq_base * j_max;

            A_ineq = Eigen::MatrixXd::Zero(8 * outdim * N * K, outdim * N * K);
            A_ineq = A_ineq_dynamics;
            b_ineq = Eigen::MatrixXd::Zero(8 * outdim * N * K, 1);
            b_ineq = b_ineq_dynamics;
        }

        void update_ineq_const() {
            Eigen::MatrixXd A_ineq_col = Eigen::MatrixXd::Zero(N * (N - 1) / 2 * K, outdim * N * K);
            Eigen::MatrixXd b_ineq_col = Eigen::MatrixXd::Zero(N * (N - 1) / 2 * K, 1);

            Eigen::MatrixXd picker_i, picker_j, p_i, p_j, p_prev, eta, temp;
            p_prev = P * u_prev + p_start;

            double offset = 0;
            for (int qi = 0; qi < N; qi++) {
                for (int qj = qi + 1; qj < N; qj++) {
                    double R = mission.quad_size[qi] + mission.quad_size[qj];
                    for (int k = 0; k < K; k++) {

                        position_picker(qi, k, picker_i);
                        position_picker(qj, k, picker_j);

                        p_i = picker_i * p_prev;
                        p_j = picker_j * p_prev;

                        double distance = (p_i - p_j).norm();
                        eta = (p_i - p_j) / distance;

                        A_ineq_col.block(offset + k, 0, 1, outdim * N * K) =
                                -eta.transpose() * (picker_i - picker_j) * P;

                        temp = eta.transpose() * ((p_i - p_j) - (picker_i - picker_j) * p_start);
                        b_ineq_col(offset + k, 0) = -(R - distance + temp(0, 0));
                    }
                    offset += K;
                }
            }

            A_ineq = Eigen::MatrixXd::Zero(8 * outdim * N * K + N * (N - 1) / 2 * K, outdim * N * K);
            A_ineq << A_ineq_dynamics,
                    A_ineq_col;
            b_ineq = Eigen::MatrixXd::Zero(8 * outdim * N * K + N * (N - 1) / 2 * K, 1);
            b_ineq << b_ineq_dynamics,
                    b_ineq_col;
        }

        void populatebyrow(IloModel model, IloNumVarArray x, IloRangeArray c) {
            IloEnv env = model.getEnv();

            for (int dim = 0; dim < outdim; dim++) {
                for (int qi = 0; qi < N; qi++) {
                    for (int k = 0; k < K; k++) {
                        x.add(IloNumVar(env, -IloInfinity, IloInfinity));

                        std::string name;
                        if (dim == 0) {
                            name = "x_" + std::to_string(qi) + "_" + std::to_string(k);
                        } else if (dim == 1) {
                            name = "y_" + std::to_string(qi) + "_" + std::to_string(k);
                        } else {
                            name = "z_" + std::to_string(qi) + "_" + std::to_string(k);
                        }

                        int row = dim * N * K + qi * K + k;
                        x[row].setName(name.c_str());
                    }
                }
            }
            count_x = x.getSize();

            // Cost function
            IloNumExpr cost(env);
            for (int i = 0; i < Q.rows(); i++) {
                for (int j = 0; j < Q.cols(); j++) {
                    if (Q(i, j) != 0) {
                        cost += Q(i, j) * x[i] * x[j];
                    }
                }
            }
            model.add(IloMinimize(env, cost));

            // Equality Constraints
            for (int i = 0; i < A_eq.rows(); i++) {
                IloNumExpr expr(env);
                for (int j = 0; j < A_eq.cols(); j++) {
                    if (A_eq(i, j) != 0) {
                        expr += A_eq(i, j) * x[j];
                    }
                }
                c.add(expr == b_eq(i));
                expr.end();
            }
            count_eq = c.getSize();

            // Inequality Constraints
            for (int i = 0; i < A_ineq.rows(); i++) {
                IloNumExpr expr(env);
                for (int j = 0; j < A_ineq.cols(); j++) {
                    if (A_ineq(i, j) != 0) {
                        expr += A_ineq(i, j) * x[j];
                    }
                }
                c.add(expr <= b_ineq(i));
                expr.end();
            }
            count_lq = c.getSize() - count_eq;

            model.add(c);
        }

        void position_picker(int qi, int k, Eigen::MatrixXd &P_pick) {
            P_pick = Eigen::MatrixXd::Zero(outdim, outdim * N * K);
            for (int dim = 0; dim < outdim; dim++) {
                P_pick(dim, dim * N * K + qi * K + k) = 1;
            }
        }
    };
}


// Author(s): Thomas Neele
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/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 quantifiers_propagate_test.cpp
/// \brief Test program for absinthe algorithm.

#define BOOST_TEST_MODULE quantifiers_propagate_test
#include <boost/test/included/unit_test_framework.hpp>
#include "mcrl2/pbes/quantifier_propagate.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

BOOST_AUTO_TEST_CASE(test_single_forall)
{
  std::string PBES_TEXT =
    "pbes                                                                                \n"
    " nu X(s: List(Nat)) =                                                                         \n"
    "   forall m1: Nat. Y(s, m1);                                                                  \n"
    " mu Y(s: List(Nat), m1: Nat) =                                                                \n"
    "   forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1);  \n"
    "init X([]);                                                                                \n"
  ;

  std::string RESULT_TEXT =
    "pbes                                                                                \n"
    " nu X(s: List(Nat)) =                                                                                \n"
    "   Y1(s);                                                                                \n"
    " mu Y1(s: List(Nat)) =                                                                                \n"
    "   forall m1: Nat. forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1); \n"
    " mu Y(s: List(Nat), m1: Nat) =                                                                               \n"
    "   forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1);                 \n"
    "init X([]);                                                                                \n"
  ;

  BOOST_CHECK_EQUAL(quantifier_propagate(txt2pbes(PBES_TEXT)), txt2pbes(RESULT_TEXT));
}

BOOST_AUTO_TEST_CASE(test_two_quantifiers)
{
  std::string PBES_TEXT =
    "pbes                                                                                \n"
    " nu X(s: List(Nat)) =                                                                         \n"
    "   exists m2: Nat. forall m1: Nat. Y(s, m1 + m2);                                             \n"
    " mu Y(s: List(Nat), m1: Nat) =                                                                \n"
    "   forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1);  \n"
    "init X([]);                                                                                \n"
  ;

  std::string RESULT_TEXT =
    "pbes                                                                                \n"
    " nu X(s: List(Nat)) =                                                                                \n"
    "   Y1(s);                                                                                \n"
    " mu Y1(s: List(Nat)) =                                                                                \n"
    "   exists m2: Nat. forall m1: Nat. forall n: Nat. val(!(n mod 6 == m1 + m2)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1 + m2); \n"
    " mu Y(s: List(Nat), m1: Nat) =                                                                                \n"
    "   forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1);                                           \n"
    "init X([]);                                                                                \n"
  ;

  BOOST_CHECK_EQUAL(quantifier_propagate(txt2pbes(PBES_TEXT)), txt2pbes(RESULT_TEXT));
}

BOOST_AUTO_TEST_CASE(test_parameter_dependency)
{
  std::string PBES_TEXT =
    "pbes                                                                                \n"
    " nu X(s: List(Nat)) =                                                                         \n"
    "   forall m1: Nat. Y(s, head(s) + m1);                                                        \n"
    " mu Y(s: List(Nat), m1: Nat) =                                                                \n"
    "   forall n: Nat. val(!(n mod 6 == m1)) || val(!(n <= 50 && n div 3 == 5)) || Y(n |> s, m1);  \n"
    "init X([]);                                                                                \n"
  ;

  // No changes expected, since m1 depends on the parameter s in Y(s, head(s) + m1)
  std::string RESULT_TEXT = PBES_TEXT;

  BOOST_CHECK_EQUAL(quantifier_propagate(txt2pbes(PBES_TEXT)), txt2pbes(RESULT_TEXT));
}

BOOST_AUTO_TEST_CASE(test_no_quantifier)
{
  std::string PBES_TEXT =
    "pbes             \n"
    " nu X(s: Nat) =  \n"
    "   Y(2);         \n"
    " mu Y(s: Nat) =  \n"
    "   val(s == 2);  \n"
    "init X(0);       \n"
  ;

  // No changes expected, since there is no quantifier to propagate
  std::string RESULT_TEXT = PBES_TEXT;

  BOOST_CHECK_EQUAL(quantifier_propagate(txt2pbes(PBES_TEXT)), txt2pbes(RESULT_TEXT));
}


#ifndef PARMCB_DETAIL_LEX_DIJKSTRA_HPP_
#define PARMCB_DETAIL_LEX_DIJKSTRA_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// 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)

#include <iostream>

#include <boost/scoped_array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>

#include <parmcb/detail/dijkstra.hpp>
#include <parmcb/detail/util.hpp>

namespace parmcb {

    namespace detail {

        template<class Graph, class DistanceMap>
        struct LexDistance {
            typedef typename boost::property_traits<DistanceMap>::value_type DistanceType;

            LexDistance() :
                    distance(DistanceType()), edge_count(0), min_vertex_index(0) {
            }

            LexDistance(DistanceType distance, std::size_t edge_count, std::size_t min_vertex_index) :
                    distance(distance), edge_count(edge_count), min_vertex_index(min_vertex_index) {
            }

            DistanceType distance;
            std::size_t edge_count;
            std::size_t min_vertex_index;
        };

        template<class Graph, class DistanceMap>
        struct LexDistanceCompare {
            typedef typename boost::property_traits<DistanceMap>::value_type DistanceType;

            bool operator()(const LexDistance<Graph, DistanceMap> &a, const LexDistance<Graph, DistanceMap> &b) {
                if (a.distance < b.distance) {
                    return true;
                } else if (a.distance > b.distance) {
                    return false;
                }
                if (a.edge_count < b.edge_count) {
                    return true;
                } else if (a.edge_count > b.edge_count) {
                    return false;
                }
                if (a.min_vertex_index < b.min_vertex_index) {
                    return true;
                }
                return false;
            }
        };

        template<class Graph, class IndexMap, class WeightMap, class DistanceMap>
        struct LexDistanceCombine {
            typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
            typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
            typedef typename boost::property_traits<DistanceMap>::value_type DistanceType;
            typedef typename boost::property_traits<WeightMap>::value_type WeightType;

            LexDistanceCombine(const Graph &g, const IndexMap &index_map, const WeightMap &weight_map) :
                    inf((std::numeric_limits<DistanceType>::max)()), g(g), index_map(
                            index_map), weight_map(weight_map) {
            }

            LexDistance<Graph, DistanceMap> operator()(const LexDistance<Graph, DistanceMap> &a, const Edge &e) {
                const auto index_target = index_map[boost::target(e, g)];
                const auto index_source = index_map[boost::source(e, g)];

                const WeightType e_weight = boost::get(weight_map, e);
                const WeightType sum = combine(a.distance, e_weight);

                return LexDistance<Graph, DistanceMap>(sum, a.edge_count + 1, std::min( { a.min_vertex_index,
                        index_target, index_source }));
            }

            const DistanceType inf;
            const parmcb::detail::closed_plus<DistanceType> combine;
            const Graph &g;
            const VertexIndexMapType &index_map;
            const WeightMap &weight_map;
        };

    } // detail

    template<class Graph, class WeightMap, class DistanceMap, class PredecessorMap>
    void lex_dijkstra(const Graph &g, const WeightMap &weight_map,
            const typename boost::graph_traits<Graph>::vertex_descriptor &s, DistanceMap &dist_map,
            PredecessorMap &pred_map) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::graph_traits<Graph>::edge_descriptor Edge;
        typedef typename boost::property_traits<DistanceMap>::value_type DistanceType;

        typedef typename parmcb::detail::LexDistance<Graph, DistanceMap> LexDistanceType;
        typedef typename parmcb::detail::LexDistanceCompare<Graph, DistanceMap> LexDistanceCompareType;

        const VertexIndexMapType &index_map = boost::get(boost::vertex_index, g);
        std::vector<std::size_t> index_in_heap(boost::num_vertices(g));
        boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::size_t>, Vertex, std::size_t&> index_in_heap_map(
                parmcb::detail::VertexIndexFunctor<Graph, std::size_t>(index_in_heap, index_map));

        std::vector<LexDistanceType> lex_dist(boost::num_vertices(g));
        boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, LexDistanceType>, Vertex,
                LexDistanceType&> lex_dist_map(
                parmcb::detail::VertexIndexFunctor<Graph, LexDistanceType>(lex_dist, index_map));

        typedef boost::d_ary_heap_indirect<Vertex, 4,
                boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, std::size_t>, Vertex,
                        std::size_t&>,
                boost::function_property_map<parmcb::detail::VertexIndexFunctor<Graph, LexDistanceType>, Vertex,
                        LexDistanceType&>, LexDistanceCompareType> VertexQueue;

        LexDistanceCompareType compare;
        parmcb::detail::LexDistanceCombine<Graph, VertexIndexMapType, WeightMap, DistanceMap> combine(g, index_map, weight_map);

        VertexQueue queue(lex_dist_map, index_in_heap_map, compare);

        boost::put(lex_dist_map, s, LexDistanceType(DistanceType(), 0, index_map[s]));
        boost::put(pred_map, s, std::make_tuple(false, Edge()));
        queue.push(s);

        while (!queue.empty()) {
            Vertex u = queue.top();
            queue.pop();
            LexDistanceType d_u = boost::get(lex_dist_map, u);

            auto eiRange = boost::out_edges(u, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto e = *ei;

                auto w = boost::target(e, g);
                if (w == u) {
                    w = boost::source(e, g);
                }
                if (w == u) {
                    // self-loop
                    continue;
                }
                if (w == s) {
                    continue;
                }

                const LexDistanceType c = combine(d_u, e);
                bool visited_w = std::get<0>(boost::get(pred_map, w));
                if (!visited_w) {
                    // first time found
                    boost::put(lex_dist_map, w, c);
                    boost::put(dist_map, w, c.distance);
                    boost::put(pred_map, w, std::make_tuple(true, e));
                    queue.push(w);
                } else if (compare(c, boost::get(lex_dist_map, w))) {
                    // already reached
                    boost::put(lex_dist_map, w, c);
                    boost::put(dist_map, w, c.distance);
                    boost::put(pred_map, w, std::make_tuple(true, e));
                    queue.update(w);
                }
            }
        }
    }

} // mcb

#endif


#ifndef MST_CONTROLLER_HPP
#define MST_CONTROLLER_HPP 1

#include "linterp.h"
#include "toml.h"
#include <Eigen/Core>
#include <boost/numeric/odeint.hpp>
#include <fstream>
#include <functional>
#include <iostream>
#include <memory>
#include <string>

namespace MST { // The MST namespace contains the classes used for control.

using std::string;
using namespace boost::numeric::odeint;
namespace pl = std::placeholders;

// Matrix and Vector are shortcuts for Eigen types
using Matrix = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
using Vector = Eigen::Matrix<double, Eigen::Dynamic, 1>;
using Eigen::Vector2d;

namespace Utils {
template <typename T> T load_dat(const std::string &path) {
	std::ifstream indata(path);
	std::string line;
	std::vector<double> values;
	size_t rows = 0;
	while (std::getline(indata, line)) {
		std::stringstream lineStream(line);
		std::string cell;
		while (std::getline(lineStream, cell, ' ')) {
			values.push_back(std::stod(cell));
		}
		++rows;
	}
	return Eigen::Map<const T>(values.data(), rows, values.size() / rows);
}
} // namespace Utils

class Config {
public:
	Config(const char *filename) : Config(std::string(filename)) {}
	Config(const std::string &filename) {
		std::ifstream config_file(filename);
		toml::ParseResult parsed_config = toml::parse(config_file);

		// make sure config parsed correctly
		if (!parsed_config.valid()) {
			throw std::runtime_error(parsed_config.errorReason);
		}

		// extract control parameters
		m_config = parsed_config.value;
	}

	Config operator[](const std::string &path) {
		return Config(m_config[path]);
	}

	double getDouble(const std::string &name) const {
		return m_config.get<double>(name);
	}

	std::string getString(const std::string &name) const {
		return m_config.get<std::string>(name);
	}

	// Load a column vector from a TOML array.
	// All rows of the TOML array must have exactly one element.
	Vector getVector(const std::string &name) const {
		const toml::Array &rows = m_config.get<toml::Array>(name);
		const size_t n = rows.size();
		Vector vec(n);
		for (int i = 0; i < n; ++i) {
			const toml::Value row = rows[i];
			vec(i) = row.get<double>(0);
		}
		return vec;
	}

	// Load a matrix from a TOML array.
	// All rows of the TOML array must have the same length.
	Matrix getMatrix(const std::string &name) const {
		const toml::Array &rows = m_config.get<toml::Array>(name);
		const size_t n = rows.size();
		const size_t p = rows.at(0).size();
		Matrix mat(n, p);
		for (int i = 0; i < n; ++i) {
			const toml::Value row = rows.at(i);
			for (int j = 0; j < p; ++j) {
				mat(i, j) = row.get<double>(j);
			}
		}
		return mat;
	}

private:
	toml::Value m_config;

	Config(const toml::Value &config) : m_config(config) {}
};

class System {
public:
	template <typename T> System &operator>>(const T &next) {
		if (m_next) {
			*m_next >> next;
		} else {
			m_next = std::make_shared<T>(next);
		}
		return *this;
	}

	Vector operator()(const double t, const Vector &v) {
		const Vector output = step(t, v, Vector());
		if (m_next) {
			return (*m_next)(t, output, Vector());
		}
		return output;
	}

	Vector operator()(const double t, const Vector &ref, const Vector &actual) {
		const Vector output = step(t, ref, actual);
		if (m_next) {
			return (*m_next)(t, output, actual);
		}
		return output;
	}

private:
	std::shared_ptr<System> m_next;

	virtual Vector step(const double t, const Vector &ref,
	                    const Vector &actual) = 0;
};

class Controller : public System {
private:
	// Matrices of coefficients
	Matrix A;
	Matrix B;
	Matrix C;
	Matrix D;
	Matrix F;
	Matrix K;
	Matrix Ki;
	Matrix L;

	// Precalculated matrices
	Matrix AmLC;
	Matrix BmLD;

	// Desired values
	Vector ud;
	Vector xd;

	// Natural equilibrium
	Vector r0;
	Vector u0;

	// Saturation
	Vector u_min;
	Vector u_max;

	// State of the controller
	Vector xh;
	Vector xi;

	// Time step
	double dt;

public:
	Controller(const Config &config) {
		A = config.getMatrix("A");
		B = config.getMatrix("B");
		C = config.getMatrix("C");
		D = config.getMatrix("D");
		F = config.getMatrix("F");
		K = config.getMatrix("K");
		Ki = config.getMatrix("Ki");
		L = config.getMatrix("L");

		AmLC = A - L * C;
		BmLD = B - L * D;

		ud = config.getVector("ud");
		xd = config.getVector("xd");

		r0 = config.getVector("r0");
		u0 = config.getVector("u0");

		u_min = config.getVector("u_min");
		u_max = config.getVector("u_max");

		dt = config.getDouble("dt");

		xh.setZero(A.rows());
		xi.setZero(C.rows());
	}

	virtual Vector step(const double t, const Vector &r, const Vector &y) {
		if (r.rows() == 0 || r.cols() == 0) {
			return Vector();
		}

		Vector u = F * (r - r0) - K * xh - Ki * xi;

		// Saturation
		u = (u + u0).cwiseMax(u_min).cwiseMin(u_max) - u0;

		xh = AmLC * xh + BmLD * u + L * (y - r0);
		xi += dt * (y - r);

		return u + u0;
	}
};

class OutputConverter : public System {
public:
	OutputConverter(const Config &config) {
		m_coeff(0) = config.getDouble("coeff_phi");
		m_coeff(1) = config.getDouble("coeff_theta");
		m_R(0) = config.getDouble("R_phi");
		m_R(1) = config.getDouble("R_theta");
		m_L(0) = config.getDouble("L_phi");
		m_L(1) = config.getDouble("L_theta");
		m_dt = config.getDouble("dt");
		m_shot = Utils::load_dat<Matrix>(config.getString("shot_data"));
		m_count = 1;
	}

	virtual Vector step(const double, const Vector &V, const Vector &) {
		if (V.rows() == 0 || V.cols() == 0) {
			m_V = m_shot.row(m_count);
		} else {
			m_V = V;
		}
		m_stepper.do_step(std::bind(&OutputConverter::primaryCurrentDot, this,
		                            pl::_1, pl::_2, pl::_3),
		                  m_I, 0.0, m_dt);
		++m_count;

		return m_I;
	}

private:
	// state
	Vector2d m_I;
	Vector2d m_V;

	// parameters
	Vector2d m_coeff;
	Vector2d m_R;
	Vector2d m_L;
	double m_dt;
	Matrix m_shot;
	size_t m_count;

	runge_kutta_dopri5<Vector2d, double, Vector2d, double, vector_space_algebra>
	    m_stepper;

	void primaryCurrentDot(const Vector2d &I, Vector2d &dIdt,
	                       const double /*t*/) {
		dIdt(0) = (m_coeff(0) * m_V(0) - m_R(0) * I(0)) / m_L(0);
		dIdt(1) = (m_coeff(1) * m_V(1) - m_R(1) * I(1)) / m_L(1);
	}
};

class NonLinearityFirstComponent : public System {
public:
	NonLinearityFirstComponent(const Config &config) {
		Vector x(Utils::load_dat<Vector>(config.getString("x")));
		Vector y(Utils::load_dat<Vector>(config.getString("y")));

		const auto grid_iter_list = {x.data()};
		const auto grid_sizes = {x.size()};
		const size_t num_elements = x.size();

		m_f.reset(new InterpMultilinear<1, double>(grid_iter_list.begin(),
		                                           grid_sizes.begin(), y.data(),
		                                           y.data() + num_elements));
	}

	virtual Vector step(const double, const Vector &r, const Vector &) {
		if (r.rows() == 0 || r.cols() == 0) {
			return Vector();
		}
		Vector r1(r);
		r1(0) = f(r(0));
		return r1;
	}

	double f(const double x) const {
		const std::array<double, 1> args = {{x}};
		return m_f->interp(args.begin());
	}

private:
	std::shared_ptr<InterpMultilinear<1, double>> m_f;
};

class InputConverter : public System {
public:
	InputConverter(const Config &config) {
		Vector F(Utils::load_dat<Vector>(config.getString("F")));
		Vector Ip(Utils::load_dat<Vector>(config.getString("Ip")));
		Matrix lambda0(Utils::load_dat<Matrix>(config.getString("lambda0")));
		Matrix flux(Utils::load_dat<Matrix>(config.getString("flux")));

		const auto grid_iter_list = {F.data(), Ip.data()};
		const auto grid_sizes = {F.size(), Ip.size()};
		const size_t num_elements = F.size() * Ip.size();

		lambda0.transposeInPlace();
		flux.transposeInPlace();

		m_lambda0.reset(new InterpMultilinear<2, double>(
		    grid_iter_list.begin(), grid_sizes.begin(), lambda0.data(),
		    lambda0.data() + num_elements));
		m_flux.reset(new InterpMultilinear<2, double>(
		    grid_iter_list.begin(), grid_sizes.begin(), flux.data(),
		    flux.data() + num_elements));
	}

	virtual Vector step(const double, const Vector &r, const Vector &) {
		if (r.rows() == 0 || r.cols() == 0) {
			return Vector();
		}
		Vector r1(2);
		r1(0) = lambda0(r(0), r(1));
		r1(1) = flux(r(0), r(1));
		return r1;
	}

	double lambda0(const double F, const double Ip) const {
		const std::array<double, 2> args = {{F, Ip}};
		return m_lambda0->interp(args.begin());
	}

	double flux(const double F, const double Ip) const {
		const std::array<double, 2> args = {{F, Ip}};
		return m_flux->interp(args.begin());
	}

private:
	std::unique_ptr<InterpMultilinear<2, double>> m_lambda0;
	std::unique_ptr<InterpMultilinear<2, double>> m_flux;
};

class ReferenceOutput {
public:
	ReferenceOutput(const Config &config) {
		const double F1 = config.getDouble("F1");
		const double F2 = config.getDouble("F2");
		const double Ip1 = config.getDouble("Ip1");
		const double Ip2 = config.getDouble("Ip2");

		m_t1 = config.getDouble("t1");
		m_t2 = config.getDouble("t2");

		m_r1 = Vector2d(F1, Ip1);
		m_r2 = Vector2d(F2, Ip2);
	}

	Vector operator()(const double t) const {
		if (t < m_t1) {
			return Vector(); // FIXME openloop
		} else if (t < m_t2) {
			return m_r1;
		} else {
			return m_r2;
		}
	}

private:
	double m_t1;
	double m_t2;
	Vector2d m_r1;
	Vector2d m_r2;
};

} // namespace MST

#endif


/**
 * @file async_learning_test.hpp
 * @author Shangtong Zhang
 *
 * Test for async deep RL methods.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/loss_functions/mean_squared_error.hpp>
#include <mlpack/methods/ann/loss_functions/sigmoid_cross_entropy_error.hpp>
#include <mlpack/methods/reinforcement_learning/async_learning.hpp>
#include <mlpack/methods/reinforcement_learning/environment/cart_pole.hpp>
#include <mlpack/core/optimizers/sgd/update_policies/vanilla_update.hpp>
#include <mlpack/methods/reinforcement_learning/policy/greedy_policy.hpp>
#include <mlpack/methods/reinforcement_learning/policy/aggregated_policy.hpp>
#include <mlpack/methods/reinforcement_learning/training_config.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;
using namespace mlpack::optimization;
using namespace mlpack::rl;

BOOST_AUTO_TEST_SUITE(AsyncLearningTest);

// Test async one step q-learning in Cart Pole.
BOOST_AUTO_TEST_CASE(OneStepQLearningTest)
{
  /**
   * This is for the Travis CI server, in your own machine you should use more
   * threads.
   */
  #ifdef HAS_OPENMP
    omp_set_num_threads(1);
  #endif

  // Set up the network.
  FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
      GaussianInitialization(0, 0.001));
  model.Add<Linear<>>(4, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 2);

  // Set up the policy.
  using Policy = GreedyPolicy<CartPole>;
  AggregatedPolicy<Policy> policy({Policy(0.7, 5000, 0.1),
                                  Policy(0.7, 5000, 0.01),
                                  Policy(0.7, 5000, 0.5)},
                                  arma::colvec("0.4 0.3 0.3"));

  TrainingConfig config;
  config.StepSize() = 0.0001;
  config.Discount() = 0.99;
  config.NumWorkers() = 16;
  config.UpdateInterval() = 6;
  config.StepLimit() = 200;
  config.TargetNetworkSyncInterval() = 200;

  OneStepQLearning<CartPole, decltype(model), VanillaUpdate, decltype(policy)>
      agent(std::move(config), std::move(model), std::move(policy));

  arma::vec rewards(20, arma::fill::zeros);
  size_t pos = 0;
  size_t testEpisodes = 0;
  auto measure = [&rewards, &pos, &testEpisodes](double reward)
  {
    size_t maxEpisode = 10000;
    if (testEpisodes > maxEpisode)
      BOOST_REQUIRE(false);
    testEpisodes++;
    rewards[pos++] = reward;
    pos %= rewards.n_elem;
    // Maybe underestimated.
    double avgReward = arma::mean(rewards);
    Log::Debug << "Average return: " << avgReward
        << " Episode return: " << reward << std::endl;
    if (avgReward > 60)
      return true;
    return false;
  };

  agent.Train(measure);
  Log::Debug << "Total test episodes: " << testEpisodes << std::endl;
}

// Test async one step Sarsa in Cart Pole.
BOOST_AUTO_TEST_CASE(OneStepSarsaTest)
{
  /**
   * This is for the Travis CI server, in your own machine you shuold use more
   * threads.
   */
  #ifdef HAS_OPENMP
    omp_set_num_threads(1);
  #endif

  // Set up the network.
  FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
      GaussianInitialization(0, 0.001));
  model.Add<Linear<>>(4, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 2);

  // Set up the policy.
  using Policy = GreedyPolicy<CartPole>;
  AggregatedPolicy<Policy> policy({Policy(0.7, 5000, 0.1),
                                  Policy(0.7, 5000, 0.01),
                                  Policy(0.7, 5000, 0.5)},
                                  arma::colvec("0.4 0.3 0.3"));

  TrainingConfig config;
  config.StepSize() = 0.0001;
  config.Discount() = 0.99;
  config.NumWorkers() = 16;
  config.UpdateInterval() = 6;
  config.StepLimit() = 200;
  config.TargetNetworkSyncInterval() = 200;

  OneStepSarsa<CartPole, decltype(model), VanillaUpdate, decltype(policy)>
      agent(std::move(config), std::move(model), std::move(policy));

  arma::vec rewards(20, arma::fill::zeros);
  size_t pos = 0;
  size_t testEpisodes = 0;
  auto measure = [&rewards, &pos, &testEpisodes](double reward)
  {
    size_t maxEpisode = 100000;
    if (testEpisodes > maxEpisode)
      BOOST_REQUIRE(false);
    testEpisodes++;
    rewards[pos++] = reward;
    pos %= rewards.n_elem;
    // Maybe underestimated.
    double avgReward = arma::mean(rewards);
    Log::Debug << "Average return: " << avgReward
               << " Episode return: " << reward << std::endl;
    if (avgReward > 60)
      return true;
    return false;
  };

  agent.Train(measure);
  Log::Debug << "Total test episodes: " << testEpisodes << std::endl;
}

// Test async n step q-learning in Cart Pole.
BOOST_AUTO_TEST_CASE(NStepQLearningTest)
{
  /**
   * This is for the Travis CI server, in your own machine you shuold use more
   * threads.
   */
  #ifdef HAS_OPENMP
    omp_set_num_threads(1);
  #endif

  // Set up the network.
  FFN<MeanSquaredError<>, GaussianInitialization> model(MeanSquaredError<>(),
      GaussianInitialization(0, 0.001));
  model.Add<Linear<>>(4, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 20);
  model.Add<ReLULayer<>>();
  model.Add<Linear<>>(20, 2);

  // Set up the policy.
  using Policy = GreedyPolicy<CartPole>;
  AggregatedPolicy<Policy> policy({Policy(0.7, 5000, 0.1),
                                   Policy(0.7, 5000, 0.01),
                                   Policy(0.7, 5000, 0.5)},
                                  arma::colvec("0.4 0.3 0.3"));

  TrainingConfig config;
  config.StepSize() = 0.0001;
  config.Discount() = 0.99;
  config.NumWorkers() = 16;
  config.UpdateInterval() = 6;
  config.StepLimit() = 200;
  config.TargetNetworkSyncInterval() = 200;

  NStepQLearning<CartPole, decltype(model), VanillaUpdate, decltype(policy)>
      agent(std::move(config), std::move(model), std::move(policy));

  arma::vec rewards(20, arma::fill::zeros);
  size_t pos = 0;
  size_t testEpisodes = 0;
  auto measure = [&rewards, &pos, &testEpisodes](double reward)
  {
    size_t maxEpisode = 100000;
    if (testEpisodes > maxEpisode)
      BOOST_REQUIRE(false);
    testEpisodes++;
    rewards[pos++] = reward;
    pos %= rewards.n_elem;
    // Maybe underestimated.
    double avgReward = arma::mean(rewards);
    Log::Debug << "Average return: " << avgReward
               << " Episode return: " << reward << std::endl;
    if (avgReward > 60)
      return true;
    return false;
  };

  agent.Train(measure);
  Log::Debug << "Total test episodes: " << testEpisodes << std::endl;
}

BOOST_AUTO_TEST_SUITE_END();


#pragma once

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

#include "fft/ft_grid_helpers.hpp"

enum class derivative_t
{
  dX,
  dY
};

template <typename DERIVED1, typename DERIVED2>
double
compute_mass_entry(const Eigen::SparseMatrixBase<DERIVED1> &m1,
                   const Eigen::SparseMatrixBase<DERIVED2> &m2)
{
  int n = std::max(m1.rows(), m2.rows());
  int m = std::max(m1.cols(), m2.cols());

  typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sp_mat_t;
  sp_mat_t b1, b2;
  ftpad(b1, m1, n, m);
  ftpad(b2, m2, n, m);

  double sum = 0;

  for (int ii = 0; ii < b1.outerSize(); ++ii) {
    sp_mat_t::InnerIterator it2(b2, ii);
    for (sp_mat_t::InnerIterator it1(b1, ii); it1; ++it1) {
      while (it2.col() < it1.col()) ++it2;
      if (!bool(it2))
        break;
      else if (it2.col() == it1.col())
        sum += it2.value() * it1.value();
      else
        continue;
    }
  }
  return sum;
}

/**
 * \f$ computes ... \f$
 *
 * @param m1
 * @param m2
 * @param tdx
 * @param tdy
 *
 * @return
 */
template <typename DERIVED1, typename DERIVED2>
double
compute_mass_entry(const Eigen::SparseMatrixBase<DERIVED1> &m1,
                   const Eigen::SparseMatrixBase<DERIVED2> &m2,
                   double tdx,
                   double tdy)
{
  const double PI = boost::math::constants::pi<double>();

  int n = std::max(m1.rows(), m2.rows());
  int m = std::max(m1.cols(), m2.cols());

  typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sp_mat_t;
  sp_mat_t b1, b2;
  ftpad(b1, m1, n, m);
  ftpad(b2, m2, n, m);

  double sum = 0;

  for (int ii = 0; ii < b1.outerSize(); ++ii) {
    int yhat = to_freq(ii, n);
    sp_mat_t::InnerIterator it2(b2, ii);
    for (sp_mat_t::InnerIterator it1(b1, ii); it1; ++it1) {
      while (bool(it2) && it2.col() < it1.col()) ++it2;
      if (!bool(it2))
        break;
      else if (it2.col() == it1.col()) {
        int xhat = to_freq(it2.col(), m);
        const double f = std::cos(2 * PI * (tdy * yhat + tdx * xhat));
        sum += it2.value() * it1.value() * f;
      }

      else
        continue;
    }
  }
  return sum;
}

/**
 * \f$ \int_{L_x, L_y} \partial_k r_{i1}(x) \partial_l r_{i2}(x) \mathrm{d}x \f$
 *
 * @param m1 Fourier coefficients corresponding to r_{i1}
 * @param m2 Fourier coefficients corresponding to r_{i2}
 * @param tdx translation grid difference vector in x
 * @param tdy translation grid difference vector in y
 * @param dm1 derivative applied to \f$r_{i1}\f$
 * @param dm2 derivative applied to \f$r_{i2}\f$
 *
 * @return
 */
template <typename DERIVED1, typename DERIVED2>
double
compute_tentry(const Eigen::SparseMatrixBase<DERIVED1> &m1,
               const Eigen::SparseMatrixBase<DERIVED2> &m2,
               double tdx,
               double tdy,
               derivative_t dm1,
               derivative_t dm2,
               double Lx = 1.0,
               double Ly = 1.0)
{
  const double PI = boost::math::constants::pi<double>();
  const double PI2 = PI * PI;

  int n = std::max(m1.rows(), m2.rows());
  int m = std::max(m1.cols(), m2.cols());

  typedef Eigen::SparseMatrix<double, Eigen::RowMajor> sp_mat_t;
  sp_mat_t b1, b2;
  ftpad(b1, m1, n, m);
  ftpad(b2, m2, n, m);

  double sum = 0;
  if (dm1 == derivative_t::dX && dm2 == derivative_t::dX) {
    for (int ii = 0; ii < b1.outerSize(); ++ii) {
      int yhat = to_freq(ii, n);
      sp_mat_t::InnerIterator it2(b2, ii);
      for (sp_mat_t::InnerIterator it1(b1, ii); it1; ++it1) {
        while (bool(it2) && it2.col() < it1.col()) ++it2;
        if (!bool(it2))
          break;
        else if (it2.col() == it1.col()) {
          int xhat = to_freq(it2.col(), m);
          const double f = std::cos(2 * PI * (tdy * yhat + tdx * xhat));
          sum += xhat * xhat / (Lx * Lx) * it2.value() * it1.value() * f;
        } else
          continue;
      }
    }
  } else if (dm1 == derivative_t::dY && dm2 == derivative_t::dY) {
    for (int ii = 0; ii < b1.outerSize(); ++ii) {
      int yhat = to_freq(ii, n);
      sp_mat_t::InnerIterator it2(b2, ii);
      for (sp_mat_t::InnerIterator it1(b1, ii); it1; ++it1) {
        while (bool(it2) && it2.col() < it1.col()) ++it2;
        if (!bool(it2))
          break;
        else if (it2.col() == it1.col()) {
          int xhat = to_freq(it2.col(), m);
          const double f = std::cos(2 * PI * (tdy * yhat + tdx * xhat));
          sum += yhat * yhat / (Ly * Ly) * it2.value() * it1.value() * f;
        } else
          continue;
      }
    }
  } else {
    // dm1 == dY and dm2 == dX or vice versa
    for (int ii = 0; ii < b1.outerSize(); ++ii) {
      int yhat = to_freq(ii, n);
      sp_mat_t::InnerIterator it2(b2, ii);
      for (sp_mat_t::InnerIterator it1(b1, ii); it1; ++it1) {
        while (bool(it2) && it2.col() < it1.col()) ++it2;
        if (!bool(it2))
          break;
        else if (it2.col() == it1.col()) {
          int xhat = to_freq(it2.col(), m);
          const double f = std::cos(2 * PI * (tdy * yhat + tdx * xhat));
          sum += xhat * yhat / (Lx * Ly) * it2.value() * it1.value() * f;
        } else
          continue;
      }
    }
  }

  return 4 * PI2 * sum;
}


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#ifndef MODULES_GEOMETRY_POLYGON_HPP_
#define MODULES_GEOMETRY_POLYGON_HPP_

#include <vector>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include "commons.hpp"
#include "modules/geometry/line.hpp"

namespace modules {
namespace geometry {

//! templated polygon class with a boost polygon as a member function
template <typename T>
struct Polygon_t : public Shape<bg::model::polygon<T>, T> {
  Polygon_t() : Shape<bg::model::polygon<T>, T>(Pose(0, 0, 0),
    std::vector<T>(), 0) {}
  Polygon_t(const Pose &center, const std::vector<T>& points) :
    Shape<bg::model::polygon<T>, T>(center, points, 0) {}

  Polygon_t(
      const Pose &center,
      const Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> &points) : Shape<bg::model::polygon<T>, T>(center, points, 0) {}

  virtual Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> toArray() const;

  virtual Shape<bg::model::polygon<T>, T> *Clone() const;

};

template <typename T>
inline Shape<bg::model::polygon<T>, T> *Polygon_t<T>::Clone() const {
  return new Polygon_t<T>(*this);
}

//! for better usage simple float defines
using PolygonPoint = Point2d;  // for internal stores of collision checkers
using Polygon = Polygon_t<PolygonPoint>;

template <>
inline Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> Polygon::toArray() const {
  std::vector<Point2d> points = obj_.outer();
  Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic> mat(points.size(), 2);
  for (std::vector<Point2d>::size_type i = 0; i < points.size(); ++i) {
    mat.row(i) << bg::get<0>(points[i]), bg::get<1>(points[i]);
  }
  return mat;
}

inline bool equals(const Polygon &poly1, const Polygon &poly2) {
  return bg::equals(poly1.obj_, poly2.obj_);
}

inline float distance(const Polygon &poly, const Point2d &p) {
  return bg::distance(poly.obj_, p);
}

inline float distance(const Polygon &poly, const Line &l) {
  return bg::distance(poly.obj_, l.obj_);
}

inline float distance(const Polygon &poly1, const Polygon &poly2) {
  return bg::distance(poly1.obj_, poly2.obj_);
}

//! Polygon - Point collision checker using boost::within
inline bool Collide(const Polygon &poly, const PolygonPoint &p) {
  return bg::within(p, poly.obj_);
}

//! Point - Polygon collision checker using boost::within
inline bool Collide(const PolygonPoint &p, const Polygon &poly) {
  return Collide(poly, p);
}

//! Polygon - Line collision checker using boost::intersection
//! @note we only check the shape intersection(s) and no line RHS/LHS line crossing!
inline bool Collide(const Polygon &poly, const Line &l) {
  std::vector<bg::model::linestring<LinePoint>> shape_intersect;
  bg::intersection(poly.obj_, l.obj_, shape_intersect);
  const bool inner_intersection = !shape_intersect.empty();
  if (inner_intersection) {
    return inner_intersection;
  } else {
    // boost interection does not treat edge intersections as intersection1,
    // but this shall be a collision! -> cast poly edge to line and re-check collision.
    Line outer_polyline;
    //! @todo geht das eleganter?
    for (auto it = boost::begin(boost::geometry::exterior_ring(poly.obj_)); it != boost::end(boost::geometry::exterior_ring(poly.obj_)); ++it) {
      outer_polyline.add_point(*it);
    }
    return Collide(outer_polyline, l);
  }
}

//! Line - Polygon collision checker using boost::intersection
inline bool Collide(const Line &line, const Polygon &poly) {
  return Collide(poly, line);
}

//! Polygon - Polygon collision checker using boost::intersection
//! @todo might not be very efficient without Strategy...
inline bool Collide(const Polygon &poly1, const Polygon &poly2) {
  std::vector<bg::model::polygon<PolygonPoint>> shape_intersect;
  bg::intersection(poly1.obj_, poly2.obj_, shape_intersect);
  const bool inner_intersection = !shape_intersect.empty();
  if (inner_intersection) {
    return inner_intersection;
  } else {
    // boost interection does not treat edge intersections as intersection,
    // but this shall be a collision! -> cast poly edge to line and re-check collision.
    Line outer_polyline1;
    Line outer_polyline2;
    //! @todo geht das eleganter?
    for (auto it = boost::begin(boost::geometry::exterior_ring(poly1.obj_)); it != boost::end(boost::geometry::exterior_ring(poly1.obj_)); ++it) {
      outer_polyline1.add_point(*it);
    }
    for (auto it = boost::begin(boost::geometry::exterior_ring(poly2.obj_)); it != boost::end(boost::geometry::exterior_ring(poly2.obj_)); ++it) {
      outer_polyline2.add_point(*it);
    }
    return Collide(outer_polyline1, outer_polyline2);
  }
}

}  // namespace geometry
}  // namespace modules

#endif  // MODULES_GEOMETRY_POLYGON_HPP_


/*
Copyright 2010 Intel Corporation

Use, modification and distribution are subject to the Boost Software License,
Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
http://www.boost.org/LICENSE_1_0.txt).
*/
#include <iostream>
#include <boost/polygon/polygon.hpp>

typedef boost::polygon::point_data<int> point;
typedef boost::polygon::polygon_set_data<int> polygon_set;
typedef boost::polygon::polygon_with_holes_data<int> polygon;
typedef std::pair<point, point> edge;
using namespace boost::polygon::operators;

void convolve_two_segments(std::vector<point>& figure, const edge& a, const edge& b) {
  using namespace boost::polygon;
  figure.clear();
  figure.push_back(point(a.first));
  figure.push_back(point(a.first));
  figure.push_back(point(a.second));
  figure.push_back(point(a.second));
  convolve(figure[0], b.second);
  convolve(figure[1], b.first);
  convolve(figure[2], b.first);
  convolve(figure[3], b.second);
}

template <typename itrT1, typename itrT2>
void convolve_two_point_sequences(polygon_set& result, itrT1 ab, itrT1 ae, itrT2 bb, itrT2 be) {
  using namespace boost::polygon;
  if(ab == ae || bb == be)
    return;
  point first_a = *ab;
  point prev_a = *ab;
  std::vector<point> vec;
  polygon poly;
  ++ab;
  for( ; ab != ae; ++ab) {
    point first_b = *bb;
    point prev_b = *bb;
    itrT2 tmpb = bb;
    ++tmpb;
    for( ; tmpb != be; ++tmpb) {
      convolve_two_segments(vec, std::make_pair(prev_b, *tmpb), std::make_pair(prev_a, *ab));
      set_points(poly, vec.begin(), vec.end());
      result.insert(poly);
      prev_b = *tmpb;
    }
    prev_a = *ab;
  }
}

template <typename itrT>
void convolve_point_sequence_with_polygons(polygon_set& result, itrT b, itrT e, const std::vector<polygon>& polygons) {
  using namespace boost::polygon;
  for(std::size_t i = 0; i < polygons.size(); ++i) {
    convolve_two_point_sequences(result, b, e, begin_points(polygons[i]), end_points(polygons[i]));
    for(polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(polygons[i]);
        itrh != end_holes(polygons[i]); ++itrh) {
      convolve_two_point_sequences(result, b, e, begin_points(*itrh), end_points(*itrh));
    }
  }
}

void convolve_two_polygon_sets(polygon_set& result, const polygon_set& a, const polygon_set& b) {
  using namespace boost::polygon;
  result.clear();
  std::vector<polygon> a_polygons;
  std::vector<polygon> b_polygons;
  a.get(a_polygons);
  b.get(b_polygons);
  for(std::size_t ai = 0; ai < a_polygons.size(); ++ai) {
    convolve_point_sequence_with_polygons(result, begin_points(a_polygons[ai]),
                                          end_points(a_polygons[ai]), b_polygons);
    for(polygon_with_holes_traits<polygon>::iterator_holes_type itrh = begin_holes(a_polygons[ai]);
        itrh != end_holes(a_polygons[ai]); ++itrh) {
      convolve_point_sequence_with_polygons(result, begin_points(*itrh),
                                            end_points(*itrh), b_polygons);
    }
    for(std::size_t bi = 0; bi < b_polygons.size(); ++bi) {
      polygon tmp_poly = a_polygons[ai];
      result.insert(convolve(tmp_poly, *(begin_points(b_polygons[bi]))));
      tmp_poly = b_polygons[bi];
      result.insert(convolve(tmp_poly, *(begin_points(a_polygons[ai]))));
    }
  }
}

namespace boost { namespace polygon{

  template <typename T>
  std::ostream& operator<<(std::ostream& o, const polygon_data<T>& poly) {
    o << "Polygon { ";
    for(typename polygon_data<T>::iterator_type itr = poly.begin();
        itr != poly.end(); ++itr) {
      if(itr != poly.begin()) o << ", ";
      o << (*itr).get(HORIZONTAL) << " " << (*itr).get(VERTICAL);
    }
    o << " } ";
    return o;
  }

  template <typename T>
  std::ostream& operator<<(std::ostream& o, const polygon_with_holes_data<T>& poly) {
    o << "Polygon With Holes { ";
    for(typename polygon_with_holes_data<T>::iterator_type itr = poly.begin();
        itr != poly.end(); ++itr) {
      if(itr != poly.begin()) o << ", ";
      o << (*itr).get(HORIZONTAL) << " " << (*itr).get(VERTICAL);
    } o << " { ";
    for(typename polygon_with_holes_data<T>::iterator_holes_type itr = poly.begin_holes();
        itr != poly.end_holes(); ++itr) {
      o << (*itr);
    }
    o << " } } ";
    return o;
  }
}}

int main(int argc, char **argv) {
  polygon_set a, b, c;
  a += boost::polygon::rectangle_data<int>(0, 0, 1000, 1000);
  a -= boost::polygon::rectangle_data<int>(100, 100, 900, 900);
  a += boost::polygon::rectangle_data<int>(1000, -1000, 1010, -990);
  std::vector<polygon> polys;
  std::vector<point> pts;
  pts.push_back(point(-40, 0));
  pts.push_back(point(-10, 10));
  pts.push_back(point(0, 40));
  pts.push_back(point(10, 10));
  pts.push_back(point(40, 0));
  pts.push_back(point(10, -10));
  pts.push_back(point(0, -40));
  pts.push_back(point(-10, -10));
  pts.push_back(point(-40, 0));
  polygon poly;
  boost::polygon::set_points(poly, pts.begin(), pts.end());
  b+=poly;
  pts.clear();
  pts.push_back(point(1040, 1040));
  pts.push_back(point(1050, 1045));
  pts.push_back(point(1045, 1050));
  boost::polygon::set_points(poly, pts.begin(), pts.end());
  b+=poly;
  polys.clear();
  convolve_two_polygon_sets(c, a, b);
  c.get(polys);
  for(int i = 0; i < polys.size(); ++i ){
    std::cout << polys[i] << std::endl;
  }
  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_POW_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_POW_ABS_HPP_INCLUDED

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

  /*!
    @ingroup group-exponential
    This function object computes \f$|x|^y\f$.

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

    @par Decorators

    - raw_  is faster but can be inaccurate.

    @see pow, abs

    @par Example:

      @snippet pow_abs.cpp pow_abs

    @par Possible output:

      @snippet pow_abs.txt pow_abs

  **/
  IEEEValue pow_abs(IEEEValue const& x, IEEEValue const& y);
} }
#endif

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

#endif


#include <iostream>
#include <cassert>
#include <vector>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor,
                                                                              boost::property<boost::edge_weight_t, long>>>>>
    Graph;

const int max_possible_bid = 100;

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

class EdgeAdder
{
  Graph &G;

public:
  explicit EdgeAdder(Graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost)
  {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G);
    const Graph::edge_descriptor e = boost::add_edge(from, to, G).first;
    const Graph::edge_descriptor rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;
    w_map[rev_e] = -cost;
  }
};

void testcase()
{
  int n, m, s;
  std::cin >> n >> m >> s;
  assert(n >= 1 && n <= 100 && m >= 1 && m <= 100 && s >= 1 && s <= m);

  std::vector<int> limits_by_state(s);
  for (int &limit : limits_by_state)
  {
    std::cin >> limit;
    assert(limit >= 0 && limit <= n);
  }

  std::vector<int> state_by_site(m);
  for (int &state : state_by_site)
  {
    std::cin >> state;
    assert(state >= 1 && state <= s);
    state--;
  }

  std::vector<std::vector<int>> bids_by_buyer(n, std::vector<int>(m));
  for (std::vector<int> &bids : bids_by_buyer)
  {
    for (int &bid : bids)
    {
      std::cin >> bid;
      assert(bid >= 1 && bid <= max_possible_bid);
    }
  }

  int next_free_node = 0;
  const int node_source = next_free_node++;
  const int node_sink = next_free_node++;
  const auto get_node_for_buyer = [next_free_node, n](int buyer) {
    assert(buyer >= 0 && buyer < n);
    return next_free_node + buyer;
  };
  next_free_node += n;
  const auto get_node_for_site = [next_free_node, m](int site) {
    assert(site >= 0 && site < m);
    return next_free_node + site;
  };
  next_free_node += m;
  const auto get_node_for_state = [next_free_node, s](int state) {
    assert(state >= 0 && state < s);
    return next_free_node + state;
  };
  next_free_node += s;
  const int num_nodes = next_free_node;

  Graph G(num_nodes);
  EdgeAdder adder(G);

  for (int i = 0; i < n; i++)
  {
    adder.add_edge(node_source, get_node_for_buyer(i), 1, 0);
    for (int j = 0; j < m; j++)
    {
      const int bid = bids_by_buyer.at(i).at(j);
      adder.add_edge(get_node_for_buyer(i), get_node_for_site(j), 1, -bid + max_possible_bid);
    }
  }

  for (int i = 0; i < m; i++)
  {
    adder.add_edge(get_node_for_site(i), get_node_for_state(state_by_site.at(i)), 1, 0);
  }

  for (int i = 0; i < s; i++)
  {
    adder.add_edge(get_node_for_state(i), node_sink, limits_by_state.at(i), 0);
  }

  const int flow = boost::push_relabel_max_flow(G, node_source, node_sink);
  assert(flow >= std::min(s, std::min(n, m)) && flow <= std::min(n, m));
  boost::successive_shortest_path_nonnegative_weights(G, node_source, node_sink);
  const int profit = -(boost::find_flow_cost(G) - max_possible_bid * flow);
  assert(profit > 0);
  std::cout << flow << " " << profit << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


/*
 * InnerMapTask.cpp
 *
 *  Created on: 25 Jul 2018
 *      Author: scsjd
 */

#include <fstream>
#include <sstream>
#include <iterator>
#include <chrono>
#include <NTL/ZZ.h>
#include <NTL/ZZ_p.h>
#include <NTL/vec_ZZ.h>
#include <jsoncpp/json/json.h>
#include "InnerMapTask.h"
#include "HE1CiphertextCPU.h"

InnerMapTask::InnerMapTask(int numPerLine, const char* parametersPath, const char* inputPath){
	this->inputPath = inputPath;
	this->numPerLine = numPerLine;
	parseParameters(parametersPath);
	totalSumTime=0;
	numberAdditions=0;
	totalProductTime=0;
	numberMultiplications=0;
}

InnerMapTask::~InnerMapTask(){
}

void InnerMapTask::parseParameters(const char* parametersPath){
	NTL::ZZ mod;
	std::ifstream ifs(parametersPath);
	if (ifs.is_open()){
		std::string json;
		getline(ifs,json);
		Json::Value root;   // will contains the root value after parsing.
		Json::Reader reader;
		bool parsingSuccessful = reader.parse(json,root);
		if (parsingSuccessful){
			modulus = NTL::conv<NTL::ZZ>(root["modulus"].asCString());
		}
	}
}

NTL::ZZ InnerMapTask::run(){
	std::ifstream ifs(inputPath);
	if (!ifs.is_open()){
		throw std::ios_base::failure("Could not open input file.");
	}
	std::string line;

	HE1CiphertextCPU::setModulus(modulus);
	NTL::ZZ zero;
	HE1CiphertextCPU sum(zero);

	while(getline(ifs,line)){
		NTL::ZZ one;
		set(one);
		HE1CiphertextCPU prod(one);
		std::istringstream iss(line);
		std::vector<std::string> words((std::istream_iterator<std::string>(iss)),std::istream_iterator<std::string>());
		for(int i = 0; i < words.size(); i++){
			NTL::ZZ z = NTL::conv<NTL::ZZ>(words[i].c_str());
			HE1CiphertextCPU tmp(z);
			auto start = std::chrono::high_resolution_clock::now();
			prod*=tmp;
			auto finish = std::chrono::high_resolution_clock::now();
			totalProductTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
			numberMultiplications++;
		}
		auto start = std::chrono::high_resolution_clock::now();
		sum+=prod;
		auto finish = std::chrono::high_resolution_clock::now();
		totalSumTime += std::chrono::duration_cast<std::chrono::nanoseconds>(finish-start).count();
		numberAdditions++;
	}
	ifs.close();

	return NTL::rep(sum.get_ciphertext());
};


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// See LICENSE file in the project root for full license information.
//
#include "InferenceTestImage.hpp"
#include "MobileNetDatabase.hpp"

#include <boost/numeric/conversion/cast.hpp>
#include <boost/assert.hpp>
#include <boost/format.hpp>

#include <iostream>
#include <fcntl.h>
#include <array>

namespace
{

inline float Lerp(float a, float b, float w)
{
    return w * b + (1.f - w) * a;
}

inline void PutData(std::vector<float> & data,
                    const unsigned int width,
                    const unsigned int x,
                    const unsigned int y,
                    const unsigned int c,
                    float value)
{
    data[(3*((y*width)+x)) + c] = value;
}

std::vector<float>
ResizeBilinearAndNormalize(const InferenceTestImage & image,
                           const unsigned int outputWidth,
                           const unsigned int outputHeight)
{
    std::vector<float> out;
    out.resize(outputWidth * outputHeight * 3);

    // We follow the definition of TensorFlow and AndroidNN: The top-left corner of a texel in the output
    // image is projected into the input image to figure out the interpolants and weights. Note that this
    // will yield different results than if projecting the centre of output texels.

    const unsigned int inputWidth = image.GetWidth();
    const unsigned int inputHeight = image.GetHeight();

    // How much to scale pixel coordinates in the output image to get the corresponding pixel coordinates
    // in the input image
    const float scaleY = boost::numeric_cast<float>(inputHeight) / boost::numeric_cast<float>(outputHeight);
    const float scaleX = boost::numeric_cast<float>(inputWidth) / boost::numeric_cast<float>(outputWidth);

    uint8_t rgb_x0y0[3];
    uint8_t rgb_x1y0[3];
    uint8_t rgb_x0y1[3];
    uint8_t rgb_x1y1[3];

    for (unsigned int y = 0; y < outputHeight; ++y)
    {
        // Corresponding real-valued height coordinate in input image
        const float iy = boost::numeric_cast<float>(y) * scaleY;

        // Discrete height coordinate of top-left texel (in the 2x2 texel area used for interpolation)
        const float fiy = floorf(iy);
        const unsigned int y0 = boost::numeric_cast<unsigned int>(fiy);

        // Interpolation weight (range [0,1])
        const float yw = iy - fiy;

        for (unsigned int x = 0; x < outputWidth; ++x)
        {
            // Real-valued and discrete width coordinates in input image
            const float ix = boost::numeric_cast<float>(x) * scaleX;
            const float fix = floorf(ix);
            const unsigned int x0 = boost::numeric_cast<unsigned int>(fix);

            // Interpolation weight (range [0,1])
            const float xw = ix - fix;

            // Discrete width/height coordinates of texels below and to the right of (x0, y0)
            const unsigned int x1 = std::min(x0 + 1, inputWidth - 1u);
            const unsigned int y1 = std::min(y0 + 1, inputHeight - 1u);

            std::tie(rgb_x0y0[0], rgb_x0y0[1], rgb_x0y0[2]) = image.GetPixelAs3Channels(x0, y0);
            std::tie(rgb_x1y0[0], rgb_x1y0[1], rgb_x1y0[2]) = image.GetPixelAs3Channels(x1, y0);
            std::tie(rgb_x0y1[0], rgb_x0y1[1], rgb_x0y1[2]) = image.GetPixelAs3Channels(x0, y1);
            std::tie(rgb_x1y1[0], rgb_x1y1[1], rgb_x1y1[2]) = image.GetPixelAs3Channels(x1, y1);

            for (unsigned c=0; c<3; ++c)
            {
                const float ly0 = Lerp(float(rgb_x0y0[c]), float(rgb_x1y0[c]), xw);
                const float ly1 = Lerp(float(rgb_x0y1[c]), float(rgb_x1y1[c]), xw);
                const float l = Lerp(ly0, ly1, yw);
                PutData(out, outputWidth, x, y, c, l/255.0f);
            }
        }
    }

    return out;
}

} // end of anonymous namespace

MobileNetDatabase::MobileNetDatabase(const std::string& binaryFileDirectory,
                                     unsigned int width,
                                     unsigned int height,
                                     const std::vector<ImageSet>& imageSet)
:   m_BinaryDirectory(binaryFileDirectory)
,   m_Height(height)
,   m_Width(width)
,   m_ImageSet(imageSet)
{
}

std::unique_ptr<MobileNetDatabase::TTestCaseData>
MobileNetDatabase::GetTestCaseData(unsigned int testCaseId)
{
    testCaseId = testCaseId % boost::numeric_cast<unsigned int>(m_ImageSet.size());
    const ImageSet& imageSet = m_ImageSet[testCaseId];
    const std::string fullPath = m_BinaryDirectory + imageSet.first;

    InferenceTestImage image(fullPath.c_str());

    // this ResizeBilinear result is closer to the tensorflow one than STB.
    // there is still some difference though, but the inference results are
    // similar to tensorflow for MobileNet
    std::vector<float> resized(ResizeBilinearAndNormalize(image, m_Width, m_Height));

    const unsigned int label = imageSet.second;
    return std::make_unique<TTestCaseData>(label, std::move(resized));
}


// Petter Strandmark 2013.

#include <cstring>
#include <iostream>
#include <limits>
#include <memory>
#include <random>
#include <stdexcept>

// GNU 4.8.1 define _X on Cygwin.
// This breaks Eigen.
// http://eigen.tuxfamily.org/bz/show_bug.cgi?id=658
#ifdef _X
#undef _X
#endif
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include <spii/spii.h>
#include <spii/solver.h>

#ifndef USE_SYM_ILDL

void spii::Solver::BKP_sym_ildl(const Eigen::MatrixXd& Hinput,
                                const Eigen::VectorXd& g,
                                Eigen::VectorXd* p,
                                SolverResults* results) const
{
	throw std::runtime_error("sym-ildl is not available.");
}

void spii::Solver::BKP_sym_ildl(const Eigen::SparseMatrix<double>& Hinput,
                                const Eigen::VectorXd& g,
                                Eigen::VectorXd* p,
                                SolverResults* results) const
{
	throw std::runtime_error("sym-ildl is not available.");
}

#else

#include <lilc_matrix.h>

#include <spii/sym-ildl-conversions.h>

namespace spii {

namespace{
void modify_block_diagonal_matrix(block_diag_matrix<double>* B)
{
	using namespace Eigen;

	auto n = B->n_rows();
	spii_assert(B->n_cols() == n);

	//
	// Modify the block diagonalization.
	//
	const double delta = 1e-12;

	VectorXd tau(n);
	VectorXd lambda(n);

	SelfAdjointEigenSolver<MatrixXd> eigensolver;

	bool onebyone;
	for (int i = 0; i < n; i = (onebyone ? i+1 : i+2) ) {
		onebyone = (i == n-1 || B->block_size(i) == 1);

		if ( onebyone ) {
			auto& Bii = (*B)[i];

			double lambda = Bii;
			double tau;
			if (lambda >= delta) {
				tau = 0;
			}
			else {
				tau = delta - (1.0 + delta) * lambda;
			}
			//Q(i, i) = 1;
			Bii = Bii + tau;
		}
		else {
			Matrix2d Bblock;
			Bblock(0, 0) = (*B)[i];
			Bblock(0, 1) = B->off_diagonal(i);
			Bblock(1, 0) = B->off_diagonal(i);
			Bblock(1, 1) = (*B)[i+1];
			spii_assert(Bblock(1, 0) == Bblock(0, 1));

			eigensolver.compute(Bblock);
			Vector2d lambda;
			lambda(0) = eigensolver.eigenvalues()(0);
			lambda(1) = eigensolver.eigenvalues()(1);

			Vector2d tau;
			for (int k = 0; k < 2; ++k) {
				if (lambda(k) >= delta) {
					tau(k) = 0;
				}
				else {
					tau(k) = delta - (1.0 + delta) * lambda(k);
				}
			}

			Matrix2d Qblock = eigensolver.eigenvectors();
			Bblock = Bblock + Qblock * tau.asDiagonal() * Qblock.transpose();
			(*B)[i]            = Bblock(0, 0);
			B->off_diagonal(i) = Bblock(0, 1);
			(*B)[i+1]          = Bblock(1, 1);
		}
	}
}
} // anon. namespace

template<typename MatrixType>
void BKP_sym_ildl_generic(const MatrixType& Hinput,
                          const Eigen::VectorXd& g,
                          Eigen::VectorXd* p,
                          SolverResults* results)
{
	using namespace std;
	using namespace Eigen;
	double start_time = wall_time();

	//
	// Create sym-ildl matrix.
	//
	lilc_matrix<double> Hlilc;
	eigen_to_lilc(Hinput, &Hlilc);

	//
	// Factorize the matrix.
	//
	lilc_matrix<double> L;	          // The lower triangular factor of A.
	vector<int> perm;	                  // A permutation vector containing all permutations on A.
	perm.reserve(Hlilc.n_cols());
	block_diag_matrix<double> B; // The diagonal factor of A.

	Hlilc.sym_amd(perm);
	Hlilc.sym_perm(perm);

	const double fill_factor = 1.0;
	const double tol         = 1e-12;
	const double pp_tol      = 1.0; // For full Bunch-Kaufman.
	Hlilc.ildl(L, B, perm, fill_factor, tol, pp_tol);

	// Convert back to Eigen matrices.
	MyPermutation P(perm);
	auto S = diag_to_eigen(Hlilc.S);

	//
	// Modify the block diagonalization.
	//
	modify_block_diagonal_matrix(&B);

	results->matrix_factorization_time += wall_time() - start_time;
	//
	// Solve the system.
	//
	start_time = wall_time();

	solve_system_ildl(B, L, S, P, -g, p);

	results->linear_solver_time += wall_time() - start_time;
}

void Solver::BKP_sym_ildl(const Eigen::MatrixXd& Hinput,
                          const Eigen::VectorXd& g,
                          Eigen::VectorXd* p,
                          SolverResults* results) const
{
	BKP_sym_ildl_generic(Hinput, g, p, results);
}

void Solver::BKP_sym_ildl(const Eigen::SparseMatrix<double>& Hinput,
                          const Eigen::VectorXd& g,
                          Eigen::VectorXd* p,
                          SolverResults* results) const
{
	BKP_sym_ildl_generic(Hinput, g, p, results);
}

}  // namespace spii
#endif // #ifndef USE_SYM_ILDL


//==================================================================================================
/*!
  @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)
*/
//==================================================================================================
#include <boost/simd/pack.hpp>
#include <boost/simd/function/significants.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/meta/cardinal_of.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

template <typename T, std::size_t N, typename Env>
void test(Env& runtime)
{

  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using iT =  bd::as_integer_t<T>;
  using p_t = bs::pack<T, N>;
  using pi_t= bs::pack<iT, N>;

  T a1[N], b[N];
  iT a2[N];
  for(std::size_t i = 0; i < N; ++i)
  {
    a1[i] = (i%2) ? T(i) : bs::rec(T(i));
    a2[i] = i+2;
    b[i] = bs::significants(a1[i], a2[i]) ;
  }
  p_t aa1(&a1[0], &a1[0]+N);
  pi_t aa2(&a2[0], &a2[0]+N);
  p_t bb (&b[0], &b[0]+N);

  STF_ULP_EQUAL(bs::significants(aa1, aa2), bb, 0.5);
}

STF_CASE_TPL("Check significants on pack" , STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using p_t = bs::pack<T>;
  static const std::size_t N = bs::cardinal_of<p_t>::value;
  test<T, N>(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}

STF_CASE_TPL (" significants",  (float))//STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::significants;
  using p_t = bs::pack<T>;

  using ip_t =  bd::as_integer_t<p_t>;
  using r_t = decltype(significants(p_t(), ip_t()));

  // return type conformity test
  STF_TYPE_IS( r_t, p_t );

#ifndef BOOST_SIMD_NO_INVALIDS
   STF_ULP_EQUAL(significants(bs::Inf<p_t>(), ip_t(1)), bs::Inf<r_t>(), 0.5);
   STF_ULP_EQUAL(significants(bs::Minf<p_t>(), ip_t(1)), bs::Minf<r_t>(), 0.5);
   STF_ULP_EQUAL(significants(bs::Nan<p_t>(),ip_t(1)), bs::Nan<r_t>(), 0.5);
#endif
   STF_ULP_EQUAL(significants(p_t(0), ip_t(1)), p_t(0), 0.5);
   STF_ULP_EQUAL(significants(p_t(25.34), ip_t(1)), p_t(30), 0.5);
   STF_ULP_EQUAL(significants(p_t(25.34), ip_t(2)), p_t(25), 0.5);
   STF_ULP_EQUAL(significants(p_t(25.34), ip_t(3)), p_t(25.3), 0.5);
   STF_ULP_EQUAL(significants(p_t(25.34), ip_t(4)), p_t(25.34), 0.5);
   STF_ULP_EQUAL(significants(p_t(-25.34), ip_t(1)), p_t(-30), 0.5);
   STF_ULP_EQUAL(significants(p_t(-25.34),ip_t(2)), p_t(-25), 0.5);
   STF_ULP_EQUAL(significants(p_t(-25.34), ip_t(3)), p_t(-25.3), 0.5);
   STF_ULP_EQUAL(significants(p_t(-25.34), ip_t(4)), p_t(-25.34), 0.5);
   STF_ULP_EQUAL(significants(p_t(-25.34), 4), p_t(-25.34), 0.5);
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file   testLinearEquality.cpp
 *  @brief  Unit tests for LinearEquality
 *  @author Duy-Nguyen Ta
 **/

#include <gtsam_unstable/linear/LinearEquality.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/VectorValues.h>
#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/vector.hpp>
#include <boost/assign/list_of.hpp>

using namespace std;
using namespace gtsam;
using namespace boost::assign;

GTSAM_CONCEPT_TESTABLE_INST (LinearEquality)

namespace {
namespace simple {
// Terms we'll use
const vector<pair<Key, Matrix> > terms = list_of < pair<Key, Matrix>
    > (make_pair(5, Matrix3::Identity()))(
        make_pair(10, 2 * Matrix3::Identity()))(
        make_pair(15, 3 * Matrix3::Identity()));

// RHS and sigmas
const Vector b = (Vector(3) << 1., 2., 3.).finished();
const SharedDiagonal noise = noiseModel::Constrained::All(3);
}
}

/* ************************************************************************* */
TEST(LinearEquality, constructors_and_accessors)
{
  using namespace simple;

  // Test for using different numbers of terms
  {
    // One term constructor
    LinearEquality expected(
        boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0);
    LinearEquality actual(terms[0].first, terms[0].second, b, 0);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[0].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(assert_equal(*noise, *actual.get_model()));
  }
  {
    // Two term constructor
    LinearEquality expected(
        boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0);
    LinearEquality actual(terms[0].first, terms[0].second,
        terms[1].first, terms[1].second, b, 0);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(assert_equal(*noise, *actual.get_model()));
  }
  {
    // Three term constructor
    LinearEquality expected(
        boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0);
    LinearEquality actual(terms[0].first, terms[0].second,
        terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0);
    EXPECT(assert_equal(expected, actual));
    LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back());
    EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1)));
    EXPECT(assert_equal(b, expected.getb()));
    EXPECT(assert_equal(b, actual.getb()));
    EXPECT(assert_equal(*noise, *actual.get_model()));
  }
}

/* ************************************************************************* */
TEST(LinearEquality, Hessian_conversion) {
  HessianFactor hessian(0, (Matrix(4,4) <<
          1.57, 2.695, -1.1, -2.35,
          2.695, 11.3125, -0.65, -10.225,
          -1.1, -0.65, 1, 0.5,
          -2.35, -10.225, 0.5, 9.25).finished(),
      (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
      73.1725);

  try {
    LinearEquality actual(hessian);
    EXPECT(false);
  }
  catch (const std::runtime_error& exception) {
    EXPECT(true);
  }
}

/* ************************************************************************* */
TEST(LinearEquality, error)
{
  LinearEquality factor(simple::terms, simple::b, 0);

  VectorValues values;
  values.insert(5, Vector::Constant(3, 1.0));
  values.insert(10, Vector::Constant(3, 0.5));
  values.insert(15, Vector::Constant(3, 1.0/3.0));

  Vector expected_unwhitened(3); expected_unwhitened << 2.0, 1.0, 0.0;
  Vector actual_unwhitened = factor.unweighted_error(values);
  EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));

  // whitened is meaningless in constraints
  Vector expected_whitened(3); expected_whitened = expected_unwhitened;
  Vector actual_whitened = factor.error_vector(values);
  EXPECT(assert_equal(expected_whitened, actual_whitened));

  double expected_error = 0.0;
  double actual_error = factor.error(values);
  DOUBLES_EQUAL(expected_error, actual_error, 1e-10);
}

/* ************************************************************************* */
TEST(LinearEquality, matrices_NULL)
{
  // Make sure everything works with NULL noise model
  LinearEquality factor(simple::terms, simple::b, 0);

  Matrix AExpected(3, 9);
  AExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
  Vector rhsExpected = simple::b;
  Matrix augmentedJacobianExpected(3, 10);
  augmentedJacobianExpected << AExpected, rhsExpected;

  // Whitened Jacobian
  EXPECT(assert_equal(AExpected, factor.jacobian().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));

  // Unwhitened Jacobian
  EXPECT(assert_equal(AExpected, factor.jacobianUnweighted().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
}

/* ************************************************************************* */
TEST(LinearEquality, matrices)
{
  // And now witgh a non-unit noise model
  LinearEquality factor(simple::terms, simple::b, 0);

  Matrix jacobianExpected(3, 9);
  jacobianExpected << simple::terms[0].second, simple::terms[1].second, simple::terms[2].second;
  Vector rhsExpected = simple::b;
  Matrix augmentedJacobianExpected(3, 10);
  augmentedJacobianExpected << jacobianExpected, rhsExpected;

  Matrix augmentedHessianExpected =
  augmentedJacobianExpected.transpose() * simple::noise->R().transpose()
  * simple::noise->R() * augmentedJacobianExpected;

  // Whitened Jacobian
  EXPECT(assert_equal(jacobianExpected, factor.jacobian().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobian().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobian()));

  // Unwhitened Jacobian
  EXPECT(assert_equal(jacobianExpected, factor.jacobianUnweighted().first));
  EXPECT(assert_equal(rhsExpected, factor.jacobianUnweighted().second));
  EXPECT(assert_equal(augmentedJacobianExpected, factor.augmentedJacobianUnweighted()));
}

/* ************************************************************************* */
TEST(LinearEquality, operators )
{
  Matrix I = I_2x2;
  Vector b = (Vector(2) << 0.2,-0.1).finished();
  LinearEquality lf(1, -I, 2, I, b, 0);

  VectorValues c;
  c.insert(1, (Vector(2) << 10.,20.).finished());
  c.insert(2, (Vector(2) << 30.,60.).finished());

  // test A*x
  Vector expectedE = (Vector(2) << 20.,40.).finished();
  Vector actualE = lf * c;
  EXPECT(assert_equal(expectedE, actualE));

  // test A^e
  VectorValues expectedX;
  expectedX.insert(1, (Vector(2) << -20.,-40.).finished());
  expectedX.insert(2, (Vector(2) << 20., 40.).finished());
  VectorValues actualX = VectorValues::Zero(expectedX);
  lf.transposeMultiplyAdd(1.0, actualE, actualX);
  EXPECT(assert_equal(expectedX, actualX));

  // test gradient at zero
  Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian();
  VectorValues expectedG;
  expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished());
  expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished());
  VectorValues actualG = lf.gradientAtZero();
  EXPECT(assert_equal(expectedG, actualG));
}

/* ************************************************************************* */
TEST(LinearEquality, default_error )
{
  LinearEquality f;
  double actual = f.error(VectorValues());
  DOUBLES_EQUAL(0.0, actual, 1e-15);
}

//* ************************************************************************* */
TEST(LinearEquality, empty )
{
  // create an empty factor
  LinearEquality f;
  EXPECT(f.empty());
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


#pragma once

#include <cstdint>
#include <utility>
#include <Eigen/Core>

namespace neon
{
using indices = Eigen::Array<std::int32_t, Eigen::Dynamic, Eigen::Dynamic>;

/// Type alias for whatever type is returned from these views
using index_view = decltype(std::declval<const indices>()(Eigen::placeholders::all, 0l));
}


#ifndef AIKIDO_COMMON_PSEUDOINVERSE_HPP_
#define AIKIDO_COMMON_PSEUDOINVERSE_HPP_

#include <Eigen/Dense>

namespace aikido {
namespace common {

/// Computes the Moore-Penrose pseudoinverse of a matrix.
///
/// \param mat input matrix
/// \param eps represents tolerance
/// \return pseudo-inverse of \c mat
Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd& mat, double eps = 1e-6);

} // namespace common
} // namespace aikido

#endif // AIKIDO_COMMON_PSEUDOINVERSE_HPP_