// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_MOTIFS_HH
#define GRAPH_MOTIFS_HH

#include <boost/graph/isomorphism.hpp>
#include <boost/functional/hash.hpp>
#include <algorithm>
#include <vector>

#include "random.hh"
#include "hash_map_wrap.hh"

namespace graph_tool
{
namespace mpl = boost::mpl;

template <class Value>
void insert_sorted(std::vector<Value>& v, const Value& val)
{
    auto iter = lower_bound(v.begin(), v.end(), val);
    if (iter != v.end() && *iter == val)
        return; // no repetitions
    v.insert(iter, val);
}

template <class Value>
bool has_val(std::vector<Value>& v, const Value& val)
{
    auto iter = lower_bound(v.begin(), v.end(), val);
    if (iter == v.end())
        return false;
    return *iter == val;
}

// gets all the subgraphs starting from vertex v and store it in subgraphs.
template <class Graph, class Sampler>
void get_subgraphs(Graph& g, typename boost::graph_traits<Graph>::vertex_descriptor v,
                   size_t n,
                   std::vector<std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> >& subgraphs,
                   Sampler sampler)
{
    typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_t;

    // extension and subgraph stack
    std::vector<std::vector<vertex_t>> ext_stack(1);
    std::vector<std::vector<vertex_t>> sub_stack(1);
    std::vector<std::vector<vertex_t>> sub_neighbors_stack(1);

    sub_stack[0].push_back(v);
    for (auto e : out_edges_range(v, g))
    {
        typename boost::graph_traits<Graph>::vertex_descriptor u = target(e, g);
        if (u > v && !has_val(ext_stack[0], u))
        {
            insert_sorted(ext_stack[0], u);
            insert_sorted(sub_neighbors_stack[0],u);
        }
    }

    while (!sub_stack.empty())
    {
        std::vector<vertex_t>& ext = ext_stack.back();
        std::vector<vertex_t>& sub = sub_stack.back();
        std::vector<vertex_t>& sub_neighbors = sub_neighbors_stack.back();

        if (sub.size() == n)
        {
            // found a subgraph of the desired size; put it in the list and go
            // back a level
            subgraphs.push_back(sub);
            sub_stack.pop_back();
            ext_stack.pop_back();
            sub_neighbors_stack.pop_back();
            continue;
        }

        if (ext.empty())
        {
            // no where else to go
            ext_stack.pop_back();
            sub_stack.pop_back();
            sub_neighbors_stack.pop_back();
            continue;
        }
        else
        {
            // extend subgraph
            std::vector<vertex_t> new_ext, new_sub = sub,
                new_sub_neighbors = sub_neighbors;

            // remove w from ext
            vertex_t w = ext.back();
            ext.pop_back();

            // insert w in subgraph
            insert_sorted(new_sub, w);

            // update new_ext
            new_ext = ext;
            for (auto e : out_edges_range(w, g))
            {
                vertex_t u = target(e, g);
                if (u > v)
                {
                    if (!has_val(sub_neighbors, u))
                        insert_sorted(new_ext, u);
                    insert_sorted(new_sub_neighbors, u);
                }
            }

            sampler(new_ext, ext_stack.size());

            ext_stack.push_back(new_ext);
            sub_stack.push_back(new_sub);
            sub_neighbors_stack.push_back(new_sub_neighbors);
        }
    }
}

// sampling selectors

struct sample_all
{
    template <class val_type>
    void operator()(std::vector<val_type>&, size_t) {}
};

struct sample_some
{
    sample_some(std::vector<double>& p, rng_t& rng): _p(&p), _rng(&rng) {}
    sample_some() {}

    template <class val_type>
    void operator()(std::vector<val_type>& extend, size_t d)
    {
        typedef std::uniform_real_distribution<double> rdist_t;
        auto random = std::bind(rdist_t(), std::ref(*_rng));

        double pd = (*_p)[d+1];
        size_t nc = extend.size();
        double u = nc*pd - floor(nc*pd);
        size_t n;
        double r;
        #pragma omp critical (random)
        {
            r = random();
        }
        if (r < u)
            n = size_t(ceil(nc*pd));
        else
            n = size_t(floor(nc*pd));

        if (n == extend.size())
            return;
        if (n == 0)
        {
            extend.clear();
            return;
        }

        typedef std::uniform_int_distribution<size_t> idist_t;
        for (size_t i = 0; i < n; ++i)
        {
            auto random_v = std::bind(idist_t(0, extend.size()-i-1),
                                      std::ref(*_rng));
            size_t j;
            #pragma omp critical (random)
            {
                j = i + random_v();
            }
            std::swap(extend[i], extend[j]);
        }
        extend.resize(n);
    }

    std::vector<double>* _p;
    rng_t* _rng;
};

// build the actual induced subgraph from the vertex list
template <class Graph, class GraphSG>
void make_subgraph
    (std::vector<typename boost::graph_traits<Graph>::vertex_descriptor>& vlist,
     Graph& g, GraphSG& sub)
{
    for (size_t i = 0; i < vlist.size(); ++i)
        add_vertex(sub);
    for (size_t i = 0; i < vlist.size(); ++i)
    {
        typename boost::graph_traits<Graph>::vertex_descriptor ov = vlist[i], ot;
        typename boost::graph_traits<GraphSG>::vertex_descriptor nv = vertex(i,sub);
        for (auto e : out_edges_range(ov, g))
        {
            ot = target(e, g);
            auto viter = lower_bound(vlist.begin(), vlist.end(), ot);
            size_t ot_index = viter - vlist.begin();
            if (viter != vlist.end() && vlist[ot_index] == ot &&
                (graph_tool::is_directed(g) || ot < ov))
                add_edge(nv, vertex(ot_index, sub), sub);
        }
    }
}

// compare two graphs for labeled exactness (not isomorphism)
template <class Graph>
bool graph_cmp(Graph& g1, Graph& g2)
{
    if (num_vertices(g1) != num_vertices(g2) || num_edges(g1) != num_edges(g2))
        return false;

    typename boost::graph_traits<Graph>::vertex_iterator v1, v1_end;
    typename boost::graph_traits<Graph>::vertex_iterator v2, v2_end;
    std::tie(v2, v2_end) = vertices(g2);
    for (std::tie(v1, v1_end) = vertices(g1); v1 != v1_end; ++v1)
    {
        if (out_degree(*v1, g1) != out_degree(*v2, g2))
            return false;
        if (in_degreeS()(*v1, g1) != in_degreeS()(*v2, g2))
            return false;

        std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> out1, out2;
        typename boost::graph_traits<Graph>::out_edge_iterator e, e_end;
        for (std::tie(e, e_end) = out_edges(*v1, g1); e != e_end; ++e)
            out1.push_back(target(*e, g1));
        for (std::tie(e, e_end) = out_edges(*v2, g2); e != e_end; ++e)
            out2.push_back(target(*e, g2));
        std::sort(out1.begin(), out1.end());
        std::sort(out2.begin(), out2.end());
        if (out1 != out2)
            return false;
    }
    return true;
}

// short hand for subgraph types
typedef boost::adj_list<size_t> d_graph_t;

// we need this wrap to use the undirected_adaptor only on directed graphs
struct wrap_undirected
{
    template <class Graph>
    struct apply
    {
        typedef typename mpl::if_<typename is_directed_::apply<Graph>::type,
                                  boost::undirected_adaptor<Graph>,
                                  Graph&>::type type;
    };
};

struct wrap_directed
{
    template <class Graph, class Sub>
    struct apply
    {
        typedef typename mpl::if_<typename is_directed_::apply<Graph>::type,
                                  Sub&,
                                  boost::undirected_adaptor<Sub>>::type type;
    };
};

// get the signature of the graph: sorted degree sequence
template <class Graph>
void get_sig(Graph& g, std::vector<size_t>& sig)
{
    sig.clear();
    size_t N = num_vertices(g);
    if (N > 0)
        sig.resize(graph_tool::is_directed(g) ? 2 * N : N);
    for (size_t i = 0; i < N; ++i)
    {
        auto v = vertex(i, g);
        sig[i] = out_degree(v, g);
        if(graph_tool::is_directed(g))
            sig[i + N] = in_degreeS()(v, g);
    }
    sort(sig.begin(), sig.end());
}

// gets (or samples) all the subgraphs in graph g
struct get_all_motifs
{
    get_all_motifs(bool collect_vmaps, double p, bool comp_iso, bool fill_list,
                   rng_t& rng)
        : collect_vmaps(collect_vmaps), p(p),
          comp_iso(comp_iso), fill_list(fill_list), rng(rng) {}
    bool collect_vmaps;
    double p;
    bool comp_iso;
    bool fill_list;
    rng_t& rng;

    template <class Graph, class Sampler, class VMap>
    void operator()(Graph& g, size_t k, std::vector<d_graph_t>& subgraph_list,
                    std::vector<size_t>& hist, std::vector<std::vector<VMap> >& vmaps,
                    Sampler sampler) const
    {
        // this hashes subgraphs according to their signature
        gt_hash_map<std::vector<size_t>,
                    std::vector<std::pair<size_t, d_graph_t> >,
                    std::hash<std::vector<size_t>>> sub_list;
        std::vector<size_t> sig; // current signature

        for (size_t i = 0; i < subgraph_list.size(); ++i)
        {
            auto& sub = subgraph_list[i];
            typename wrap_directed::apply<Graph,d_graph_t>::type
                usub(sub);
            get_sig(usub, sig);
            sub_list[sig].emplace_back(i, sub);
        }

        // the subgraph count
        hist.resize(subgraph_list.size());

        typedef std::uniform_real_distribution<double> rdist_t;
        auto random = std::bind(rdist_t(), std::ref(rng));

        // the set of vertices V to be sampled (filled only if p < 1)
        std::vector<size_t> V;
        if (p < 1)
        {
            for (auto v : vertices_range(g))
                V.push_back(v);

            size_t n;
            if (random() < p)
                n = size_t(ceil(V.size()*p));
            else
                n = size_t(floor(V.size()*p));

            typedef std::uniform_int_distribution<size_t> idist_t;
            for (size_t i = 0; i < n; ++i)
            {
                auto random_v = std::bind(idist_t(0, V.size()-i-1),
                                          std::ref(rng));
                size_t j = i + random_v();
                std::swap(V[i], V[j]);
            }
            V.resize(n);
        }

        size_t N = (p < 1) ? V.size() : num_vertices(g);
        #pragma omp parallel for if (num_vertices(g) > OPENMP_MIN_THRESH) \
            private(sig)
        for (size_t i = 0; i < N; ++i)
        {
            std::vector<std::vector<typename boost::graph_traits<Graph>::vertex_descriptor> >
                subgraphs;
            typename boost::graph_traits<Graph>::vertex_descriptor v =
                (p < 1) ? V[i] : vertex(i, g);
            if (!is_valid_vertex(v, g))
                continue;

            typename wrap_undirected::apply<Graph>::type ug(g);
            get_subgraphs(ug, v, k, subgraphs, sampler);

            for (size_t j = 0; j < subgraphs.size(); ++j)
            {
                d_graph_t sub;
                typename wrap_directed::apply<Graph,d_graph_t>::type
                    usub(sub);
                make_subgraph(subgraphs[j], g, usub);
                get_sig(usub, sig);

                #pragma omp critical (gather)
                {
                    bool skip = false;
                    auto iter = sub_list.find(sig);
                    if(iter == sub_list.end())
                    {
                        if (!fill_list)
                            skip = true; // avoid inserting an element in sub_list
                        sub_list[sig].clear();
                    }

                    if (!skip)
                    {
                        bool found = false;
                        size_t pos;
                        auto sl = sub_list.find(sig);
                        if (sl != sub_list.end())
                        {
                            for (auto& mpos : sl->second)
                            {
                                d_graph_t& motif = mpos.second;
                                typename wrap_directed::apply<Graph,d_graph_t>::type
                                    umotif(motif);
                                if (comp_iso)
                                {
                                    if (isomorphism(umotif, usub,
                                                    vertex_index1_map(get(boost::vertex_index, umotif)).
                                                    vertex_index2_map(get(boost::vertex_index, usub))))
                                        found = true;
                                }
                                else
                                {
                                    if (graph_cmp(umotif, usub))
                                        found = true;
                                }
                                if (found)
                                {
                                    pos = mpos.first;
                                    hist[pos]++;
                                    break;
                                }
                            }
                        }

                        if (found == false && fill_list)
                        {
                            subgraph_list.push_back(sub);
                            sub_list[sig].emplace_back(subgraph_list.size() - 1,
                                                       sub);
                            hist.push_back(1);
                            pos = hist.size() - 1;
                            found = true;
                        }

                        if (found && collect_vmaps)
                        {
                            if (pos >= vmaps.size())
                                vmaps.resize(pos + 1);
                            vmaps[pos].push_back(VMap(get(boost::vertex_index,sub)));
                            for (size_t vi = 0; vi < num_vertices(sub); ++vi)
                                vmaps[pos].back()[vertex(vi, sub)] = subgraphs[j][vi];
                        }
                    }
                }
            }
        }
    }
};

} //graph-tool namespace

#endif // GRAPH_MOTIFS_HH


#include "MeshCuboidPredictor.h"

#include "ICP.h"
#include "MeshCuboidParameters.h"
#include "Utilities.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <ANN/ANN.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

MeshCuboidPredictor::MeshCuboidPredictor(unsigned int _num_labels)
	: num_labels_(_num_labels)
{

}

void MeshCuboidPredictor::get_missing_label_indices(
	const std::list<LabelIndex> &_given_label_indices,
	std::list<LabelIndex> &_missing_label_indices) const
{
	_missing_label_indices.clear();
}

Real MeshCuboidPredictor::get_single_potential(
	const MeshCuboid *_cuboid,
	const MeshCuboidAttributes *_attributes,
	const MeshCuboidTransformation *_transformation,
	const LabelIndex _label_index)const
{
	assert(_cuboid);
	assert(_attributes);
	assert(_transformation);

	Real potential = 0.0;
	unsigned int num_sample_points = 0;

	// FIXME:
	// This should be done in pre-processing.
	//unsigned int num_confidence_tol_sample_point = 0;
	//for (std::vector<MeshSamplePoint *>::const_iterator sample_it = _cuboid->get_sample_points().begin();
	//	sample_it != _cuboid->get_sample_points().end(); ++sample_it)
	//{
	//	assert(_label_index < (*sample_it)->label_index_confidence_.size());
	//	if ((*sample_it)->label_index_confidence_[_label_index] >= FLAGS_param_min_sample_point_confidence)
	//		++num_confidence_tol_sample_point;
	//}

	//if (num_confidence_tol_sample_point <
	//	FLAGS_param_min_num_confidence_tol_sample_points * _cuboid->get_sample_points().size())
	//{
	//	potential = FLAGS_param_max_potential;
	//}

	for (std::vector<MeshSamplePoint *>::const_iterator sample_it = _cuboid->get_sample_points().begin();
		sample_it != _cuboid->get_sample_points().end(); ++sample_it)
	{
		if (_label_index >= (*sample_it)->label_index_confidence_.size())
		{
			// Dummy label.
			return -std::log(FLAGS_param_null_cuboid_probability);
		}

		assert((*sample_it)->label_index_confidence_[_label_index] >= 0.0);
		assert((*sample_it)->label_index_confidence_[_label_index] <= 1.0);
		if ((*sample_it)->label_index_confidence_[_label_index] < 10E-12)
		{
			potential += 12;
		}
		else
		{
			potential -= std::log((*sample_it)->label_index_confidence_[_label_index]);
		}
		++num_sample_points;
	}

	if (num_sample_points == 0)
	{
		return -std::log(FLAGS_param_null_cuboid_probability);
	}
	else
	{
		potential /= num_sample_points;
	}

	return potential;
}

Real MeshCuboidPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2)const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	// Not implemented.
	Real potential = 0.0;
	return potential;
}

void MeshCuboidPredictor::get_single_quadratic_form(
	MeshCuboid *_cuboid, const unsigned int _cuboid_index,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, double& _constant_term) const
{
	assert(_cuboid);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	unsigned int num_sample_points = _cuboid->num_sample_points();
	unsigned int num_cuboid_surface_points = _cuboid->num_cuboid_surface_points();
	const unsigned int mat_size = _quadratic_term.cols();
	const unsigned int num_corners = MeshCuboid::k_num_corners;
	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;

	// X: sample points, Y: cuboid surface points.
	unsigned int num_X_points = num_sample_points;
	unsigned int num_Y_points = num_cuboid_surface_points;

	if (num_X_points == 0 || num_Y_points == 0)
		return;

	double sum_visibility = 0.0;
	for (int cuboid_surface_point_index = 0;
		cuboid_surface_point_index < num_cuboid_surface_points;
		++cuboid_surface_point_index)
	{
		sum_visibility += _cuboid->get_cuboid_surface_point(cuboid_surface_point_index)->visibility_;
	}

	if (sum_visibility == 0)
		return;

	Eigen::MatrixXd all_X_points(3, num_X_points + num_Y_points);
	Eigen::MatrixXd all_Y_points(3, num_X_points + num_Y_points);
	Eigen::MatrixXd all_Y_coeffs(num_corners, num_X_points + num_Y_points);
	Eigen::VectorXd all_weights(num_X_points + num_Y_points);

	unsigned int pair_index = 0;

	// Sample point -> Cuboid surface point.
	for (int sample_point_index = 0; sample_point_index < num_sample_points; ++sample_point_index)
	{
		int cuboid_surface_point_index =
			_cuboid->get_sample_to_cuboid_surface_correspondences(sample_point_index);
		assert(cuboid_surface_point_index >= 0);

		//
		Eigen::Vector3d X_point;
		Eigen::Vector3d Y_point;
		Eigen::VectorXd Y_coeff(MeshCuboid::k_num_corners);

		for (unsigned int i = 0; i < 3; ++i)
		{
			X_point(i) = _cuboid->get_sample_point(sample_point_index)->point_[i];
			Y_point(i) = _cuboid->get_cuboid_surface_point(cuboid_surface_point_index)->point_[i];
		}

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			Y_coeff(corner_index) = _cuboid->get_cuboid_surface_point(
			cuboid_surface_point_index)->corner_weights_[corner_index];

		all_X_points.col(pair_index) = X_point;
		all_Y_coeffs.col(pair_index) = Y_coeff;
		all_Y_points.col(pair_index) = Y_point;
		all_weights(pair_index) = 1.0 / num_sample_points;

		++pair_index;
		//
	}

	// Cuboid surface point -> Sample point.
	for (int cuboid_surface_point_index = 0;
		cuboid_surface_point_index < num_cuboid_surface_points;
		++cuboid_surface_point_index)
	{
		int sample_point_index =
			_cuboid->get_cuboid_surface_to_sample_correspondence(cuboid_surface_point_index);
		assert(sample_point_index >= 0);

		//
		Eigen::Vector3d X_point;
		Eigen::Vector3d Y_point;
		Eigen::VectorXd Y_coeff(MeshCuboid::k_num_corners);

		for (unsigned int i = 0; i < 3; ++i)
		{
			X_point(i) = _cuboid->get_sample_point(sample_point_index)->point_[i];
			Y_point(i) = _cuboid->get_cuboid_surface_point(cuboid_surface_point_index)->point_[i];
		}

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
			Y_coeff(corner_index) = _cuboid->get_cuboid_surface_point(
			cuboid_surface_point_index)->corner_weights_[corner_index];

		double visibility = _cuboid->get_cuboid_surface_point(cuboid_surface_point_index)->visibility_;

		all_X_points.col(pair_index) = X_point;
		all_Y_coeffs.col(pair_index) = Y_coeff;
		all_Y_points.col(pair_index) = Y_point;
		all_weights(pair_index) = visibility / sum_visibility;

		++pair_index;
		//
	}

	assert(pair_index == num_X_points + num_Y_points);

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index * MeshCuboidAttributes::k_num_attributes <= mat_size);

	MeshCuboidAttributes attribute;
	attribute.compute_attributes(_cuboid);
	Eigen::VectorXd x = attribute.get_attributes();

	for (unsigned int point_index = 0; point_index < num_X_points + num_Y_points; ++point_index)
	{
		Eigen::Vector3d X_point = all_X_points.col(point_index);
		Eigen::VectorXd Y_coeff = all_Y_coeffs.col(point_index);
		Eigen::VectorXd Y_point = all_Y_points.col(point_index);
		assert(Y_coeff.size() == MeshCuboid::k_num_corners);
		double weight = all_weights(point_index);

		Eigen::MatrixXd A0(3, num_attributes);
		A0.setZero();

		for (unsigned int corner_index = 0; corner_index < MeshCuboid::k_num_corners; ++corner_index)
		{
			for (unsigned int i = 0; i < 3; ++i)
				A0.col(MeshCuboidAttributes::k_corner_index + 3 * corner_index + i)(i) =
				Y_coeff(corner_index);
		}

#ifdef DEBUG_TEST
		Real error = std::abs(((A0 * x) - Y_point).norm());
		CHECK_NUMERICAL_ERROR(__FUNCTION__, error);
#endif

		Eigen::Vector3d b = -X_point;

		Eigen::MatrixXd A = Eigen::MatrixXd::Zero(3, mat_size);
		unsigned int start_index = (_cuboid_index * MeshCuboidAttributes::k_num_attributes);
		A.block<3, MeshCuboidAttributes::k_num_attributes>(0, start_index) = A0;

		_quadratic_term = _quadratic_term + weight * (A.transpose() * A);
		_linear_term = _linear_term + weight * (A.transpose() * b);
		_constant_term = _constant_term + weight * (b.transpose() * b)(0);
	}
}

Real MeshCuboidPredictor::get_pair_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, double& _constant_term)const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	return 0;
}

Real MeshCuboidPredictor::get_pair_conditional_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, double& _constant_term)const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	return 0;
}

MeshCuboidJointNormalRelationPredictor::MeshCuboidJointNormalRelationPredictor(
	const std::vector< std::vector<MeshCuboidJointNormalRelations *> > &_relations)
	: MeshCuboidPredictor(_relations.size())
	, relations_(_relations)
{
	for (unsigned int label_index = 0; label_index < num_labels_; ++label_index)
		assert(relations_[label_index].size() == num_labels_);
}

void MeshCuboidJointNormalRelationPredictor::get_missing_label_indices(
	const std::list<LabelIndex> &_given_label_indices,
	std::list<LabelIndex> &_missing_label_indices)const
{
	_missing_label_indices.clear();
	unsigned int num_labels = relations_.size();

	bool *is_missing_label = new bool[num_labels];
	memset(is_missing_label, true, num_labels * sizeof(bool));

	for (std::list<LabelIndex>::const_iterator it = _given_label_indices.begin();
		it != _given_label_indices.end(); ++it)
	{
		LabelIndex label_index = (*it);
		assert(label_index < num_labels);
		is_missing_label[label_index] = false;
	}

	for (LabelIndex label_index_1 = 0; label_index_1 < num_labels; ++label_index_1)
	{
		if (is_missing_label[label_index_1])
		{
			bool is_label_confliced = false;

			for (LabelIndex label_index_2 = 0; label_index_2 < num_labels; ++label_index_2)
			{
				if (!is_missing_label[label_index_2] && !relations_[label_index_1][label_index_2])
				{
					is_label_confliced = true;
					break;
				}
			}

			if (!is_label_confliced)
			{
				_missing_label_indices.push_back(label_index_1);
			}
		}
	}

	delete[] is_missing_label;
}

Real MeshCuboidJointNormalRelationPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	Real potential = FLAGS_param_max_potential;

	//const MeshCuboidJointNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	//const MeshCuboidJointNormalRelations *relation_21 = relations_[_label_index_2][_label_index_1];

	//if (relation_12 && relation_21)
	//{
	//	Real potential_12 = relation_12->compute_error(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2);
	//	Real potential_21 = relation_21->compute_error(_cuboid_2, _cuboid_1, _transformation_2, _transformation_1);
	//	potential = potential_12 + potential_21;
	//}

	const MeshCuboidJointNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	if (relation_12)
	{
		Real potential_12 = relation_12->compute_error(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2);
		potential = potential_12;

		// Using bilateral relations.
		// (A, B) and (B, A) pairs are the same.
#ifdef DEBUG_TEST
		const MeshCuboidJointNormalRelations *relation_21 = relations_[_label_index_2][_label_index_1];
		if (relation_21)
		{
			Real potential_21 = relation_12->compute_error(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2);
			CHECK_NUMERICAL_ERROR(__FUNCTION__, potential_12, potential_21);
		}
	}
#endif

	return potential;
}

Real MeshCuboidJointNormalRelationPredictor::get_pair_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, Real& _constant_term) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	const MeshCuboidJointNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	if (!relation_12) return 0.0;

	MeshCuboidFeatures features_1, features_2;
	Eigen::MatrixXd attributes_to_features_map_1, attributes_to_features_map_2;

	features_1.compute_features(_cuboid_1, &attributes_to_features_map_1);
	features_2.compute_features(_cuboid_2, &attributes_to_features_map_2);

	assert(attributes_to_features_map_1.rows() == num_features);
	assert(attributes_to_features_map_2.rows() == num_features);

	assert(attributes_to_features_map_1.cols() == num_attributes);
	assert(attributes_to_features_map_2.cols() == num_attributes);

	MeshCuboidTransformation transformation_1;
	transformation_1.compute_transformation(_cuboid_1);
	Eigen::MatrixXd rotation_1;
	Eigen::MatrixXd translation_1;
	transformation_1.get_linear_map_transformation(rotation_1, translation_1);

	MeshCuboidTransformation transformation_2;
	transformation_2.compute_transformation(_cuboid_2);
	Eigen::MatrixXd rotation_2;
	Eigen::MatrixXd translation_2;
	transformation_2.get_linear_map_transformation(rotation_2, translation_2);

	// (Ax + b)'C(Ax + b) = x'(A'CA)x + 2*(b'CA)x.
	unsigned int start_index_1 = (_cuboid_index_1 * num_attributes);
	unsigned int start_index_2 = (_cuboid_index_2 * num_attributes);

	Eigen::MatrixXd A1_orig = Eigen::MatrixXd::Zero(2 * num_features, mat_size);
	Eigen::MatrixXd A2_orig = Eigen::MatrixXd::Zero(2 * num_features, mat_size);

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(0, start_index_1)
		= A1_orig.block<num_features, num_attributes>(0, start_index_1)
		+ rotation_1 * attributes_to_features_map_1;

	// The translation is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(0, start_index_1)
		= A1_orig.block<num_features, num_attributes>(0, start_index_1)
		+ translation_1;

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_2' attributes.
	A1_orig.block<num_features, num_attributes>(num_features, start_index_2)
		= A1_orig.block<num_features, num_attributes>(num_features, start_index_2)
		+ rotation_1 * attributes_to_features_map_2;

	// The translation is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(num_features, start_index_1)
		= A1_orig.block<num_features, num_attributes>(num_features, start_index_1)
		+ translation_1;

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_2' attributes.
	A2_orig.block<num_features, num_attributes>(0, start_index_2)
		= A2_orig.block<num_features, num_attributes>(0, start_index_2)
		+ rotation_2 * attributes_to_features_map_2;

	// The translation is a function of 'cuboid_2' attributes.
	A2_orig.block<num_features, num_attributes>(0, start_index_2)
		= A2_orig.block<num_features, num_attributes>(0, start_index_2)
		+ translation_2;

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_1' attributes.
	A2_orig.block<num_features, num_attributes>(num_features, start_index_1)
		= A2_orig.block<num_features, num_attributes>(num_features, start_index_1)
		+ rotation_2 * attributes_to_features_map_1;

	// The translation is a function of 'cuboid_2' attributes.
	A2_orig.block<num_features, num_attributes>(num_features, start_index_2)
		= A2_orig.block<num_features, num_attributes>(num_features, start_index_2)
		+ translation_2;

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	const unsigned int num_rows = MeshCuboidJointNormalRelations::k_mat_size;
	Eigen::MatrixXd A(num_rows, mat_size);
	A.topRows(2 * num_features - MeshCuboidFeatures::k_corner_index)
		= A1_orig.bottomRows(2 * num_features - MeshCuboidFeatures::k_corner_index);
	A.bottomRows(2 * num_features - MeshCuboidFeatures::k_corner_index)
		= A2_orig.bottomRows(2 * num_features - MeshCuboidFeatures::k_corner_index);

	Eigen::VectorXd b = -relation_12->get_mean();
	assert(b.rows() == MeshCuboidJointNormalRelations::k_mat_size);

	Eigen::MatrixXd C = relation_12->get_inv_cov();
	assert(C.rows() == MeshCuboidJointNormalRelations::k_mat_size);
	assert(C.cols() == MeshCuboidJointNormalRelations::k_mat_size);

	_quadratic_term = A.transpose() * C * A;
	_linear_term = (b.transpose() * C * A).transpose();
	_constant_term = (b.transpose() * C * b);

	MeshCuboidAttributes attributes_1, attributes_2;
	attributes_1.compute_attributes(_cuboid_1);
	attributes_2.compute_attributes(_cuboid_2);

	Eigen::VectorXd x1 = attributes_1.get_attributes();
	Eigen::VectorXd x2 = attributes_2.get_attributes();
	Eigen::VectorXd x = Eigen::VectorXd::Zero(mat_size);
	x.block<num_attributes, 1>(start_index_1, 0) = x1;
	x.block<num_attributes, 1>(start_index_2, 0) = x2;

	Real potential = 0;
	potential += (x.transpose() * _quadratic_term * x);
	potential += (2 * _linear_term.transpose() * x);
	potential += _constant_term;

#ifdef DEBUG_TEST
	Real same_potential = relation_12->compute_error(_cuboid_1, _cuboid_2, &transformation_1, &transformation_2);
	CHECK_NUMERICAL_ERROR(__FUNCTION__, potential, same_potential);
#endif

	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//std::stringstream filename_sstr;
	//filename_sstr << std::string("quadratic_mat_")
	//	<< _cuboid_index_1 << std::string("_")
	//	<< _cuboid_index_2 << std::string(".csv");

	//std::ofstream csv_file(filename_sstr.str());
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//csv_file << _quadratic_term.format(csv_format) << std::endl;
	//csv_file.close();

	return potential;
}

Real MeshCuboidJointNormalRelationPredictor::get_pair_conditional_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, Real& _constant_term) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	const MeshCuboidJointNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	if (!relation_12) return 0.0;

	MeshCuboidFeatures features_1, features_2;
	Eigen::MatrixXd attributes_to_features_map_1, attributes_to_features_map_2;

	features_1.compute_features(_cuboid_1, &attributes_to_features_map_1);
	features_2.compute_features(_cuboid_2, &attributes_to_features_map_2);

	assert(attributes_to_features_map_1.rows() == num_features);
	assert(attributes_to_features_map_2.rows() == num_features);

	assert(attributes_to_features_map_1.cols() == num_attributes);
	assert(attributes_to_features_map_2.cols() == num_attributes);

	MeshCuboidTransformation transformation_1;
	transformation_1.compute_transformation(_cuboid_1);
	Eigen::MatrixXd rotation_1;
	Eigen::MatrixXd translation_1;
	transformation_1.get_linear_map_transformation(rotation_1, translation_1);

	// (Ax + b)'C(Ax + b) = x'(A'CA)x + 2*(b'CA)x.
	unsigned int start_index_1 = (_cuboid_index_1 * num_attributes);
	unsigned int start_index_2 = (_cuboid_index_2 * num_attributes);

	Eigen::MatrixXd A1_orig = Eigen::MatrixXd::Zero(2 * num_features, mat_size);

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(0, start_index_1)
		= A1_orig.block<num_features, num_attributes>(0, start_index_1)
		+ rotation_1 * attributes_to_features_map_1;

	// The translation is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(0, start_index_1)
		= A1_orig.block<num_features, num_attributes>(0, start_index_1)
		+ translation_1;

	// The rotation is fixed, and attribute-to-feature map is a function of 'cuboid_2' attributes.
	A1_orig.block<num_features, num_attributes>(num_features, start_index_2)
		= A1_orig.block<num_features, num_attributes>(num_features, start_index_2)
		+ rotation_1 * attributes_to_features_map_2;

	// The translation is a function of 'cuboid_1' attributes.
	A1_orig.block<num_features, num_attributes>(num_features, start_index_1)
		= A1_orig.block<num_features, num_attributes>(num_features, start_index_1)
		+ translation_1;

	// NOTE:
	// 'A2_orig' is not computed since here it is assumed that
	// the local coordinates of 'cuboid_2' is unknown.
	// See function 'MeshCuboidJointNormalRelationPredictor::get_pair_quadratic_form()'

	// NOTE:
	// Since the center point is always the origin in the local coordinates,
	// it is not used as the feature values.
	const unsigned int num_rows =
		2 * MeshCuboidFeatures::k_num_features - MeshCuboidFeatures::k_corner_index;
	Eigen::MatrixXd A = A1_orig.bottomRows(num_rows);

	// NOTE:
	// Since the local coordinates of 'cuboid_2' is unknown,
	// Features for only 1 -> 1 and 1 -> 2 are used.
	Eigen::VectorXd b = -relation_12->get_mean().segment(0, num_rows);

	// NOTE:
	// Since the local coordinates of 'cuboid_2' is unknown,
	// Features for only 1 -> 1 and 1 -> 2 are used.
	Eigen::MatrixXd C = relation_12->get_inv_cov().block(0, 0, num_rows, num_rows);

	_quadratic_term = A.transpose() * C * A;
	_linear_term = (b.transpose() * C * A).transpose();
	_constant_term = (b.transpose() * C * b);

	MeshCuboidAttributes attributes_1, attributes_2;
	attributes_1.compute_attributes(_cuboid_1);
	attributes_2.compute_attributes(_cuboid_2);

	Eigen::VectorXd x1 = attributes_1.get_attributes();
	Eigen::VectorXd x2 = attributes_2.get_attributes();
	Eigen::VectorXd x = Eigen::VectorXd::Zero(mat_size);
	x.block<num_attributes, 1>(start_index_1, 0) = x1;
	x.block<num_attributes, 1>(start_index_2, 0) = x2;

	Real potential = 0;
	potential += (x.transpose() * _quadratic_term * x);
	potential += (2 * _linear_term.transpose() * x);
	potential += _constant_term;

#ifdef DEBUG_TEST
	Real same_potential = relation_12->compute_conditional_error(_cuboid_1, _cuboid_2, &transformation_1);
	CHECK_NUMERICAL_ERROR(__FUNCTION__, potential, same_potential);
#endif

	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//std::stringstream filename_sstr;
	//filename_sstr << std::string("conditional_quadratic_mat_")
	//	<< _cuboid_index_1 << std::string("_")
	//	<< _cuboid_index_2 << std::string(".csv");

	//std::ofstream csv_file(filename_sstr.str());
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//csv_file << _quadratic_term.format(csv_format) << std::endl;
	//csv_file.close();

	return potential;
}

MeshCuboidCondNormalRelationPredictor::MeshCuboidCondNormalRelationPredictor(
	const std::vector< std::vector<MeshCuboidCondNormalRelations *> > &_relations)
	: MeshCuboidPredictor(_relations.size())
	, relations_(_relations)
{
	for (unsigned int label_index = 0; label_index < num_labels_; ++label_index)
		assert(relations_[label_index].size() == num_labels_);
}

void MeshCuboidCondNormalRelationPredictor::get_missing_label_indices(
	const std::list<LabelIndex> &_given_label_indices,
	std::list<LabelIndex> &_missing_label_indices)const
{
	_missing_label_indices.clear();
	unsigned int num_labels = relations_.size();

	bool *is_missing_label = new bool[num_labels];
	memset(is_missing_label, true, num_labels * sizeof(bool));

	for (std::list<LabelIndex>::const_iterator it = _given_label_indices.begin();
		it != _given_label_indices.end(); ++it)
	{
		LabelIndex label_index = (*it);
		assert(label_index < num_labels);
		is_missing_label[label_index] = false;
	}

	for (LabelIndex label_index_1 = 0; label_index_1 < num_labels; ++label_index_1)
	{
		if (is_missing_label[label_index_1])
		{
			bool is_label_confliced = false;

			for (LabelIndex label_index_2 = 0; label_index_2 < num_labels; ++label_index_2)
			{
				if (!is_missing_label[label_index_2] && !relations_[label_index_1][label_index_2])
				{
					is_label_confliced = true;
					break;
				}
			}

			if (!is_label_confliced)
			{
				_missing_label_indices.push_back(label_index_1);
			}
		}
	}

	delete[] is_missing_label;
}

Real MeshCuboidCondNormalRelationPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	Real potential = FLAGS_param_max_potential;

	const MeshCuboidCondNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	const MeshCuboidCondNormalRelations *relation_21 = relations_[_label_index_2][_label_index_1];

	if (relation_12 && relation_21)
	{
		Real potential_12 = relation_12->compute_error(_cuboid_1, _cuboid_2, _transformation_1, _transformation_2);
		Real potential_21 = relation_21->compute_error(_cuboid_2, _cuboid_1, _transformation_2, _transformation_1);
		potential = potential_12 + potential_21;
	}

	return potential;
}

Real MeshCuboidCondNormalRelationPredictor::get_pair_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, Real& _constant_term) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int num_global_coord_points = MeshCuboidFeatures::k_num_local_points;
	const unsigned int num_non_global_coord_features = num_features - 3 * num_global_coord_points;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	MeshCuboidFeatures features_1, features_2;
	Eigen::MatrixXd attributes_to_features_map_1, attributes_to_features_map_2;

	features_1.compute_features(_cuboid_1, &attributes_to_features_map_1);
	features_2.compute_features(_cuboid_2, &attributes_to_features_map_2);

	assert(attributes_to_features_map_1.rows() == num_features);
	assert(attributes_to_features_map_2.rows() == num_features);

	assert(attributes_to_features_map_1.cols() == num_attributes);
	assert(attributes_to_features_map_2.cols() == num_attributes);

	MeshCuboidTransformation transformation_1;
	transformation_1.compute_transformation(_cuboid_1);
	Eigen::MatrixXd rotation_1;
	Eigen::MatrixXd translation_1;
	transformation_1.get_linear_map_transformation(rotation_1, translation_1);

	MeshCuboidTransformation transformation_2;
	transformation_2.compute_transformation(_cuboid_2);

	const MeshCuboidCondNormalRelations *relation_12 = relations_[_label_index_1][_label_index_2];
	if (!relation_12) return 0.0;

	// (Ax + b)'C(Ax + b) = x'(A'CA)x + 2*(b'CA)x.
	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(num_features, mat_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(num_features);
	Eigen::MatrixXd C = Eigen::MatrixXd::Zero(num_features, num_features);

	unsigned int start_index_1 = (_cuboid_index_1 * num_attributes);
	unsigned int start_index_2 = (_cuboid_index_2 * num_attributes);

	Eigen::MatrixXd mean_A = Eigen::MatrixXd::Zero(num_features, num_features);
	mean_A.rightCols(num_non_global_coord_features) = relation_12->get_mean_A();

	A.block<num_features, num_attributes>(0, start_index_1)
		= A.block<num_features, num_attributes>(0, start_index_1)
		- mean_A * attributes_to_features_map_1;

	A.block<num_features, num_attributes>(0, start_index_2)
		= A.block<num_features, num_attributes>(0, start_index_2)
		+ rotation_1 * attributes_to_features_map_2;

	A.block<num_features, num_attributes>(0, start_index_1)
		= A.block<num_features, num_attributes>(0, start_index_1)
		+ translation_1;

	b = -relation_12->get_mean_b();

	C = relation_12->get_inv_cov();

	_quadratic_term = A.transpose() * C * A;
	_linear_term = (b.transpose() * C * A).transpose();
	_constant_term = (b.transpose() * C * b);

	MeshCuboidAttributes attributes_1, attributes_2;
	attributes_1.compute_attributes(_cuboid_1);
	attributes_2.compute_attributes(_cuboid_2);

	Eigen::VectorXd x1 = attributes_1.get_attributes();
	Eigen::VectorXd x2 = attributes_2.get_attributes();
	Eigen::VectorXd x = Eigen::VectorXd::Zero(mat_size);
	x.block<num_attributes, 1>(start_index_1, 0) = x1;
	x.block<num_attributes, 1>(start_index_2, 0) = x2;

	Real potential = 0;
	potential += (x.transpose() * _quadratic_term * x);
	potential += (2 * _linear_term.transpose() * x);
	potential += _constant_term;

#ifdef DEBUG_TEST
	Real same_potential = relation_12->compute_error(_cuboid_1, _cuboid_2, &transformation_1, &transformation_2);
	CHECK_NUMERICAL_ERROR(__FUNCTION__, potential, same_potential);
#endif

	//std::stringstream filename_sstr;
	//filename_sstr << std::string("quadratic_mat_")
	//	<< _cuboid_index_1 << std::string("_")
	//	<< _cuboid_index_2 << std::string(".csv");
	//std::ofstream csv_file(filename_sstr.str());
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//csv_file << quadratic_form.format(csv_format) << std::endl;
	//csv_file.close();

	return potential;
}

/*
MeshCuboidPCARelationPredictor::MeshCuboidPCARelationPredictor(
	const std::vector< std::vector<MeshCuboidPCARelations> > &_relations)
	: MeshCuboidPredictor(_relations.size())
	, relations_(_relations)
{
	for (unsigned int label_index = 0; label_index < num_labels_; ++label_index)
		assert(relations_[label_index].size() == num_labels_);
}

Real MeshCuboidPCARelationPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	Real potential = 0.0;

	MeshCuboidFeatures features_1, features_2;
	features_1.compute_features(_cuboid_1);
	features_2.compute_features(_cuboid_2);

	Eigen::VectorXd features_vec_1 = features_1.get_features();
	Eigen::VectorXd features_vec_2 = features_2.get_features();
	Eigen::VectorXd transformed_features_vec_1 = _transformation_2->get_transformed_features(_cuboid_1);
	Eigen::VectorXd transformed_features_vec_2 = _transformation_1->get_transformed_features(_cuboid_2);

	if (_label_index_1 < _label_index_2)
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_1][_label_index_2];
		potential += relation.compute_error(transformed_features_vec_1, transformed_features_vec_2);
	}
	else
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_2][_label_index_1];
		potential += relation.compute_error(transformed_features_vec_1, transformed_features_vec_2);
	}

	return potential;
}

Real MeshCuboidPCARelationPredictor::get_pair_quadratic_form(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const unsigned int _cuboid_index_1, const unsigned int _cuboid_index_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2,
	Eigen::MatrixXd &_quadratic_term, Eigen::VectorXd &_linear_term, Real& _constant_term) const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	const unsigned int num_attributes = MeshCuboidAttributes::k_num_attributes;
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int num_global_coord_points = MeshCuboidFeatures::k_num_local_points;
	const unsigned int mat_size = _quadratic_term.cols();

	assert(_quadratic_term.rows() == mat_size);
	assert(_linear_term.rows() == mat_size);
	assert(_cuboid_index_1 * num_attributes <= mat_size);
	assert(_cuboid_index_2 * num_attributes <= mat_size);

	_quadratic_term.setZero();
	_linear_term.setZero();
	_constant_term = 0;

	MeshCuboidFeatures features_1, features_2;
	Eigen::MatrixXd attributes_to_features_map_1, attributes_to_features_map_2;

	features_1.compute_features(_cuboid_1, &attributes_to_features_map_1);
	features_2.compute_features(_cuboid_2, &attributes_to_features_map_2);

	assert(attributes_to_features_map_1.rows() == num_features);
	assert(attributes_to_features_map_2.rows() == num_features);

	assert(attributes_to_features_map_1.cols() == num_attributes);
	assert(attributes_to_features_map_2.cols() == num_attributes);

	MeshCuboidTransformation transformation_1;
	transformation_1.compute_transformation(_cuboid_1);
	Eigen::MatrixXd rotation_1;
	Eigen::MatrixXd translation_1;
	transformation_1.get_feature_transformation(rotation_1, translation_1);

	MeshCuboidTransformation transformation_2;
	transformation_2.compute_transformation(_cuboid_2);
	Eigen::MatrixXd rotation_2;
	Eigen::MatrixXd translation_2;
	transformation_2.get_feature_transformation(rotation_2, translation_2);

	MeshCuboidAttributes attributes_1, attributes_2;
	attributes_1.compute_attributes(_cuboid_1);
	attributes_2.compute_attributes(_cuboid_2);

	// (Ax + b)'C(Ax + b) = x'(A'CA)x + 2*(b'CA)x.
	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(2 * num_features, mat_size);
	Eigen::VectorXd b = Eigen::VectorXd::Zero(2 * num_features);
	Eigen::MatrixXd C = Eigen::MatrixXd::Zero(2 * num_features, 2 * num_features);

	unsigned int start_index_1 = (_cuboid_index_1 * num_attributes);
	unsigned int start_index_2 = (_cuboid_index_2 * num_attributes);

	Eigen::VectorXd x1 = attributes_1.get_attributes();
	Eigen::VectorXd x2 = attributes_2.get_attributes();
	Eigen::VectorXd x0 = Eigen::VectorXd::Zero(mat_size);
	x0.block<num_attributes, 1>(start_index_1, 0) = x1;
	x0.block<num_attributes, 1>(start_index_2, 0) = x2;

	A.block<num_features, num_attributes>(0, start_index_1)
		= A.block<num_features, num_attributes>(0, start_index_1)
		+ rotation_2 * attributes_to_features_map_1;

	A.block<num_features, num_attributes>(0, start_index_2)
		= A.block<num_features, num_attributes>(0, start_index_2)
		+ translation_2;

	A.block<num_features, num_attributes>(num_features, start_index_2)
		= A.block<num_features, num_attributes>(num_features, start_index_2)
		+ rotation_1 * attributes_to_features_map_2;

	A.block<num_features, num_attributes>(num_features, start_index_1)
		= A.block<num_features, num_attributes>(num_features, start_index_1)
		+ translation_1;

	Eigen::MatrixXd P;
	if (_label_index_1 < _label_index_2)
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_1][_label_index_2];
		b = b - relation.get_mean();
		P = relation.get_pca_bases();
	}
	else
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_2][_label_index_1];
		b = b - relation.get_mean();
		P = relation.get_pca_bases();
	}

	Eigen::MatrixXd I = Eigen::MatrixXd::Identity(2 * num_features, 2 * num_features);

	_quadratic_term = _quadratic_term + A.transpose() * (I - P).transpose() * (I - P) * A;
	_linear_term = _linear_term + ((b.transpose() * (I - P).transpose() * (I - P) * A).transpose());
	_constant_term = _constant_term + (b.transpose() * (I - P).transpose() * (I - P) * b);

	_quadratic_term = _quadratic_term + A.transpose() * P * A;
	_linear_term = _linear_term - (A.transpose() * P * A * x0);
	_constant_term = _constant_term + (x0.transpose() * A.transpose() * P * A * x0);

	Real potential = 0;
	potential += (x0.transpose() * _quadratic_term * x0);
	potential += (2 * _linear_term.transpose() * x0);
	potential += _constant_term;

	// DEBUG.
	Eigen::VectorXd transformed_features_vec_1 = transformation_2.get_transformed_features(_cuboid_1);
	Eigen::VectorXd transformed_features_vec_2 = transformation_1.get_transformed_features(_cuboid_2);
	Real same_potential = 0.0;
	if (_label_index_1 < _label_index_2)
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_1][_label_index_2];
		same_potential = relation.compute_error(transformed_features_vec_1, transformed_features_vec_2);
	}
	else
	{
		const MeshCuboidPCARelations &relation = relations_[_label_index_2][_label_index_1];
		same_potential = relation.compute_error(transformed_features_vec_1, transformed_features_vec_2);
	}

	CHECK_ERROR(__FUNCTION__, potential, same_potential);

	// DEBUG.
	//std::stringstream filename_sstr;
	//filename_sstr << std::string("quadratic_mat_")
	//	<< _cuboid_index_1 << std::string("_")
	//	<< _cuboid_index_2 << std::string(".csv");
	//std::ofstream csv_file(filename_sstr.str());
	//Eigen::IOFormat csv_format(Eigen::StreamPrecision, 0, ",");
	//csv_file << quadratic_form.format(csv_format) << std::endl;
	//csv_file.close();

	return potential;
}

MeshCuboidCCARelationPredictor::MeshCuboidCCARelationPredictor(
	const std::vector< std::vector< std::vector<MeshCuboidCCARelations> > >& _relations)
	: MeshCuboidPredictor(_relations.size())
	, relations_(_relations)
{
	for (unsigned int label_index_1 = 0; label_index_1 < num_labels_; ++label_index_1)
	{
		assert(_relations[label_index_1].size() == num_labels_);
		for (unsigned int label_index_2 = 0; label_index_2 < num_labels_; ++label_index_2)
			assert(_relations[label_index_1][label_index_2].size() == num_labels_);
	}
}

Real MeshCuboidCCARelationPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2)const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	// NOTE:
	// Now considering only different label pairs.
	//assert(_label_index_1 != _label_index_2);

	Real potential = 0.0;

	// Compute potential.
	//MeshCuboidCCARelations *relation_12;
	//// NOTE:
	//// Use increasing order of label indices for cuboid pairs.
	//if (label_index_1 <= label_index_2)
	//relation_12 = &(_relations[label_index_1][label_index_2]);
	//else
	//relation_12 = &(_relations[label_index_2][label_index_1]);
	//assert(relation_12);
	//potential = relation_12->compute_error(&attributes_1, &attributes_2);

	// NOTE:
	const std::vector<MeshCuboidCCARelations> *relation_12;
	// Use increasing order of label indices for cuboid pairs.
	if (_label_index_1 <= _label_index_2)
		relation_12 = &(relations_[_label_index_1][_label_index_2]);
	else
		relation_12 = &(relations_[_label_index_2][_label_index_1]);

	Eigen::VectorXd transformed_features_11 = _transformation_1->get_transformed_features(_cuboid_1);
	Eigen::VectorXd transformed_features_21 = _transformation_1->get_transformed_features(_cuboid_2);
	potential += (*relation_12)[_label_index_1].compute_error(
		transformed_features_11, transformed_features_21);

	Eigen::VectorXd transformed_features_12 = _transformation_2->get_transformed_features(_cuboid_1);
	Eigen::VectorXd transformed_features_22 = _transformation_2->get_transformed_features(_cuboid_2);
	potential += (*relation_12)[_label_index_2].compute_error(
		transformed_features_12, transformed_features_22);

	return potential;
}

MeshCuboidManualRelationPredictor::MeshCuboidManualRelationPredictor(
	const std::vector<MeshCuboidStats> &_single_stats,
	const std::vector< std::vector<MeshCuboidStats> > &_pair_stats)
	: MeshCuboidPredictor(_single_stats.size())
	, single_stats_(_single_stats)
	, pair_stats_(_pair_stats)
{
	assert(_pair_stats.size() == num_labels_);
	for (unsigned int label_index = 0; label_index < num_labels_; ++label_index)
		assert(_pair_stats[label_index].size() == num_labels_);
}

Real MeshCuboidManualRelationPredictor::get_single_potential(
	const MeshCuboid *_cuboid,
	const MeshCuboidAttributes *_attributes,
	const MeshCuboidTransformation *_transformation,
	const LabelIndex _label_index)const
{
	assert(_cuboid);
	assert(_attributes);
	assert(_transformation);

	assert(_label_index < num_labels_);

	Real potential = 0.0;

	// Compute potential.
	//
	MeshSingleCuboidManualFeatures single_features("single_potential");
	single_features.compute_values(_cuboid);
	potential -= single_features.compute_log_probability(single_stats_[_label_index]);

	// NOTE:
	// Pairwise attribute relations are also defined for a single part attributes.
	MeshPairCuboidManualFeatures pair_features("pair_potential");
	pair_features.compute_values(_cuboid, _cuboid);
	potential -= pair_features.compute_log_probability(pair_stats_[_label_index][_label_index]);
	//

	return potential;
}

Real MeshCuboidManualRelationPredictor::get_pair_potential(
	const MeshCuboid *_cuboid_1, const MeshCuboid *_cuboid_2,
	const MeshCuboidAttributes *_attributes_1, const MeshCuboidAttributes *_attributes_2,
	const MeshCuboidTransformation *_transformation_1, const MeshCuboidTransformation *_transformation_2,
	const LabelIndex _label_index_1, const LabelIndex _label_index_2)const
{
	assert(_cuboid_1); assert(_cuboid_2);
	assert(_attributes_1); assert(_attributes_2);
	assert(_transformation_1); assert(_transformation_2);

	assert(_label_index_1 < num_labels_);
	assert(_label_index_2 < num_labels_);

	Real potential = 0.0;

	// Compute potential.
	//
	MeshPairCuboidManualFeatures pair_features("pair_potential");
	pair_features.compute_values(_cuboid_1, _cuboid_2);

	// NOTE:
	// Check symmetry:
	// pair_features.compute_values(label_2, cuboid_2, label_1, cuboid_1);

	// NOTE:
	// Use increasing order of label indices for cuboid pairs.
	if (_label_index_1 <= _label_index_2)
	{
		potential -= pair_features.compute_log_probability(
			pair_stats_[_label_index_1][_label_index_2]);
	}
	else
	{
		potential -= pair_features.compute_log_probability(
			pair_stats_[_label_index_2][_label_index_1]);
	}

	return potential;
}
*/


//////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2019, Brian Lee, Vinitha Ranganeni
// 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 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 INCLUDE_STATESPACE_SE2_HPP_
#define INCLUDE_STATESPACE_SE2_HPP_

#include <Eigen/Dense>
#include <vector>
#include <unordered_map>
#include <memory>
#include <utility>
#include <boost/functional/hash.hpp>
#include "StateSpace.hpp"

namespace libcozmo {
namespace statespace {

// This class implements a discretized two-dimensional Special Euclidean
// group SE(2), i.e. the space of planar rigid body transformations.
class SE2 : public virtual StateSpace {
 public:
    class State : public StateSpace::State {
     public:
        /// Constructs identity state
        State() : x(0), y(0), theta(0) {}

        ~State() = default;

        /// Constructs state with given parameters
        explicit State(const int& x, const int& y, const int& theta);

        /// Documentation Inherited
        /// Vector in format [x, y, theta]
        void from_vector(const Eigen::VectorXd& state);

        /// Documentation Inherited
        bool operator== (const StateSpace::State& state) const override;

        /// Custom state hash
        friend std::size_t hash_value(const State& state) {
            std::size_t seed = 0;
            boost::hash_combine(seed, boost::hash_value(state.x));
            boost::hash_combine(seed, boost::hash_value(state.y));
            boost::hash_combine(seed, boost::hash_value(state.theta));
            return seed;
        }

        int X() const;
        int Y() const;
        int Theta() const;

        /// Documentation Inherited
        Eigen::VectorXd vector() const override;

     private:
        int x;
        int y;
        int theta;

        friend class SE2;
    };

    /// Constructs a discretized SE2 state space
    ///
    /// \param resolution_m Resolution of the environment (mm)
    /// \param num_theta_vals Number of discretized theta values; Must be a
    /// power of 2
    SE2(
        const double& resolution_m,
        const int& num_theta_vals) : \
        m_resolution(resolution_m),
        m_num_theta_vals(num_theta_vals),
        m_statespace(std::make_shared<aikido::statespace::SE2>()),
        m_distance_metric(aikido::distance::SE2(m_statespace)) {}

    ~SE2();

    /// Documentation inherited
    int get_or_create_state(const StateSpace::State& _state) override;

    /// Documentation inherited
    int get_or_create_state(
        const aikido::statespace::StateSpace::State& _state) override;

    /// Documentation inherited
    /// Input vector in format [x, y, theta]
    int get_or_create_state(const Eigen::VectorXd& _state) override;

    /// Documentation inherited
    void discrete_state_to_continuous(
        const StateSpace::State& _state,
        aikido::statespace::StateSpace::State*
            _continuous_state) const override;

    /// Documentation inherited
    void continuous_state_to_discrete(
        const aikido::statespace::StateSpace::State& _state,
        StateSpace::State* _discrete_state) const override;

    /// Documentation inherited
    bool get_state_id(
        const StateSpace::State& _state, int* _state_id) const override;

    /// Documentation inherited
    StateSpace::State* get_state(const int& _state_id) const override;

    /// Documentation inherited
    /// State is valid if theta is in [0, num_theta_vals]
    bool is_valid_state(const StateSpace::State& _state) const override;

    /// Documentation inherited
    int size() const override;

    /// Documentation inherited
    double get_distance(
        const StateSpace::State& _state_1,
        const StateSpace::State& _state_2) const override;

    /// Documentation inherited
    double get_distance(
        const aikido::statespace::StateSpace::State& _state_1,
        const aikido::statespace::StateSpace::State& _state_2) const override;

    /// Documentation inherited
    void copy_state(
        const StateSpace::State& _source,
        StateSpace::State* _destination) const override;

    /// Documentation inherited
    double get_resolution() const override;

 private:
    /// Creates a new state and adds it to the statespace
    ///
    /// \return pointer to the state
    StateSpace::State* create_state() override;

    /// Gets normalized angle (radians) in [0, 2pi]
    ///
    /// \param theta_rad Angle (radians)
    /// \return normalized angle
    double normalize_angle_rad(const double& theta_rad) const;

    /// Converts discrete angle to continuous (radians)
    ///
    /// \param theta Discrete angle in [0, num_theta_vals]
    /// \return Continuous angle in [0, 2pi]
    double discrete_angle_to_continuous(const int& theta) const;

    /// Converts discrete angle to continuous (radians)
    ///
    /// \param theta Continuous angle (radians)
    /// \return Discrete angle in [0, num_theta_vals]
    int continuous_angle_to_discrete(const double& theta) const;

    /// Converts discrete position to continuous (x_mm, y_mm)
    ///
    /// \param position Discrete coordinates
    /// \return Continous coordinates in meters
    Eigen::Vector2d discrete_position_to_continuous(
        const Eigen::Vector2i& position) const;

    /// Converts continuous position (x_mm, y_mm) to discrete
    ///
    /// \param position Continuous coordinates in millimeters
    /// \return Discrete coordinates
    Eigen::Vector2i continuous_position_to_discrete(
        const Eigen::Vector2d& position) const;

    /// Maps discrete state (libcozmo::statespace::SE2::State) to state ID
    std::unordered_map<State, int, boost::hash<State>> m_state_to_id_map;

    /// Vector of discrete states (libcozmo::statespace::SE2::State)
    /// Index of state is the state ID
    std::vector<State*> m_state_map;

    /// Number of discretized theta values
    const int m_num_theta_vals;

    /// Resolution of environment (mm)
    const double m_resolution;

    std::shared_ptr<aikido::statespace::SE2> m_statespace;
    aikido::distance::SE2 m_distance_metric;
};

}  // namespace statespace
}  // namespace libcozmo

#endif  // INCLUDE_STATESPACE_SE2_HPP_


/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   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 "dart/optimizer/nlopt/NloptSolver.hpp"

#include <memory>

#include <Eigen/Dense>

#include "dart/common/Console.hpp"
#include "dart/common/StlHelpers.hpp"
#include "dart/optimizer/Function.hpp"
#include "dart/optimizer/Problem.hpp"

namespace dart {
namespace optimizer {

//==============================================================================
NloptSolver::NloptSolver(
    const Solver::Properties& properties, nlopt::algorithm alg)
  : NloptSolver(properties, convertAlgorithm(alg))
{
  // Do nothing
}

//==============================================================================
NloptSolver::NloptSolver(
    const Solver::Properties& properties, NloptSolver::Algorithm alg)
  : Solver(properties), mOpt(nullptr), mAlg(convertAlgorithm(alg)), mMinF(0.0)
{
  // Do nothing
}

//==============================================================================
NloptSolver::NloptSolver(std::shared_ptr<Problem> problem, nlopt::algorithm alg)
  : NloptSolver(std::move(problem), convertAlgorithm(alg))
{
  // Do nothing
}

//==============================================================================
NloptSolver::NloptSolver(
    std::shared_ptr<Problem> problem, NloptSolver::Algorithm alg)
  : Solver(std::move(problem)),
    mOpt(nullptr),
    mAlg(convertAlgorithm(alg)),
    mMinF(0.0)
{
  // Do nothing
}

//==============================================================================
NloptSolver::~NloptSolver()
{
  // Do nothing
}

//==============================================================================
static std::vector<double> convertToStd(const Eigen::VectorXd& v)
{
  return std::vector<double>(v.data(), v.data() + v.size());
}

//==============================================================================
static Eigen::VectorXd convertToEigen(const std::vector<double>& v)
{
  Eigen::VectorXd result(v.size());
  for (std::size_t i = 0; i < v.size(); ++i)
    result[static_cast<Eigen::Index>(i)] = v[i];

  return result;
}

//==============================================================================
bool NloptSolver::solve()
{
  // Allocate a new nlopt::opt structure if needed
  std::size_t dimension = mProperties.mProblem->getDimension();
  if (nullptr == mOpt || mOpt->get_dimension() != dimension
      || mOpt->get_algorithm() != mAlg)
  {
    mOpt = std::make_unique<nlopt::opt>(mAlg, dimension);
  }
  else
  {
    mOpt->remove_equality_constraints();
    mOpt->remove_inequality_constraints();
  }

  const std::shared_ptr<Problem>& problem = mProperties.mProblem;

  mOpt->set_maxeval(static_cast<int>(mProperties.mNumMaxIterations));
  mOpt->set_xtol_rel(mProperties.mTolerance);
  mOpt->set_lower_bounds(convertToStd(problem->getLowerBounds()));
  mOpt->set_upper_bounds(convertToStd(problem->getUpperBounds()));

  // Set up the nlopt::opt
  mOpt->set_min_objective(
      NloptSolver::_nlopt_func, problem->getObjective().get());

  for (std::size_t i = 0; i < problem->getNumEqConstraints(); ++i)
  {
    FunctionPtr fn = problem->getEqConstraint(i);
    try
    {
      mOpt->add_equality_constraint(
          NloptSolver::_nlopt_func, fn.get(), mProperties.mTolerance);
    }
    catch (const std::invalid_argument& e)
    {
      dterr << "[NloptSolver::solve] Encountered exception [" << e.what()
            << "] while adding an equality constraint to an Nlopt solver. "
            << "Check whether your algorithm [" << nlopt::algorithm_name(mAlg)
            << "] (" << mAlg << ") supports equality constraints!\n";
      assert(false);
    }
    catch (const std::exception& e)
    {
      dterr << "[NloptSolver::solve] Encountered exception [" << e.what()
            << "] while adding an equality constraint to the Nlopt solver. "
            << "This might be a bug in DART; please report this!\n";
      assert(false);
    }
  }

  for (std::size_t i = 0; i < problem->getNumIneqConstraints(); ++i)
  {
    FunctionPtr fn = problem->getIneqConstraint(i);
    try
    {
      mOpt->add_inequality_constraint(
          NloptSolver::_nlopt_func, fn.get(), mProperties.mTolerance);
    }
    catch (const std::invalid_argument& e)
    {
      dterr << "[NloptSolver::solve] Encountered exception [" << e.what()
            << "] while adding an inequality constraint to an Nlopt solver. "
            << "Check whether your algorithm [" << nlopt::algorithm_name(mAlg)
            << "] (" << mAlg << ") supports inequality constraints!\n";
      assert(false);
    }
    catch (const std::exception& e)
    {
      dterr << "[NloptSolver::solve] Encountered exception [" << e.what()
            << "] while adding an inequality constraint to the Nlopt solver. "
            << "This might be a bug in DART; please report this!\n";
      assert(false);
    }
  }

  // Optimize
  mX = convertToStd(problem->getInitialGuess());
  nlopt::result result = mOpt->optimize(mX, mMinF);

  // If the result is not in this range, then it failed
  if (!(nlopt::SUCCESS <= result && result <= nlopt::XTOL_REACHED))
    return false;

  // Store optimal and optimum values
  problem->setOptimumValue(mMinF);
  problem->setOptimalSolution(convertToEigen(mX));

  return true;
}

//==============================================================================
Eigen::VectorXd NloptSolver::getLastConfiguration() const
{
  return convertToEigen(mX);
}

//==============================================================================
std::string NloptSolver::getType() const
{
  return "NloptSolver";
}

//==============================================================================
std::shared_ptr<Solver> NloptSolver::clone() const
{
  return std::make_shared<NloptSolver>(getSolverProperties(), getAlgorithm2());
}

//==============================================================================
void NloptSolver::copy(const NloptSolver& other)
{
  setProperties(other.getSolverProperties());
  setAlgorithm(other.getAlgorithm2());
}

//==============================================================================
NloptSolver& NloptSolver::operator=(const NloptSolver& other)
{
  copy(other);
  return *this;
}

//==============================================================================
void NloptSolver::setAlgorithm(nlopt::algorithm alg)
{
  setAlgorithm(convertAlgorithm(alg));
}

//==============================================================================
void NloptSolver::setAlgorithm(NloptSolver::Algorithm alg)
{
  mAlg = convertAlgorithm(alg);
}

//==============================================================================
nlopt::algorithm NloptSolver::getAlgorithm() const
{
  return convertAlgorithm(getAlgorithm2());
}

//==============================================================================
NloptSolver::Algorithm NloptSolver::getAlgorithm2() const
{
  return convertAlgorithm(mAlg);
}

//==============================================================================
#define NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(alg_name)                          \
  case alg_name:                                                               \
    return nlopt::algorithm::alg_name;

//==============================================================================
#define NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(alg_name)                          \
  case nlopt::algorithm::alg_name:                                             \
    return alg_name;

//==============================================================================
nlopt::algorithm NloptSolver::convertAlgorithm(NloptSolver::Algorithm algorithm)
{
  switch (algorithm)
  {
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT_L)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT_L_RAND)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT_NOSCAL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT_L_NOSCAL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_DIRECT_L_RAND_NOSCAL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_ORIG_DIRECT)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_ORIG_DIRECT_L)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GD_STOGO)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GD_STOGO_RAND)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_LBFGS_NOCEDAL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_LBFGS)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_PRAXIS)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_VAR1)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_VAR2)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_TNEWTON)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_TNEWTON_RESTART)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_TNEWTON_PRECOND)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_TNEWTON_PRECOND_RESTART)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_CRS2_LM)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_MLSL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GD_MLSL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GD_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_MMA)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_COBYLA)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_NEWUOA)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_NEWUOA_BOUND)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_NELDERMEAD)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_SBPLX)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_AUGLAG)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_AUGLAG)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LN_BOBYQA)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_ISRES)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(AUGLAG)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(G_MLSL)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(G_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_SLSQP)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(LD_CCSAQ)
    NLOPTSOLVER_ALGORITHM_DART_TO_NLOPT(GN_ESCH)
    default:
      dtwarn << "[NloptSolver] Attempt to convert unsupported algorithm '"
             << algorithm << "'. Use nlopt::algorithm::LN_COBYLA instead. \n";
      return nlopt::algorithm::LN_COBYLA;
  }
}

//==============================================================================
NloptSolver::Algorithm NloptSolver::convertAlgorithm(nlopt::algorithm algorithm)
{
  switch (algorithm)
  {
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT_L)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT_L_RAND)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT_NOSCAL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT_L_NOSCAL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_DIRECT_L_RAND_NOSCAL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_ORIG_DIRECT)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_ORIG_DIRECT_L)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GD_STOGO)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GD_STOGO_RAND)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_LBFGS_NOCEDAL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_LBFGS)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_PRAXIS)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_VAR1)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_VAR2)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_TNEWTON)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_TNEWTON_RESTART)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_TNEWTON_PRECOND)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_TNEWTON_PRECOND_RESTART)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_CRS2_LM)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_MLSL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GD_MLSL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GD_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_MMA)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_COBYLA)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_NEWUOA)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_NEWUOA_BOUND)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_NELDERMEAD)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_SBPLX)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_AUGLAG)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_AUGLAG)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LN_BOBYQA)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_ISRES)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(AUGLAG)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(AUGLAG_EQ)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(G_MLSL)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(G_MLSL_LDS)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_SLSQP)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(LD_CCSAQ)
    NLOPTSOLVER_ALGORITHM_NLOPT_TO_DART(GN_ESCH)
    default:
      dtwarn << "[NloptSolver] Attempt to convert unsupported algorithm '"
             << algorithm
             << "'. Use NloptSolver::Algorithm::LN_COBYLA instead. \n";
      return LN_COBYLA;
  }
}

//==============================================================================
double NloptSolver::_nlopt_func(
    unsigned n, const double* x, double* gradient, void* func_data)
{
  Function* fn = static_cast<Function*>(func_data);

  Eigen::Map<const Eigen::VectorXd> mapX(x, n);

  if (gradient)
  {
    Eigen::Map<Eigen::VectorXd> grad(gradient, n);
    fn->evalGradient(mapX, grad);
  }

  return fn->eval(mapX);
}

//==============================================================================
void NloptSolver::_nlopt_mfunc(
    unsigned m,
    double* result,
    unsigned n,
    const double* x,
    double* gradient,
    void* func_data)
{
  Eigen::Map<const Eigen::VectorXd> mapX(x, n);
  Eigen::Map<Eigen::VectorXd> f(result, m);
  Eigen::Map<Eigen::MatrixXd> grad(gradient, 0, 0);
  if (gradient)
    new (&grad) Eigen::Map<Eigen::MatrixXd, Eigen::RowMajor>(gradient, m, n);

  return (*static_cast<MultiFunction*>(func_data))(mapX, f, grad);
}

} // namespace optimizer
} // namespace dart


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

/// \file TidalOscillation.hh
/// \brief Interpolation of NOAA data for Tidal Oscillation feature

#ifndef __TIDAL_OSCILLATION_HH__
#define __TIDAL_OSCILLATION_HH__

#include <gazebo/gazebo.hh>
#include <cstdlib>
#include <ctime>
#include <random>
#include <utility>
#include <string>
#include <vector>

// #include <boost/math/interpolators/barycentric_rational.hpp>

namespace gazebo
{
  /// \brief Interpolation of NOAA data for Tidal Oscillation feature
  class TidalOscillation
  {
    /// \brief Class constructor
    public: TidalOscillation();

    /// \brief Resets the process parameters
    public: void Reset();

    /// \brief Prepare the data for interpolation
    public: void Initiate(bool _harmonicConstituents);

    /// \brief Translate datetime string to datenum
    public: double TranslateDate(std::array<int, 5> _datetime);

    /// \brief Input Datenum data
    public: std::vector<std::array<int, 5>> dateGMT;

    /// \brief Input Tidal data
    public: std::vector<double> speedcmsec;

    /// \brief Input Datenum data
    public: std::vector<double> datenum;

    /// \brief Bool for method type
    public: bool harmonicConstituent;

    /// \brief Tidal current harmonic constituents
    public: double M2_amp;
    public: double M2_phase;
    public: double M2_speed;
    public: double S2_amp;
    public: double S2_phase;
    public: double S2_speed;
    public: double N2_amp;
    public: double N2_phase;
    public: double N2_speed;

    /// \brief Input Tidal direction
    public: double ebbDirection;
    public: double floodDirection;

    /// \brief Input world start time
    public: std::array<int, 5> worldStartTime;
    public: double worldStartTime_num;

    /// \brief Update function for a new time stamp
    /// \param _time Current time stamp
    public: std::pair<double, double>
      Update(double _time, double _currentDepthRatio);

    /// \brief save current state (Flood: true, Ebb: false)
    public: bool currentType;
  };
}

#endif  // __TIDAL_OSCILLATION_HH__