// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// 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 <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/first_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/bool.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::first_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;

using boost::mpl::apply_wrap1;
using boost::mpl::fold;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::eval_if;
using boost::mpl::bool_;
using boost::mpl::equal_to;
using boost::mpl::bool_;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= int_token ((mult_token | div_token) int_token)*
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct eval_plus
{
  template <class State, class C>
  struct apply :
    eval_if<
      is_c<front<C>, '+'>,
      plus<typename State::type, typename back<C>::type>,
      minus<typename State::type, typename back<C>::type>
    >
  {};
};

struct eval_mult
{
  template <class State, class C>
  struct apply :
    eval_if<
      is_c<front<C>, '*'>,
      times<typename State::type, typename back<C>::type>,
      divides<typename State::type, typename back<C>::type>
    >
  {};
};

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, int_token>,
    int_token,
    eval_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    eval_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > calculator_parser;

#ifdef _STR
#  error _STR already defined
#endif
#define _STR BOOST_METAPARSE_STRING

#ifdef BOOST_NO_CXX11_CONSTEXPR
int main()
{
  using std::cout;
  using std::endl;
  using boost::metaparse::string;

  cout
    << apply_wrap1<calculator_parser, string<'1','3'> >::type::value << endl
    <<
      apply_wrap1<
        calculator_parser, string<' ','1','+',' ','2','*','4','-','6','/','2'>
      >::type::value
    << endl
    ;
}
#else
int main()
{
  using std::cout;
  using std::endl;

  cout
    << apply_wrap1<calculator_parser, _STR("13")>::type::value << endl
    << apply_wrap1<calculator_parser, _STR(" 1+ 2*4-6/2")>::type::value << endl
    ;
}
#endif


/*
 * mapr_project.cpp
 *
 *  Created on: June 15, 2020
 *      Author: Kamil Miedzinski Mateusz Grycmacher
 *	 Institute: Instute of Home, Poznan University of Technology
 */

#include "../include/mapr_project/mapr_project.hpp"

// Boost
#include <boost/bind.hpp>
#include <boost/thread/recursive_mutex.hpp>

// STL
#include <string>
#include <math.h>
#include <limits>
#include <thread>

using namespace std;
using namespace ros;

namespace ob = ompl::base;
namespace og = ompl::geometric;

namespace mapr_project {

grid_map_msgs::GridMap gridMap;

double point_start_x = -0.5;
double point_start_y = -0.5;
double point_end_x = -5.0;
double point_end_y = -5.0;

class ValidityChecker : public ob::StateValidityChecker
{
public:
    ValidityChecker(const ob::SpaceInformationPtr& si) :
        ob::StateValidityChecker(si) {}

    bool isValid(const ob::State* state) const
    {
        return this->clearance(state);
    }

    double clearance(const ob::State* state) const
    {
        const ob::RealVectorStateSpace::StateType* state2D =
            state->as<ob::RealVectorStateSpace::StateType>();
        // Extract the robot's (x,y) position from its state
        double x = state2D->values[0];
        double y = state2D->values[0];
        int col = (x)/(-0.1);
     	int row = (y)/(-0.1);

	float resolution = gridMap.info.resolution;

  	float point_start_z = 0;
	float point_end_z = 0;

  	if(gridMap.info.length_x>0)  // Mapa wysokosci subskyrbowana
	{
		// Wysokosc z jakiej startujemy i do jakiej zmierzamy
		point_start_z = gridMap.data[0].data[(point_start_x/resolution*(-1)) + (point_start_y/resolution*(-1)) * 64];
		point_end_z = gridMap.data[0].data[(point_end_x/resolution*(-1)) + (point_end_y/resolution*(-1)) * 64];
		//std::cout << "point_start_z: " << point_start_z << " point_end_z: " << point_end_z << "\n";

		// Idziemy z gorki
		if(point_start_z > point_end_z)
		{
			if ((gridMap.data[0].data[col + row * 64]<point_start_z) && (gridMap.data[0].data[col + row * 64]>point_end_z))
	    		{

				return gridMap.data[0].data[col + row * 64];
	 		}
		 	else
			{
				return gridMap.data[0].data[col + row * 64]*gridMap.data[0].data[col + row * 64];
			}
		}
		else // Idziemy pod gore
		{
			if ((gridMap.data[0].data[col + row * 64]>point_start_z) && (gridMap.data[0].data[col + row * 64]<point_end_z))
	    		{
				return gridMap.data[0].data[col + row * 64];
	 		}
		 	else
			{
				return gridMap.data[0].data[col + row * 64]*gridMap.data[0].data[col + row * 64];
			}

		}
	}
	else // Brak mapy
	{
		return 1;
	}
    }
};

class ClearanceObjective : public ob::StateCostIntegralObjective
{
public:
    ClearanceObjective(const ob::SpaceInformationPtr& si) :
        ob::StateCostIntegralObjective(si, true)
    {
    }
    ob::Cost stateCost(const ob::State* s) const
    {
        return ob::Cost(si_->getStateValidityChecker()->clearance(s));
    }
};

//Optimization - getPathLengthObjective
 ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr& si)
 {
     return ob::OptimizationObjectivePtr(new ob::PathLengthOptimizationObjective(si));
 }

//Optimization - getClearanceObjective
ob::OptimizationObjectivePtr getClearanceObjective(const ob::SpaceInformationPtr& si)
{
    return std::make_shared<ClearanceObjective>(si);

}

//Optimization - getBalancedObjective
ob::OptimizationObjectivePtr getBalancedObjective(const ob::SpaceInformationPtr& si)
{
    ob::OptimizationObjectivePtr lengthObj(new ob::PathLengthOptimizationObjective(si));
    ob::OptimizationObjectivePtr clearObj(new ClearanceObjective(si));
    ob::MultiOptimizationObjective* opt = new ob::MultiOptimizationObjective(si);
    opt->addObjective(lengthObj, 1.0);
    opt->addObjective(clearObj, 10.0);
    return ob::OptimizationObjectivePtr(opt);
}

Planner2D::Planner2D(ros::NodeHandle& _nodeHandle)
    : nodeHandle(_nodeHandle)
{
    ROS_INFO("Planner node started.");
    configure(point_start_x, point_start_y, point_end_x, point_end_y);
}

Planner2D::~Planner2D()
{
}

void Planner2D::returnPoints(std_msgs::UInt8 pStartX, std_msgs::UInt8 pStartY,
                             std_msgs::UInt8 pEndX, std_msgs::UInt8 pEndY){
    point_start_x = double(pStartX.data) * (-0.1);
    point_start_y = double(pStartY.data) * (-0.1);
    point_end_x = double(pEndX.data) * (-0.1);
    point_end_y = double(pEndY.data) * (-0.1);
}

/// extract path
nav_msgs::Path Planner2D::extractPath(ob::ProblemDefinition* pdef){
   nav_msgs::Path plannedPath;
    plannedPath.header.frame_id = "/map";
    // get the obtained path
    ob::PathPtr path = pdef->getSolutionPath();
    // print the path to screen
    path->print(std::cout);
    // convert to geometric path
    const auto *path_ = path.get()->as<og::PathGeometric>();
    // iterate over each position
    for(unsigned int i=0; i<path_->getStateCount(); ++i){
        // get state
        const ob::State* state = path_->getState(i);

	const ob::RealVectorStateSpace::StateType* state2D = state->as<ob::RealVectorStateSpace::StateType>();

        // Extract the robot's (x,y) position from its state
        double x = state2D->values[0];
        double y = state2D->values[1];
	// potrzebne do obliczania wspl. Z sciezki
	int col = (x)/(-0.1);
     	int row = (y)/(-0.1);

        // fill in the ROS PoseStamped structure...
        geometry_msgs::PoseStamped poseMsg;
        poseMsg.pose.position.x = x;
        poseMsg.pose.position.y = y;
	if (gridMap.info.length_x)
    	{
		poseMsg.pose.position.z = gridMap.data[0].data[col + row * 64] +0.1;
 	}
	else
	{
		poseMsg.pose.position.z = 0;
	}
        poseMsg.pose.orientation.w = 1.0;
        poseMsg.pose.orientation.x = 0.0;
        poseMsg.pose.orientation.y = 0.0;
        poseMsg.pose.orientation.z = 0.0;
        poseMsg.header.frame_id = "/map";
        poseMsg.header.stamp = ros::Time::now();
        // ... and add the pose to the path
        plannedPath.poses.push_back(poseMsg);
    }
    return plannedPath;
}

nav_msgs::Path Planner2D::planPath(const grid_map_msgs::GridMap& globalMap){

    	gridMap = globalMap;
	configure(point_start_x, point_start_y, point_end_x, point_end_y);

	//std::cout << "point_start_x" << point_start_x << std::endl;
	//std::cout << "point_start_y" << point_start_y << std::endl;

   	 // Construct the robot state space in which we're planning. We're
	// planning in [0,1]x[0,1], a subset of R^2.
	ob::StateSpacePtr space(new ob::RealVectorStateSpace(2));

	// Set the bounds of space to be in [0,1].
	space->as<ob::RealVectorStateSpace>()->setBounds(-6.4, 0.0);

	// Construct a space information instance for this state space
	ob::SpaceInformationPtr si(new ob::SpaceInformation(space));

	 // Set the object used to check which states in the space are valid
	si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
	si->setup();

	// Set our robot's starting state to be the bottom-left corner of
	// the environment, or (0,0).
	ob::ScopedState<> start(space);
	start->as<ob::RealVectorStateSpace::StateType>()->values[0] = point_start_x;
	start->as<ob::RealVectorStateSpace::StateType>()->values[1] = point_start_y;

	// Set our robot's goal state to be the top-right corner of the
	// environment, or (5,5).
	ob::ScopedState<> goal(space);
	goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = point_end_x;
	goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = point_end_y;

	// Create a problem instance
	ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));

	// Set the start and goal states
	pdef->setStartAndGoalStates(start, goal);

	// Create the optimization objective
	pdef->setOptimizationObjective(getBalancedObjective(si));

	// Construct our optimizing planner using the RRTstar algorithm.
	auto optimizingPlanner(std::make_shared<og::RRTstar>(si));
	optimizingPlanner->setRange(maxStepLength);// max step length

	// Set the problem instance for our planner to solve
	optimizingPlanner->setProblemDefinition(pdef);
	optimizingPlanner->setup();

	// attempt to solve the planning problem within one second of
	// planning time
	ob::PlannerStatus solved = optimizingPlanner->ob::Planner::solve(1.0);

 	nav_msgs::Path plannedPath;
	 if (solved)
 	    {
  	    	plannedPath=extractPath(pdef.get());
 	    }
  	   else
	    {
  	       std::cout << "No solution found." << std::endl;
	    }

	 return plannedPath;
}

/// configure planner
void Planner2D::configure(double point_start_x, double point_start_y, double point_end_x, double point_end_y)
{
        maxStepLength = 0.05;// max step length
}

} /* namespace */


//=======================================================================
// Copyright (c)
//
// 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 tree_augmentation_example.cpp
 * @brief
 * @author Attila Bernath, Piotr Godlewski
 * @version 1.0
 * @date 2013-10-17
 */

//! [Tree Augmentation Example]
#include "paal/iterative_rounding/treeaug/tree_augmentation.hpp"

#include <boost/graph/adjacency_list.hpp>

#include <iostream>
#include <vector>

int main() {
    using EdgeProp = boost::property<boost::edge_weight_t, double,
                boost::property<boost::edge_color_t, bool>>;
    using Graph = boost::adjacency_list<boost::vecS, boost::vecS,
                boost::undirectedS, boost::no_property, EdgeProp>;
    using Edge = boost::graph_traits<Graph>::edge_descriptor;

    // sample problem
    std::vector<std::pair<int, int>> edges {{0,1},{1,2},{1,3},{3,4},{3,5},
            {0,3},{0,3},{2,4},{2,5},{4,5}};
    std::vector<EdgeProp> edge_properties {EdgeProp(0, true),
        EdgeProp(0, true), EdgeProp(0, true), EdgeProp(0, true),
        EdgeProp(0, true), EdgeProp(1, false), EdgeProp(1, false),
        EdgeProp(1, false), EdgeProp(1, false), EdgeProp(1,false)};

    Graph g(edges.begin(), edges.end(), edge_properties.begin(), 6);

    std::vector<Edge> solution;

    // optional input validity checking
    auto tree_aug = paal::ir::make_tree_aug(g, std::back_inserter(solution));
    auto error = tree_aug.check_input_validity();
    if (error) {
        std::cerr << "The input is not valid!" << std::endl;
        std::cerr << *error << std::endl;
        return -1;
    }

    // solve it
    auto result = paal::ir::tree_augmentation_iterative_rounding(
        g, std::back_inserter(solution));

    // print result
    if (result.first == paal::lp::OPTIMAL) {
        std::cout << "The solution contains the following nontree edges:"
            << std::endl;
        for (auto e : solution) {
            std::cout << "Edge " << e << std::endl;
        }
        std::cout << "Cost of the solution: " << *(result.second) << std::endl;
    } else {
        std::cout << "The instance is infeasible" << std::endl;
    }
    paal::lp::glp::free_env();
    return 0;
}
    //! [Tree Augmentation Example]


/*
 * Copyright (C) 2018-2020 HERE Europe B.V.
 *
 * 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.
 *
 * SPDX-License-Identifier: Apache-2.0
 * License-Filename: LICENSE
 */

//
// Created by Mitra, Aniket on 2019-04-03.
//

#include "movetk/utils/GeometryBackendTraits.h"
#include "movetk/utils/Iterators.h"
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// treat a trajectory as a directed graph and
// use the BFS algorithm to find the shortest path
int main(int argc, char **argv)
{
#if CGAL_BACKEND_ENABLED
    std::cerr << "Using CGAL Backend for Geometry\n";
#else
    std::cerr << "Using Boost Backend for Geometry\n";
#endif

    std::cout.setf(std::ios::fixed);

    // PolyLine is a data structure that to store a collection of points
    typedef std::vector<GeometryKernel::MovetkGeometryKernel::MovetkPoint> PolyLine;

    // Define a graph using boost::adjacency_list
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS,
                                  boost::property<boost::vertex_index_t, int>, boost::property<boost::edge_index_t, int>>
        Graph;

    // Vertex iterator type
    typedef typename boost::graph_traits<Graph>::vertex_iterator vertex_iterator;
    // Edge iterator type
    typedef typename boost::graph_traits<Graph>::edge_iterator edge_iterator;

    typedef boost::property_map<Graph, boost::vertex_index_t>::type VertexId_PMap;

    // functor to create a point
    movetk_core::MakePoint<GeometryKernel::MovetkGeometryKernel> make_point;

    // create polyline, each point in the polyline is vertex
    PolyLine polyline({make_point({-6.19, -3.46}), make_point({-4.99, 1.16}),
                       make_point({-2.79, -2.22}), make_point({-1.87, 0.58})});

    // print each vertex
    std::cout << "Polyline: ";
    std::cout << "{";
    for (auto &vertex : polyline)
    {
        std::cout << vertex;
        std::cout << ";";
    }
    std::cout << "}\n";

    // edges to connect vertex pairs
    std::vector<std::pair<std::size_t, std::size_t>> edges({
        std::make_pair(0, 1),
        std::make_pair(1, 2),
        std::make_pair(0, 2),
        std::make_pair(2, 3),
    });

    // print the edges
    std::cout << "Edges: ";
    std::cout << "{";
    for (auto &i : edges)
    {
        std::cout << i.first << "," << i.second << ";";
    }
    std::cout << "}\n";

    /** Pass iterator over edges and the number of vertices
     *  to create boost graph
    */
    Graph g{edges.begin(), edges.end(), polyline.size()};

    std::cout << "Number of vertices: " << boost::num_vertices(g) << '\n';
    std::cout << "Number of edges: " << boost::num_edges(g) << '\n';

    // container stores computed distances
    std::vector<std::size_t> distances(polyline.size());
    //
    std::vector<std::size_t> predecessors(polyline.size());

    predecessors[0] = 0;

    VertexId_PMap vertex_index = boost::get(boost::vertex_index, g);

    // iterator property map for recording distances
    boost::iterator_property_map<std::vector<std::size_t>::iterator, VertexId_PMap,
                                 std::size_t, std::size_t &>
        distances_pa(distances.begin(), vertex_index);

    /* iterator property map for recording the previous point as the
    * bfs algorithm proceeds to the next point
    */
    boost::iterator_property_map<std::vector<std::size_t>::iterator, VertexId_PMap,
                                 std::size_t, std::size_t &>
        predecessors_pa(predecessors.begin(), vertex_index);

    /** the helper function to create a visitor that record distances
    * distance is the number of lines that have to be crossed to get from one
    * point to another starting from the point that is passed as a second
    * argument to  boost::breadth_first_search
    */
    auto distance_recorder = boost::record_distances(distances_pa, boost::on_tree_edge{});
    /* returns a visitor to store the predecessor of every point
    *  whenever boost::breadth_first_search() visits a new point, the previous
    *  point is stored in the property map passed to boost::record_predecessors
    */
    auto predecessor_recorder = boost::record_predecessors(predecessors_pa, boost::on_tree_edge{});
    /* an adaptor that binds the two visitors distance_recorder & predecessor_recorder
    * to the algorithm
    */
    auto bfs_visitor = boost::make_bfs_visitor(std::make_pair(distance_recorder, predecessor_recorder));

    // the bfs algorithm on graph g
    // bfs_visitor stores the result
    // the algorithm starts at the first point as specified in the second argument
    boost::breadth_first_search(g, 0, boost::visitor(bfs_visitor));

    /* iterate over the graph and print all vertices, the distance
    * between two vertices, and the predecessor for each vertex
    */
    vertex_iterator vi, vi_end;
    for (std::tie(vi, vi_end) = boost::vertices(g); vi != vi_end; ++vi)
    {
        std::cout << "vertex index: " << *vi << ", distance: " << boost::get(distances_pa, *vi) << std::endl;
    }

    for (std::tie(vi, vi_end) = boost::vertices(g); vi != vi_end; ++vi)
    {
        std::cout << "vertex index: " << *vi << ", predecessor: " << boost::get(predecessors_pa, *vi) << std::endl;
    }

    /* get the index of the vertices returned by the
    * shortest path
    */
    std::tie(vi, vi_end) = boost::vertices(g);
    auto idx = boost::get(predecessors_pa, *(vi_end - 1));
    std::vector<std::size_t> indexes;
    indexes.push_back(*(vi_end - 1));
    while (idx != boost::get(predecessors_pa, *vi))
    {
        indexes.push_back(idx);
        idx = predecessors_pa[idx];
    }
    indexes.push_back(idx);

    /* the order of the vertices returned by the bfs algorithm
    * starts with the last vertex, so reverse the indexes
    * so that it can be used to access points from the polyline
    */
    std::reverse(std::begin(indexes), std::end(indexes));

    std::cout << "Shortest Path Polyline: ";
    std::cout << "{";
    for (auto &idx : indexes)
    {
        std::cout << polyline[idx] << ";";
    }
    std::cout << "}\n";
}


// 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_


#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


#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 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;
}


#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());
};


// 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


#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_


/********************************************************************************
Copyright (c) 2015, TRACLabs, Inc.
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
 are permitted provided that the following conditions are met:

    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.

    3. Neither the name of the copyright holder nor the names of its contributors
       may be used to endorse or promote products derived from this software
       without specific prior written permission.

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

#include <boost/date_time.hpp>
#include <boost/math/tools/precision.hpp>
#include <cmath>
#include <limits>
#include <trac_ik/dual_quaternion.h>
#include <trac_ik/nlopt_ik.hpp>

namespace NLOPT_IK
{

    dual_quaternion targetDQ;

    double minfunc(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        return c->minJoints(x, grad);
    }

    double minfuncDQ(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.
        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartDQError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartDQError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2 * jump);
            }
        }

        return result[0];
    }

    double minfuncSumSquared(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartSumSquaredError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartSumSquaredError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2.0 * jump);
            }
        }

        return result[0];
    }

    double minfuncL2(const std::vector<double> &x, std::vector<double> &grad, void *data)
    {
        // Auxilory function to minimize (Sum of Squared joint angle error
        // from the requested configuration).  Because we wanted a Class
        // without static members, but NLOpt library does not support
        // passing methods of Classes, we use these auxilary functions.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(x);

        double jump = boost::math::tools::epsilon<float>();
        double result[1];
        c->cartL2NormError(vals, result);

        if (!grad.empty())
        {
            double v1[1];
            for (uint i = 0; i < x.size(); i++)
            {
                double original = vals[i];

                vals[i] = original + jump;
                c->cartL2NormError(vals, v1);

                vals[i] = original;
                grad[i] = (v1[0] - result[0]) / (2.0 * jump);
            }
        }

        return result[0];
    }

    void constrainfuncm(uint m, double *result, uint n, const double *x, double *grad, void *data)
    {
        //Equality constraint auxilary function for Euclidean distance .
        //This also uses a small walk to approximate the gradient of the
        //constraint function at the current joint angles.

        NLOPT_IK *c = (NLOPT_IK *)data;

        std::vector<double> vals(n);

        for (uint i = 0; i < n; i++)
        {
            vals[i] = x[i];
        }

        double jump = boost::math::tools::epsilon<float>();

        c->cartSumSquaredError(vals, result);

        if (grad != NULL)
        {
            std::vector<double> v1(m);
            for (uint i = 0; i < n; i++)
            {
                double o = vals[i];
                vals[i] = o + jump;
                c->cartSumSquaredError(vals, v1.data());
                vals[i] = o;
                for (uint j = 0; j < m; j++)
                {
                    grad[j * n + i] = (v1[j] - result[j]) / (2 * jump);
                }
            }
        }
    }

    NLOPT_IK::NLOPT_IK(const KDL::Chain &_chain, const KDL::JntArray &_q_min, const KDL::JntArray &_q_max, double _maxtime, double _eps, OptType _type) : chain(_chain), fksolver(chain), maxtime(_maxtime), eps(std::abs(_eps)), TYPE(_type)
    {
        assert(chain.getNrOfJoints() == _q_min.data.size());
        assert(chain.getNrOfJoints() == _q_max.data.size());

        //Constructor for an IK Class.  Takes in a Chain to operate on,
        //the min and max joint limits, an (optional) maximum number of
        //iterations, and an (optional) desired error.
        reset();

        if (chain.getNrOfJoints() < 2)
        {
            std::cout << "WARNING :"
                      << "NLOpt_IK can only be run for chains of length 2 or more";
            return;
        }
        opt = nlopt::opt(nlopt::LD_SLSQP, _chain.getNrOfJoints());

        for (uint i = 0; i < chain.getNrOfJoints(); i++)
        {
            lb.push_back(_q_min(i));
            ub.push_back(_q_max(i));
        }

        for (uint i = 0; i < chain.segments.size(); i++)
        {
            std::string type = chain.segments[i].getJoint().getTypeName();
            if (type.find("Rot") != std::string::npos)
            {
                if (_q_max(types.size()) >= std::numeric_limits<float>::max() &&
                    _q_min(types.size()) <= std::numeric_limits<float>::lowest())
                    types.push_back(KDL::BasicJointType::Continuous);
                else
                    types.push_back(KDL::BasicJointType::RotJoint);
            }
            else if (type.find("Trans") != std::string::npos)
                types.push_back(KDL::BasicJointType::TransJoint);
        }

        assert(types.size() == lb.size());

        std::vector<double> tolerance(1, boost::math::tools::epsilon<float>());
        opt.set_xtol_abs(tolerance[0]);

        switch (TYPE)
        {
        case Joint:
            opt.set_min_objective(minfunc, this);
            opt.add_equality_mconstraint(constrainfuncm, this, tolerance);
            break;
        case DualQuat:
            opt.set_min_objective(minfuncDQ, this);
            break;
        case SumSq:
            opt.set_min_objective(minfuncSumSquared, this);
            break;
        case L2:
            opt.set_min_objective(minfuncL2, this);
            break;
        }
    }

    double NLOPT_IK::minJoints(const std::vector<double> &x, std::vector<double> &grad)
    {
        // Actual function to compute the error between the current joint
        // configuration and the desired.  The SSE is easy to provide a
        // closed form gradient for.

        bool gradient = !grad.empty();

        double err = 0;
        for (uint i = 0; i < x.size(); i++)
        {
            err += pow(x[i] - des[i], 2);
            if (gradient)
                grad[i] = 2.0 * (x[i] - des[i]);
        }

        return err;
    }

    void NLOPT_IK::cartSumSquaredError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        error[0] = KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot);

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    void NLOPT_IK::cartL2NormError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        error[0] = std::sqrt(KDL::dot(delta_twist.vel, delta_twist.vel) + KDL::dot(delta_twist.rot, delta_twist.rot));

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    void NLOPT_IK::cartDQError(const std::vector<double> &x, double error[])
    {
        // Actual function to compute Euclidean distance error.  This uses
        // the KDL Forward Kinematics solver to compute the Cartesian pose
        // of the current joint configuration and compares that to the
        // desired Cartesian pose for the IK solve.

        if (aborted || progress != -3)
        {
            opt.force_stop();
            return;
        }

        KDL::JntArray q(x.size());

        for (uint i = 0; i < x.size(); i++)
            q(i) = x[i];

        int rc = fksolver.JntToCart(q, currentPose);

        if (rc < 0)
            std::cout << "KDL FKSolver is failing: " << q.data;

        if (std::isnan(currentPose.p.x()))
        {
            std::cout << "ERROR :"
                      << "NaNs from NLOpt!!";
            error[0] = std::numeric_limits<float>::max();
            progress = -1;
            return;
        }

        KDL::Twist delta_twist = KDL::diffRelative(targetPose, currentPose);

        for (int i = 0; i < 6; i++)
        {
            if (std::abs(delta_twist[i]) <= std::abs(bounds[i]))
                delta_twist[i] = 0.0;
        }

        math3d::matrix3x3<double> currentRotationMatrix(currentPose.M.data);
        math3d::quaternion<double> currentQuaternion = math3d::rot_matrix_to_quaternion<double>(currentRotationMatrix);
        math3d::point3d currentTranslation(currentPose.p.data);
        dual_quaternion currentDQ = dual_quaternion::rigid_transformation(currentQuaternion, currentTranslation);

        dual_quaternion errorDQ = (currentDQ * !targetDQ).normalize();
        errorDQ.log();
        error[0] = 4.0f * dot(errorDQ, errorDQ);

        if (KDL::Equal(delta_twist, KDL::Twist::Zero(), eps))
        {
            progress = 1;
            best_x = x;
            return;
        }
    }

    int NLOPT_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist _bounds, const KDL::JntArray &q_desired)
    {
        // User command to start an IK solve.  Takes in a seed
        // configuration, a Cartesian pose, and (optional) a desired
        // configuration.  If the desired is not provided, the seed is
        // used.  Outputs the joint configuration found that solves the
        // IK.

        // Returns -3 if a configuration could not be found within the eps
        // set up in the constructor.

        boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time();
        boost::posix_time::time_duration diff;

        bounds = _bounds;
        q_out = q_init;

        if (chain.getNrOfJoints() < 2)
        {
            std::cout << "ERROR ："
                      << "NLOpt_IK can only be run for chains of length 2 or more";
            return -3;
        }

        if (q_init.data.size() != types.size())
        {
            printf(" ERROR : IK seeded with wrong number of joints.  Expected %d but got %d", (int)types.size(), (int)q_init.data.size());
            return -3;
        }

        opt.set_maxtime(maxtime);

        double minf; /* the minimum objective value, upon return */

        targetPose = p_in;

        if (TYPE == 1) // DQ
        {
            math3d::matrix3x3<double> targetRotationMatrix(targetPose.M.data);
            math3d::quaternion<double> targetQuaternion = math3d::rot_matrix_to_quaternion<double>(targetRotationMatrix);
            math3d::point3d targetTranslation(targetPose.p.data);
            targetDQ = dual_quaternion::rigid_transformation(targetQuaternion, targetTranslation);
        }
        // else if (TYPE == 1)
        // {
        //   z_target = targetPose*z_up;
        //   x_target = targetPose*x_out;
        //   y_target = targetPose*y_out;
        // }

        //    fksolver.JntToCart(q_init,currentPose);

        std::vector<double> x(chain.getNrOfJoints());

        for (uint i = 0; i < x.size(); i++)
        {
            x[i] = q_init(i);

            if (types[i] == KDL::BasicJointType::Continuous)
                continue;

            if (types[i] == KDL::BasicJointType::TransJoint)
            {
                x[i] = std::min(x[i], ub[i]);
                x[i] = std::max(x[i], lb[i]);
            }
            else
            {

                // Below is to handle bad seeds outside of limits

                if (x[i] > ub[i])
                {
                    //Find actual angle offset
                    double diffangle = fmod(x[i] - ub[i], 2 * M_PI);
                    // Add that to upper bound and go back a full rotation
                    x[i] = ub[i] + diffangle - 2 * M_PI;
                }

                if (x[i] < lb[i])
                {
                    //Find actual angle offset
                    double diffangle = fmod(lb[i] - x[i], 2 * M_PI);
                    // Subtract that from lower bound and go forward a full rotation
                    x[i] = lb[i] - diffangle + 2 * M_PI;
                }

                if (x[i] > ub[i])
                    x[i] = (ub[i] + lb[i]) / 2.0;
            }
        }

        best_x = x;
        progress = -3;

        std::vector<double> artificial_lower_limits(lb.size());

        for (uint i = 0; i < lb.size(); i++)
            if (types[i] == KDL::BasicJointType::Continuous)
                artificial_lower_limits[i] = best_x[i] - 2 * M_PI;
            else if (types[i] == KDL::BasicJointType::TransJoint)
                artificial_lower_limits[i] = lb[i];
            else
                artificial_lower_limits[i] = std::max(lb[i], best_x[i] - 2 * M_PI);

        opt.set_lower_bounds(artificial_lower_limits);

        std::vector<double> artificial_upper_limits(lb.size());

        for (uint i = 0; i < ub.size(); i++)
            if (types[i] == KDL::BasicJointType::Continuous)
                artificial_upper_limits[i] = best_x[i] + 2 * M_PI;
            else if (types[i] == KDL::BasicJointType::TransJoint)
                artificial_upper_limits[i] = ub[i];
            else
                artificial_upper_limits[i] = std::min(ub[i], best_x[i] + 2 * M_PI);

        opt.set_upper_bounds(artificial_upper_limits);

        if (q_desired.data.size() == 0)
        {
            des = x;
        }
        else
        {
            des.resize(x.size());
            for (uint i = 0; i < des.size(); i++)
                des[i] = q_desired(i);
        }

        try
        {
            opt.optimize(x, minf);
        }
        catch (...)
        {
        }

        if (progress == -1) // Got NaNs
            progress = -3;

        if (!aborted && progress < 0)
        {

            double time_left;
            diff = boost::posix_time::microsec_clock::local_time() - start_time;
            time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;

            while (time_left > 0 && !aborted && progress < 0)
            {

                for (uint i = 0; i < x.size(); i++)
                    x[i] = fRand(artificial_lower_limits[i], artificial_upper_limits[i]);

                opt.set_maxtime(time_left);

                try
                {
                    opt.optimize(x, minf);
                }
                catch (...)
                {
                }

                if (progress == -1) // Got NaNs
                    progress = -3;

                diff = boost::posix_time::microsec_clock::local_time() - start_time;
                time_left = maxtime - diff.total_nanoseconds() / 1000000000.0;
            }
        }

        for (uint i = 0; i < x.size(); i++)
        {
            q_out(i) = best_x[i];
        }

        return progress;
    }

}


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __LMA_OPT2_TRAIT_USE_ESTIMATOR_HPP__
#define __LMA_OPT2_TRAIT_USE_ESTIMATOR_HPP__

#include <libv/lma/version.hpp>

#include <libv/lma/lm/container/container.hpp>
#include <boost/type_traits/is_convertible.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/bool.hpp>
#include <libv/lma/numeric/mediane.hpp>
#include <iostream>

namespace lma
{
  struct MEstimator_{};

  template<class Base> struct MEstimator : MEstimator_
  {
    Base const& cast() const {return static_cast<Base const &>(*this); }

    template<class Float, size_t N>
    Eigen::Matrix<Float,N,1> me(const std::array<Float,N>& res, Float C) const
    {
      Eigen::Matrix<Float,N,1> ret;
      for(size_t i = 0 ; i < N ; ++i)
        ret[i] = cast().weight(res[i],C);
      return ret;
    }

    template<class Float>
    Eigen::Matrix<Float,1,1> me(const Float& res,const Float& C) const
    {
      Eigen::Matrix<Float,1,1> ret;
      ret << cast().weight(res,C);
      return ret;
    }

    template<class Float, int N>
    Eigen::Matrix<Float,N,1> me(const Eigen::Matrix<Float,N,1>& res, Float C) const
    {
      Eigen::Matrix<Float,N,1> ret;
      for(int i = 0 ; i < N ; ++i)
        ret[i] = cast().weight(res[i],C);
      return ret;
    }
  };

  namespace detail
  {
    template<class F> struct IsMEstimator : boost::is_convertible<F*,MEstimator_*>::type {};

    template<class W> struct Weight_J
    {
      const W& weight;
      Weight_J(const W& w_):weight(w_){}

      template<class Pair> void operator()(Pair& pair) const
      {
        for(int i = 0 ; i < Rows<decltype(pair.second)>::value ; ++i)
          for(int j = 0 ; j < Cols<decltype(pair.second)>::value ; ++j)
            pair.second(i,j) = pair.second(i,j)*weight[i];
      }
    };

    template< class F, class Jacobs, class Erreurs, class Mad>
    void apply_mestimator(const F& f, Jacobs& jacobs, Erreurs& erreur, const Mad& mad, typename boost::enable_if<detail::IsMEstimator<F>>::type* =0)
    {
      auto weight  = f.me(erreur,boost::fusion::at_key<F>(mad));
//       std::cout << " erreur " << erreur[0] << ", " << weight <<  std::endl;
      cwise_product(erreur,weight,erreur);
      boost::fusion::for_each(jacobs,Weight_J<decltype(weight)>(weight));
    }

    template<class F, class Jacobs, class Erreurs, class Mad> void apply_mestimator(const F&, Jacobs&, Erreurs&, const Mad&,typename boost::disable_if<detail::IsMEstimator<F>>::type* =0) { }

    template<class F, class Erreurs, class Mad>
    void apply_mestimator_erreur(const F& f, Erreurs& erreur, const Mad& mad, typename boost::enable_if<detail::IsMEstimator<F>>::type* =0)
    {
      auto weight  = f.me(erreur,boost::fusion::at_key<F>(mad));
      cwise_product(erreur,weight,erreur);
    }

    template<class F, class Erreurs, class Mad> void apply_mestimator_erreur(const F&, Erreurs&, const Mad&, typename boost::disable_if<detail::IsMEstimator<F>>::type* =0) { }

  }

  template<class Float>
  struct GermanMcClure : MEstimator<GermanMcClure<Float>>
  {
    Float coeff_mad;
    GermanMcClure(Float a_):coeff_mad(a_){}

    Float weight(const Float& res, const Float& C) const
    {
      return (C != 0 ? (C / (res*res+C*C)) : 1.0);
    }

    Float compute(std::vector<Float> norms) const
    {
      if (norms.empty())
        return 0;

      Float med = mediane(norms);

      for(Float& m : norms)
        m = std::abs(m-med);

      Float MAD = mediane(norms);

      Float C = med + coeff_mad * MAD;
      // std::cout << " C : " << C << " = " << med << " + " << coeff_mad << " * " << MAD << std::endl;
      return C;
    }
  };
}

#endif