// Boost.Geometry

// Copyright (c) 2018 Yaghyavardhan Singh Khangarot, Hyderabad, India.
// Contributed and/or modified by Yaghyavardhan Singh Khangarot,
//   as part of Google Summer of Code 2018 program.

// This file was modified by Oracle on 2018.
// Modifications copyright (c) 2018 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP

#include <algorithm>

#ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
#include <iostream>
#endif

#include <iterator>
#include <utility>
#include <vector>
#include <limits>

#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/distance_result.hpp>
#include <boost/geometry/util/range.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace discrete_frechet_distance
{

template <typename size_type1 , typename size_type2,typename result_type>
class coup_mat
{
public:
    coup_mat(size_type1 w, size_type2 h)
        : m_data(w * h,-1), m_width(w), m_height(h)
    {}

    result_type & operator()(size_type1 i, size_type2 j)
    {
        BOOST_GEOMETRY_ASSERT(i < m_width && j < m_height);
        return m_data[j * m_width + i];
    }

private:
    std::vector<result_type> m_data;
    size_type1 m_width;
    size_type2 m_height;
};

struct linestring_linestring
{
    template <typename Linestring1, typename Linestring2, typename Strategy>
    static inline typename distance_result
        <
            typename point_type<Linestring1>::type,
            typename point_type<Linestring2>::type,
            Strategy
        >::type apply(Linestring1 const& ls1, Linestring2 const& ls2, Strategy const& strategy)
    {
        typedef typename distance_result
            <
                typename point_type<Linestring1>::type,
                typename point_type<Linestring2>::type,
                Strategy
            >::type result_type;
        typedef typename boost::range_size<Linestring1>::type size_type1;
        typedef typename boost::range_size<Linestring2>::type size_type2;

        boost::geometry::detail::throw_on_empty_input(ls1);
        boost::geometry::detail::throw_on_empty_input(ls2);

        size_type1 const a = boost::size(ls1);
        size_type2 const b = boost::size(ls2);

        //Coupling Matrix CoupMat(a,b,-1);
        coup_mat<size_type1,size_type2,result_type> coup_matrix(a,b);

        result_type const not_feasible = -100;
        //findin the Coupling Measure
        for (size_type1 i = 0 ; i < a ; i++ )
        {
            for(size_type2 j=0;j<b;j++)
            {
                result_type dis = strategy.apply(range::at(ls1,i), range::at(ls2,j));
                if(i==0 && j==0)
                    coup_matrix(i,j) = dis;
                else if(i==0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i,j-1), dis);
                else if(i>0 && j==0)
                    coup_matrix(i,j) =
                        (std::max)(coup_matrix(i-1,j), dis);
                else if(i>0 && j>0)
                    coup_matrix(i,j) =
                        (std::max)((std::min)(coup_matrix(i,j-1),
                                              (std::min)(coup_matrix(i-1,j),
                                                         coup_matrix(i-1,j-1))),
                                   dis);
                else
                    coup_matrix(i,j) = not_feasible;
            }
        }

        #ifdef BOOST_GEOMETRY_DEBUG_FRECHET_DISTANCE
        //Print CoupLing Matrix
        for(size_type i = 0; i <a; i++)
        {
            for(size_type j = 0; j <b; j++)
            std::cout << coup_matrix(i,j) << " ";
            std::cout << std::endl;
        }
        #endif

        return coup_matrix(a-1,b-1);
    }
};

}} // namespace detail::frechet_distance
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{
template
<
    typename Geometry1,
    typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type
>
struct discrete_frechet_distance : not_implemented<Tag1, Tag2>
{};

template <typename Linestring1, typename Linestring2>
struct discrete_frechet_distance
    <
        Linestring1,
        Linestring2,
        linestring_tag,
        linestring_tag
    >
    : detail::discrete_frechet_distance::linestring_linestring
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       works for LineString-LineString) using specified strategy.
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\tparam Strategy A type fulfilling a DistanceStrategy concept
\param geometry1 Input geometry
\param geometry2 Input geometry
\param strategy Distance strategy to be used to calculate Pt-Pt distance

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Available Strategies]
\* [link geometry.reference.strategies.strategy_distance_pythagoras Pythagoras (cartesian)]
\* [link geometry.reference.strategies.strategy_distance_haversine Haversine (spherical)]
[/ \* more (currently extensions): Vincenty\, Andoyer (geographic) ]

[heading Example]
[discrete_frechet_distance_strategy]
[discrete_frechet_distance_strategy_output]
}
*/
template <typename Geometry1, typename Geometry2, typename Strategy>
inline typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type,
            Strategy
        >::type
discrete_frechet_distance(Geometry1 const& geometry1,
                          Geometry2 const& geometry2,
                          Strategy const& strategy)
{
    return dispatch::discrete_frechet_distance
            <
                Geometry1, Geometry2
            >::apply(geometry1, geometry2, strategy);
}

// Algorithm overload using default Pt-Pt distance strategy

/*!
\brief Calculate discrete Frechet distance between two geometries (currently
       work for LineString-LineString).
\ingroup discrete_frechet_distance
\tparam Geometry1 \tparam_geometry
\tparam Geometry2 \tparam_geometry
\param geometry1 Input geometry
\param geometry2 Input geometry

\qbk{[include reference/algorithms/discrete_frechet_distance.qbk]}

\qbk{
[heading Example]
[discrete_frechet_distance]
[discrete_frechet_distance_output]
}
*/
template <typename Geometry1, typename Geometry2>
inline typename distance_result
        <
            typename point_type<Geometry1>::type,
            typename point_type<Geometry2>::type
        >::type
discrete_frechet_distance(Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    typedef typename strategy::distance::services::default_strategy
              <
                  point_tag, point_tag,
                  typename point_type<Geometry1>::type,
                  typename point_type<Geometry2>::type
              >::type strategy_type;

    return discrete_frechet_distance(geometry1, geometry2, strategy_type());
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISCRETE_FRECHET_DISTANCE_HPP


/*
 * Copyright (c) 2018, The Robot Studio
 *  All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright notice, this
 *	  list of conditions and the following disclaimer.
 *
 *	* Redistributions in binary form must reproduce the above copyright notice,
 *	  this list of conditions and the following disclaimer in the documentation
 *	  and/or other materials provided with the distribution.
 *
 * 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.
 */

/**
 * @file odometry.cpp
 * @author Cyril Jourdan
 * @date Nov 26, 2018
 * @version 0.1.0
 * @brief File for the odometry of the holonomic base with 3 wheels
 * With help of website https://bharat-robotics.github.io/blog/kinematic-analysis-of-holonomic-robot
 *
 * Contact: cyril.jourdan@therobotstudio.com
 * Created on : Nov 26, 2018
 */

/*** Includes ***/
#include "ros/ros.h"
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <osa_msgs/MotorDataMultiArray.h>
//#include <Eigen/Dense>

using namespace std;

/*** Variables ***/
osa_msgs::MotorDataMultiArray motor_data_array;

//to wait for the msg from both posture and anglesArmDescription topics
bool motor_data_array_arrived = false;

/*** Callback functions ***/
void motorDataArrayCallback(const osa_msgs::MotorDataMultiArrayConstPtr& data)
{
	motor_data_array = *data;
	motor_data_array_arrived = true;
}

/*** Main ***/
int main (int argc, char** argv)
{
	// Initialize ROS
	ros::init(argc, argv, "osa_r2p1_odometry_node");
	ros::NodeHandle nh("~");

	//Subscribers
	ros::Subscriber sub_motor_data_array = nh.subscribe("/foldy_base/motor_data_array", 10, motorDataArrayCallback);

	//Publishers
	ros::Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);

	// initial position
	double x = 0.0;
	double y = 0.0;
	double th = 0.0;

	// velocity
	double vx = 0.0;
	double vy = 0.0;
	double vth = 0.0;

	// motor position
	int curr_mot_enc_pos[3] = {0};
	int prev_mot_enc_pos[3] = {0};
	int diff_mot_enc_pos[3] = {0};

	double wheel_lin_vel[3] = {0}; //V1, V2, V3

	const double gear_ratio = 28/1;
	const double enc_tic_per_motor_turn = 1000*4; //multiply by 4 for the quadrate encoder
	const double enc_tic_per_shaft_turn = enc_tic_per_motor_turn*gear_ratio;
	const double robot_base_radius = 0.29; //in meters
	const double swedish_wheel_radius = 0.0625;

	ros::Time curr_time;
	ros::Time prev_time;
	curr_time = ros::Time::now();
	prev_time = ros::Time::now();
	double dt = 0.0;

	double delta_x = 0.0;
	double delta_y = 0.0;
	double delta_th = 0.0;

	tf::TransformBroadcaster broadcaster;
	ros::Rate loop_rate(20);

	//const double degree = M_PI/180;

	// message declarations
	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id = "dummy_link";

	//grasp first position before starting the loop
	if(ros::ok())
	{
		ros::spinOnce();

		if(motor_data_array_arrived)
		{
			prev_time = ros::Time::now();

			for(int i=0; i<3; i++) prev_mot_enc_pos[i] = motor_data_array.motor_data.at(i).position; //fill the prev values

			motor_data_array_arrived = false;
		}
	}

	while(ros::ok())
	{
		ros::spinOnce();

		if(motor_data_array_arrived)
		{
			// time calculation
			curr_time = ros::Time::now();
			dt = (curr_time - prev_time).toSec();

			// encoder position
			for(int i=0; i<3; i++)
			{
				curr_mot_enc_pos[i] = motor_data_array.motor_data.at(i).position;
				diff_mot_enc_pos[i] = curr_mot_enc_pos[i] - prev_mot_enc_pos[i];

				//Explaination : enc_tic_per_shaft_turn is done in 2*Pi. So an angle theta is done in 2*Pi*diff_enc/enc_tic_per_shaft_turn
				// v = d/t so d = theta*r = 2*Pi*r*diff_enc/enc_tic_per_shaft_turn
				// then just divide by dt to get the linear velocity of the wheel
				wheel_lin_vel[i] = (2*M_PI*swedish_wheel_radius*diff_mot_enc_pos[i])/(enc_tic_per_shaft_turn*dt);

				//ROS_INFO("Motor %d: enc=%d, diff_enc=%d, lin_vel=%f", i, curr_mot_enc_pos[i], diff_mot_enc_pos[i], curr_mot_enc_pos[2]);
			}

			//ROS_INFO("enc1=%d, enc2=%d, enc3=%d", curr_mot_enc_pos[0], curr_mot_enc_pos[1], curr_mot_enc_pos[2]);

			//calculation of theta_dot which is the variable vth here
			vth = -(wheel_lin_vel[0]+wheel_lin_vel[1]+wheel_lin_vel[2])/(3*robot_base_radius);
			delta_th = vth*dt;

			//ROS_INFO("delta_th=%f", delta_th);

			vx = -2*(-cos(delta_th)*wheel_lin_vel[0] + cos(M_PI/3-delta_th)*wheel_lin_vel[1]  + cos(M_PI/3+delta_th)*wheel_lin_vel[2])/3; //multiply by -1 to change X direction
			vy = 2*(-sin(delta_th)*wheel_lin_vel[0] - sin(M_PI/3-delta_th)*wheel_lin_vel[1]  + sin(M_PI/3+delta_th)*wheel_lin_vel[2])/3;

			delta_x = vx*dt;
			delta_y = vy*dt;

			//accumulated position rotated by delta_th and final angle, this will drift over time with the accumulated errors
			//The angle of Pi/6 is because of the convention of having a wheel on the X axis as opposed to the Y axis
			x += cos(th-M_PI/6)*delta_x - sin(th-M_PI/6)*delta_y;
			y += sin(th-M_PI/6)*delta_x + cos(th-M_PI/6)*delta_y;
			th += delta_th;

			ROS_DEBUG("vth=%f, delta_th=%f, vx=%f, vy=%f, delta_x=%f, delta_y=%f, x=%f, y=%f, th=%f", vth, delta_th, vx, vy, delta_x, delta_y, x, y, th);
			//ROS_INFO("vx=%f, delta_x=%f, x=%f", vx, delta_x, x);
			//ROS_INFO("dx=%f", delta_x);

			geometry_msgs::Quaternion odom_quat;
			odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);

			// update transform
			odom_trans.header.stamp = curr_time;
			odom_trans.transform.translation.x = x;
			odom_trans.transform.translation.y = y;
			odom_trans.transform.translation.z = 0.0;
			odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);

			// filling the odometry
			nav_msgs::Odometry odom;
			odom.header.stamp = curr_time;
			odom.header.frame_id = "odom";
			odom.child_frame_id = "dummy_link";

			// position
			odom.pose.pose.position.x = x;
			odom.pose.pose.position.y = y;
			odom.pose.pose.position.z = 0.0;
			odom.pose.pose.orientation = odom_quat;

			// velocity
			odom.twist.twist.linear.x = vx;
			odom.twist.twist.linear.y = vy;
			odom.twist.twist.linear.z = 0.0;
			odom.twist.twist.angular.x = 0.0;
			odom.twist.twist.angular.y = 0.0;
			odom.twist.twist.angular.z = vth;

			// publishing the odometry and the new tf
			broadcaster.sendTransform(odom_trans);
			odom_pub.publish(odom);

			motor_data_array_arrived = false;

			prev_time = curr_time;
			prev_mot_enc_pos[0] = curr_mot_enc_pos[0];
			prev_mot_enc_pos[1] = curr_mot_enc_pos[1];
			prev_mot_enc_pos[2] = curr_mot_enc_pos[2];

			loop_rate.sleep();
		}
	}

	return 0;
}


#include <iostream>
#include <fstream>
#include <sstream>
#include <algorithm>

#include <CGAL/Simple_cartesian.h>
#include <CGAL/Polyhedron_3.h>

//for obj export
#include <CGAL/IO/print_wavefront.h>

// Simplification function
#include <CGAL/Surface_mesh_simplification/edge_collapse.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/property_map.h>

// Stop-condition policy
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Count_stop_predicate.h>
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Edge_length_cost.h>
#include <CGAL/Surface_mesh_simplification/Policies/Edge_collapse/Midpoint_placement.h>

#include<CGAL/Polyhedron_incremental_builder_3.h>

#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/trim.hpp>

#include <scm/core.h>
#include <scm/core/math.h>

typedef CGAL::Simple_cartesian<double> Kernel;
typedef CGAL::Polyhedron_3<Kernel> Surface_mesh;
typedef Surface_mesh::HalfedgeDS HalfedgeDS;

typedef boost::graph_traits<Surface_mesh>::vertex_descriptor vertex_descriptor;

typedef CGAL::Surface_mesh<Kernel::Point_3> GMesh;
typedef GMesh::Vertex_index vertex_index;

namespace SMS = CGAL::Surface_mesh_simplification ;

struct GVertex
{
  Kernel::Point_3 v_pos;
  Kernel::Point_2 t_coord;
  Kernel::Point_3 normal;
};

char* getCmdOption(char ** begin, char ** end, const std::string & option) {
  char ** itr = std::find(begin, end, option);
  if (itr != end && ++itr != end) {
      return *itr;
  }
  return 0;
}

bool cmdOptionExists(char** begin, char** end, const std::string& option) {
  return std::find(begin, end, option) != end;
}

// // A modifier creating a triangle with the incremental builder.
// template<class HDS>
// class polyhedron_builder : public CGAL::Modifier_base<HDS> {

// public:
//   std::vector<double> &coords;
//   std::vector<int>    &tris;

//   polyhedron_builder( std::vector<double> &_coords, std::vector<int> &_tris ) : coords(_coords), tris(_tris) {}

//   void operator()( HDS& hds) {
//     typedef typename HDS::Vertex   Vertex;
//     typedef typename Vertex::Point Point;

//     // create a cgal incremental builder
//     CGAL::Polyhedron_incremental_builder_3<HDS> B( hds, true);
//     B.begin_surface( coords.size()/3, tris.size()/3 );

//     // add the polyhedron vertices
//     for( int i=0; i<(int)coords.size(); i+=3 ){
//       B.add_vertex( Point( coords[i+0], coords[i+1], coords[i+2] ) );
//     }

//     // add the polyhedron triangles
//     for( int i=0; i<(int)tris.size(); i+=3 ){
//       B.begin_facet();
//       B.add_vertex_to_facet( tris[i+0] );
//       B.add_vertex_to_facet( tris[i+1] );
//       B.add_vertex_to_facet( tris[i+2] );
//       B.end_facet();
//     }

//     // finish up the surface
//     B.end_surface();
//     }
// };

//custom function to build a mesh of type Surface Mesh
void build_surface_mesh (GMesh &_gmesh,
              std::vector<GVertex> &_vertices){

  //to store refs to vertices while the mesh is being built
  std::vector<vertex_index> v_indices;

  //add vertex positions, record indexes
  for (int i = 0; i < (int)_vertices.size(); i+=3){
    vertex_index v1 = _gmesh.add_vertex(_vertices[i].v_pos);
    vertex_index v2 = _gmesh.add_vertex(_vertices[i+1].v_pos);
    vertex_index v3 = _gmesh.add_vertex(_vertices[i+2].v_pos);

    //add face
    _gmesh.add_face(v1, v2, v3);

    // v_indices.push_back(v);
    v_indices.insert(v_indices.end(), {v1, v2, v3});
  }

  //add faces
  // for (int i = 0; i < (int)_vertices.size(); i+=3){
  //   _gmesh.add_face(v_indices[_tris[i+0]],v_indices[_tris[i+1]], v_indices[_tris[i+2]]);
  // }

  //create a property map for adding normals
  //ref: https://doc.cgal.org/4.7/Surface_mesh/index.html#sectionSurfaceMesh_properties
  GMesh::Property_map<vertex_index, Kernel::Point_3> normal;
  bool created;
  boost::tie(normal, created) =
    _gmesh.add_property_map<vertex_index, Kernel::Point_3>("v:normal", Kernel::Point_3(0,0,0));
  assert(created);

  //add normals
  for (int i = 0; i < (int)v_indices.size(); ++i)
  {
    normal[v_indices[i]] = _vertices[i].normal;
    std::cout << normal[v_indices[i]] << std::endl;
  }

  // uint32_t num_normals = 0;
  // BOOST_FOREACH( vertex_index vi, _gmesh.vertices()) {
  //   normal[vi] = Kernel::Point_3(_normals[num_normals+0], _normals[num_normals+1], _normals[num_normals+2]);
  //   std::cout << normal[vi] << std::endl;
  //   num_normals+=3;
  // }

}

//parses a face string like "f  2//1  8//1  4//1 " into 3 given arrays
void parse_face_string (std::string face_string, uint32_t (&index)[3], uint32_t (&coord)[3], uint32_t (&normal)[3]){

  //split by space into faces
  std::vector<std::string> faces;
  boost::algorithm::trim(face_string);
  boost::algorithm::split(faces, face_string, boost::algorithm::is_any_of(" "), boost::algorithm::token_compress_on);

  for (int i = 0; i < 3; ++i)
  {
    //split by / for indices
    std::vector<std::string> inds;
    boost::algorithm::split(inds, faces[i], [](char c){return c == '/';}, boost::algorithm::token_compress_off);

    for (int j = 0; j < (int)inds.size(); ++j)
    {
      uint32_t idx = 0;
      //parse value from string
      if (inds[j] != ""){
        idx = (uint32_t)stoi(inds[j]);
      }
      if (j == 0){index[i] = idx;}
      else if (j == 1){coord[i] = idx;}
      else if (j == 2){normal[i] = idx;}

    }
  }
}

// load obj function from vt_obj_loader/Utils.h
void load_obj(const std::string& filename,
              std::vector<GVertex>& vertices){

  std::vector<Kernel::Point_3> v;
  std::vector<uint32_t> vindices;
  std::vector<Kernel::Point_3> n;
  std::vector<uint32_t> nindices;
  std::vector<Kernel::Point_2> t;
  std::vector<uint32_t> tindices;

  FILE *file = fopen(filename.c_str(), "r");

  if (0 != file) {

    while (true) {
      char line[128];
      int32_t l = fscanf(file, "%s", line);

      if (l == EOF) break;
      if (strcmp(line, "v") == 0) {
        double vx, vy, vz;
        fscanf(file, "%lf %lf %lf\n", &vx, &vy, &vz);
        v.push_back(Kernel::Point_3(vx,vy,vz));
      }
      else if (strcmp(line, "vn") == 0) {
        float nx, ny, nz;
        fscanf(file, "%f %f %f\n", &nx, &ny, &nz);
        n.push_back(Kernel::Point_3(nx,ny,nz));
      }
      else if (strcmp(line, "vt") == 0) {
        float tx, ty;
        fscanf(file, "%f %f\n", &tx, &ty);
        t.push_back(Kernel::Point_2(tx,ty));
      }
      else if (strcmp(line, "f") == 0) {
        fgets(line, 128, file);
        std::string face_string = line;
        uint32_t index[3];
        uint32_t coord[3];
        uint32_t normal[3];

        parse_face_string(face_string, index, coord, normal);

        vindices.insert(vindices.end(), {index[0], index[1], index[2]});
        tindices.insert(tindices.end(), {coord[0], coord[1], coord[2]});
        nindices.insert(nindices.end(), {normal[0], normal[1], normal[2]});
      }
    }

    fclose(file);

    std::cout << "positions: " << v.size() << std::endl;
    std::cout << "normals: " << n.size() << std::endl;
    std::cout << "coords: " << t.size() << std::endl;
    std::cout << "faces: " << vindices.size() << std::endl;

  }

  //read from indices arrays to construct list of vertices in open GL style
  //http://www.opengl-tutorial.org/beginners-tutorials/tutorial-7-model-loading/
  for (int i = 0; i < (int)vindices.size(); ++i)
  {
    Kernel::Point_3 newv;
    Kernel::Point_2 newt;
    Kernel::Point_3 newn;

    //guard against empty arrays
    if (v.size() == 0) {newv = Kernel::Point_3(0,0,0);}
    else {newv = v[vindices[i]-1];}

    if (t.size() == 0) {newt = Kernel::Point_2(0,0,0);}
    else {newt = t[tindices[i]-1];}

    if (n.size() == 0) {newn = Kernel::Point_3(0,0,0);}
    else {newn = n[nindices[i]-1];}

    GVertex new_Vertex = {newv, newt, newn};
    vertices.push_back(new_Vertex);
  }

}

// void output_obj (GMesh &gmesh, const std::string& filename, std::vector<double>& v, std::vector<int>& vindices) {
//   // https://doc.cgal.org/4.9/Surface_mesh/index.html

//     typedef typename HDS::Vertex   Vertex;
//     typedef typename Vertex::Point Point;

//   std::cout << "all vertices " << std::endl;
//   // The vertex iterator type is a nested type of the Vertex_range
//   GMesh::Vertex_range::iterator  vb, ve;
//   GMesh::Vertex_range r = gmesh.vertices();
//   // The iterators can be accessed through the C++ range API
//   vb = r.begin();
//   ve = r.end();
//   // or the boost Range API
//   vb = boost::begin(r);
//   ve = boost::end(r);
//   // or with boost::tie, as the CGAL range derives from std::pair
//   for(boost::tie(vb, ve) = gmesh.vertices(); vb != ve; ++vb){
//     Point p = gmesh.point(*vb);
//     std::cout << p << std::endl;
//           // std::cout << *vb << std::endl;
//   }
// }

int main( int argc, char** argv )
{
  std::string obj_filename = "dino.obj";
  if (cmdOptionExists(argv, argv+argc, "-f")) {
    obj_filename = std::string(getCmdOption(argv, argv + argc, "-f"));
  }
  else {
    std::cout << "Please provide a obj filename using -f <filename.obj>" << std::endl;
    return 1;
  }
  std::string out_filename = "data/simplified_mesh.obj";
  if (cmdOptionExists(argv, argv+argc, "-o")) {
    out_filename = std::string(getCmdOption(argv, argv + argc, "-o"));
  }

  //load OBJ into vertex array
  std::vector<GVertex> vertices;
  load_obj( obj_filename, vertices);

  if (vertices.size() == 0 ) {
    std::cout << "didnt find any vertices" << std::endl;
    return 1;
  }
  std::cout << "Mesh loaded (" << vertices.size() << " vertices)" << std::endl;

  // print loaded obj info
  // std::cout << "points" << std::endl;
  // for (int i = 0; i < points.size(); i+=3)
  // {
  //   std::cout << points[i] << " "  << points[i+1] << " "  << points[i+2] << std::endl;
  // }
  // std::cout << "faces" << std::endl;
  // for (int i = 0; i < tris.size(); i+=3)
  // {
  //   std::cout << tris[i] << " "  << tris[i+1] << " "  << tris[i+2] << std::endl;
  // }
  // std::cout << "normals" << std::endl;
  // for (int i = 0; i < normals.size(); i+=3)
  // {
  //   std::cout << normals[i] << " "  << normals[i+1] << " "  << normals[i+2] << std::endl;
  // }
  // std::cout << "norm indexes" << std::endl;
  // for (int i = 0; i < n_inds.size(); i+=3)
  // {
  //   std::cout << n_inds[i] << " "  << n_inds[i+1] << " "  << n_inds[i+2] << std::endl;
  // }

  //create a mesh from vectors
  GMesh gmesh;
  build_surface_mesh(gmesh, vertices);

  if (gmesh.is_valid(true)){
    std::cout << "mesh valid\n";
  }

  if (!CGAL::is_triangle_mesh(gmesh)){
    std::cerr << "Input geometry is not triangulated." << std::endl;
    return EXIT_FAILURE;
  }
  // This is a stop predicate (defines when the algorithm terminates).
  // In this example, the simplification stops when the number of undirected edges
  // left in the surface mesh drops below the specified number (1000)
  SMS::Count_stop_predicate<Surface_mesh> stop(300);

  std::cout << "Starting simplification" << std::endl;

  // This the actual call to the simplification algorithm.
  // The surface mesh and stop conditions are mandatory arguments.
  // The index maps are needed because the vertices and edges
  // of this surface mesh lack an "id()" field.
  // int r = SMS::edge_collapse
  //           (gmesh
  //           ,stop
  //            ,CGAL::parameters::halfedge_index_map  (get(CGAL::halfedge_external_index  ,gmesh))
  //            // ,CGAL::parameters::vertex_index_map(get(CGAL::vertex_external_index,gmesh))
  //                              // .halfedge_index_map  (get(CGAL::halfedge_external_index  ,gmesh))
  //            //                   .get_cost (SMS::Edge_length_cost <Surface_mesh>())
  //            //                   .get_placement(SMS::Midpoint_placement<Surface_mesh>())
  //           );

  // std::cout << "\nFinished...\n" << (gmesh.size_of_halfedges()/2) << " final edges.\n" ;

  // output_obj(gmesh,"bla", points, tris);

  //write to file
  std::ofstream ofs(out_filename);
  ofs << gmesh;
  // CGAL::print_polyhedron_wavefront(ofs, gmesh);
  ofs.close();
  std::cout << "Simplified mesh was written to " << out_filename << std::endl;

  return EXIT_SUCCESS ;
}


#ifndef EDGE_PLANE_IDENTITY_HPP
#define EDGE_PLANE_IDENTITY_HPP

#include <Eigen/Dense>
#include <g2o/core/base_binary_edge.h>
#include <g2o/types/slam3d_addons/vertex_plane.h>

namespace g2o {

/**
 * @brief A modified version of g2o::EdgePlane. This class takes care of flipped plane normals.
 *
 */
class EdgePlaneIdentity : public BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  EdgePlaneIdentity() : BaseBinaryEdge<4, Eigen::Vector4d, VertexPlane, VertexPlane>() {
    _information.setIdentity();
    _error.setZero();
  }
  void computeError() {
    const VertexPlane* v1 = static_cast<const VertexPlane*>(_vertices[0]);
    const VertexPlane* v2 = static_cast<const VertexPlane*>(_vertices[1]);

    Eigen::Vector4d p1 = v1->estimate().toVector();
    Eigen::Vector4d p2 = v2->estimate().toVector();

    if(p1.dot(p2) < 0.0) {
      p2 = -p2;
    }

    _error = (p2 - p1) - _measurement;
  }
  virtual bool read(std::istream& is) override {
    Eigen::Vector4d v;
    for(int i = 0; i < 4; ++i) {
      is >> v[i];
    }

    setMeasurement(v);
    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        is >> information()(i, j);
        if(i != j) {
          information()(j, i) = information()(i, j);
        }
      }
    }

    return true;
  }

  virtual bool write(std::ostream& os) const override {
    for(int i = 0; i < 4; ++i) {
      os << _measurement[i] << " ";
    }

    for(int i = 0; i < information().rows(); ++i) {
      for(int j = i; j < information().cols(); ++j) {
        os << " " << information()(i, j);
      };
    }
    return os.good();
  }

  virtual void setMeasurement(const Eigen::Vector4d& m) override {
    _measurement = m;
  }

  virtual int measurementDimension() const override {
    return 4;
  }
};

}  // namespace g2o

#endif  // EDGE_PLANE_PARALLEL_HPP


#include <boost/proto/proto.hpp>
#include <boost/proto/context/default.hpp>
#include <boost/mpl/int.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/cmath.hpp>
#include <type_traits>

struct x_var {};

template <int v>
struct derivative_constant
{
	using result_type = int;
	static const int value = v;

	template <typename ...Args>
	result_type operator()(const Args&... a) const { return v; }

	operator result_type() const { return v; }
};

template <int V>
using derivative_constant_terminal = typename boost::proto::terminal<derivative_constant<V>>::type;

struct derivative_grammar;

template<typename Expr>
struct derivative_expr;

namespace derivative_detail
{
	struct optimize
		: boost::proto::switch_<struct optimize_cases_>
	{};

	struct expr_generator
	{
		BOOST_PROTO_CALLABLE()
		BOOST_PROTO_USE_BASIC_EXPR()

		template<typename Sig>
		struct result;

		template<typename This, typename Expr>
		struct result<This(Expr)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename This, typename Expr>
		struct result<This(Expr &)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename This, typename Expr>
		struct result<This(Expr const &)>
		{
			typedef derivative_expr<Expr> type;
		};

		template<typename Expr>
		derivative_expr<Expr> operator ()(Expr const &e) const
		{
			derivative_expr<Expr> that = { e };
			return that;
		}
	};

}

struct domain
	: boost::proto::domain< derivative_detail::expr_generator, derivative_grammar >
{
	template< typename T >
	struct as_child
		: proto_base_domain::as_expr< T >
	{};
};

template< int Exp >
struct pow_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;
		BOOST_FORCEINLINE static type apply(const T& t) { using std::pow; return pow(t, Exp); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units,T>)>
	{
		using type = decltype(boost::units::pow<Exp>(std::declval<boost::units::quantity<Units,T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units,T>& t) { using boost::units::pow; return pow<Exp>(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<pow_fun<Exp>(T)>::type operator()(const T& d) const
	{
		return result<pow_fun<Exp>(T)>::apply(d);
	}
};

template< int Exp, typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, pow_fun< Exp >, Arg>::type const pow(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(pow_fun<Exp>(), arg);
}

struct sin_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::sin; return sin(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::sin(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::sin; return sin(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<sin_fun(T)>::type operator()(const T& d) const
	{
		return result<sin_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, sin_fun, Arg>::type const sin(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(sin_fun(), arg);
}

struct cos_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::cos; return cos(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::cos(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::cos; return cos(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<cos_fun(T)>::type operator()(const T& d) const
	{
		return result<cos_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, cos_fun, Arg>::type const cos(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(cos_fun(), arg);
}

struct exp_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::exp; return exp(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::exp(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::exp; return exp(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<exp_fun(T)>::type operator()(const T& d) const
	{
		return result<exp_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, exp_fun, Arg>::type const exp(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(exp_fun(), arg);
}

struct tan_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::tan; return tan(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::tan(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::tan; return tan(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<tan_fun(T)>::type operator()(const T& d) const
	{
		return result<tan_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, tan_fun, Arg>::type const tan(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(tan_fun(), arg);
}

struct sqrt_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::sqrt; using std::abs; return sqrt(abs(t)); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::root<2>(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::abs; return boost::units::root<2>(abs(t)); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<sqrt_fun(T)>::type operator()(const T& d) const
	{
		return result<sqrt_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, sqrt_fun, Arg>::type const sqrt(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(sqrt_fun(), arg);
}

struct log_fun
{
	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using type = T;

		BOOST_FORCEINLINE static type apply(const T& t) { using std::log; return log(t); }
	};

	template <typename This, typename Units, typename T>
	struct result<This(boost::units::quantity<Units, T>)>
	{
		using type = decltype(boost::units::log(std::declval<boost::units::quantity<Units, T>>()));
		BOOST_FORCEINLINE static type apply(const boost::units::quantity<Units, T>& t) { using boost::units::log; return log(t); }
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<log_fun(T)>::type operator()(const T& d) const
	{
		return result<log_fun(T)>::apply(d);
	}
};

template< typename Arg >
BOOST_FORCEINLINE typename boost::proto::result_of::make_expr<boost::proto::tag::function, log_fun, Arg>::type const log(Arg arg)
{
	return boost::proto::make_expr<boost::proto::tag::function>(log_fun(), arg);
}

template <typename T>
struct evaluation_context : boost::proto::callable_context<evaluation_context<T> const> {
	T value;

	explicit evaluation_context(T value)
		: value(value)
	{}

	template <typename Sig>
	struct result;

	template <typename This, typename Tag, int V>
	struct result<This(Tag, derivative_constant<V>)>
	{
		using type = double;
	};

	template <typename This, typename Tag, int V>
	struct result<This(Tag, const derivative_constant<V>&)>
	{
		using type = double;
	};

	template <typename This, typename Tag>
	struct result<This(Tag, x_var)>
	{
		using type = T;
	};

	template <typename This, typename Tag>
	struct result<This(Tag, const x_var&)>
	{
		using type = T;
	};

	template <int V>
	BOOST_FORCEINLINE double operator()(boost::proto::tag::terminal, derivative_constant<V>) const
	{
		return V;
	}

	BOOST_FORCEINLINE T operator()(boost::proto::tag::terminal, x_var) const
	{
		return value;
	}
};

template<typename Expr>
struct derivative_expr : boost::proto::extends<Expr, derivative_expr<Expr>, domain>
{
	typedef boost::proto::extends<Expr, derivative_expr<Expr>, domain> base_type;

	derivative_expr(Expr const& expr = Expr())
		: base_type(expr)
	{}

	template <typename Sig>
	struct result;

	template <typename This, typename T>
	struct result<This(T)>
	{
		using raw_type = typename boost::proto::result_of::eval<Expr, evaluation_context<T>>::type;
		using type = typename boost::remove_const<typename boost::remove_reference<raw_type>::type>::type;
	};

	template <typename T>
	BOOST_FORCEINLINE typename result<derivative_expr(T)>::type operator()(T d) const
	{
		evaluation_context<T> context(d);
		return boost::proto::eval(*this, context);
	}
};

//! Constants
struct constant_rule : boost::proto::callable
{
	using result_type = derivative_expr <boost::proto::terminal<derivative_constant<0>>::type>;

	BOOST_FORCEINLINE result_type operator()() const
	{
		return result_type();
	}
};

//! Derivative Variable
struct derivative_variable_rule : boost::proto::callable
{
	using result_type = derivative_expr <boost::proto::terminal<derivative_constant<1>>::type>;

	BOOST_FORCEINLINE result_type operator()() const
	{
		return result_type();
	}
};

//! Addition

namespace derivative_detail {

	template<typename Tag>
	struct make_optimized_binary
	{
		BOOST_PROTO_CALLABLE();
		BOOST_PROTO_POLY_FUNCTION();

		using Domain = boost::proto::domainns_::deduce_domain;

		template<typename Sig>
		struct result;

		template<typename This, typename A0, typename A1>
		struct result<This(A0, A1)>
		{
			using raw_type = typename boost::proto::result_of::make_expr<Tag, Domain, A0, A1>::type;

			using type = typename boost::result_of<optimize(raw_type)>::type;
		};

		/// Construct an expression node with tag type \c Tag
		/// and in the domain \c Domain.
		///
		/// \return <tt>proto::make_expr\<Tag, Domain\>(a0,...aN)</tt>
		template<typename A0, typename A1>
		BOOST_FORCEINLINE
		typename result<make_optimized_binary<Tag>(A0,A1)>::type const operator ()(A0 const &a0, A1 const& a1) const
		{
			auto result = optimize()(boost::proto::detail::make_expr_<Tag, Domain, A0 const, A1 const>()(a0, a1));
			//boost::proto::display_expr(result, std::cout);
			return result;
		}

		/// INTERNAL ONLY
		///
		template<typename A0, typename A1>
		struct impl
			: boost::proto::detail::make_expr_<Tag, Domain, A0, A1>
		{};
	};

	struct optimize_cases_
	{
		template <typename Tag, int D = 0>
		struct case_ : boost::proto::not_<boost::proto::_> {};

		template <int D>
		struct case_<boost::proto::tag::plus, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	optimize(boost::proto::_right)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::plus, boost::proto::_, boost::proto::_ >
				,	make_optimized_binary<boost::proto::tag::plus>(optimize(boost::proto::_left), optimize(boost::proto::_right))
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::minus, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	boost::proto::_make_negate(optimize(boost::proto::_right))
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::minus, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::minus>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::multiplies, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::terminal<derivative_constant<0>> >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::terminal<derivative_constant<1>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::terminal<derivative_constant<1>>, boost::proto::_ >
				,	optimize(boost::proto::_right)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::multiplies, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::multiplies>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::divides, D>
			: boost::proto::or_
			  <
				boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::terminal<derivative_constant<0>>, boost::proto::_ >
				,	constant_rule()
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::_, boost::proto::terminal<derivative_constant<1>> >
				,	optimize(boost::proto::_left)
				>
				, boost::proto::when
				<
					boost::proto::binary_expr< boost::proto::tag::divides, boost::proto::_, boost::proto::_ >
				,	boost::proto::call<make_optimized_binary<boost::proto::tag::divides>(optimize(boost::proto::_left), optimize(boost::proto::_right))>
				>
				, boost::proto::otherwise<boost::proto::_expr>
			  >
		{};

		template <int D>
		struct case_<boost::proto::tag::function, D>
			: boost::proto::when
		      <
				boost::proto::_
			  , boost::proto::_make_function(boost::proto::_left, optimize(boost::proto::_right))
			  >
		{};
	};

	template <typename T1, typename T2>
	struct plus
	{
		using type = decltype(std::declval<T1>() + std::declval<T2>());
	};

	template <typename T1>
	struct plus<T1, derivative_constant<0>>
	{
		using type = T1;
	};

	template <typename T2>
	struct plus<derivative_constant<0>, T2>
	{
		using type = T2;
	};

}//! namespace derivative_detail;

namespace boost {
	namespace proto {
		template<typename Tag>
		struct is_callable<derivative_detail::make_optimized_binary<Tag> >
			: mpl::true_
		{};
} }

struct addition_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename T1, typename T2>
	struct result<This(T1, T2)>
	{
	private:
		using t1_t = typename boost::remove_const<typename boost::remove_reference<T1>::type>::type;
		using t2_t = typename boost::remove_const<typename boost::remove_reference<T2>::type>::type;

	public:
		using raw_type = typename derivative_detail::plus<t1_t, t2_t>::type;
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename T1, typename T2>
	typename result<addition_rule(T1, T2)>::type operator()(const T1& t1, const T2& t2) const
	{
		return derivative_detail::optimize()(t1 + t2);
	}
};

//! Subtraction

namespace derivative_detail {

	template <typename T1, typename T2>
	struct minus
	{
		using type = decltype(std::declval<T1>() - std::declval<T2>());
	};

	template <typename T1>
	struct minus<T1, derivative_constant<0>>
	{
		using type = T1;
	};

	template <typename T2>
	struct minus<derivative_constant<0>, T2>
	{
		using type = decltype(-std::declval<T2>());
	};

}//! namespace derivative_detail;

struct subtraction_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename T1, typename T2>
	struct result<This(T1, T2)>
	{
	private:
		using t1_t = typename boost::remove_const<typename boost::remove_reference<T1>::type>::type;
		using t2_t = typename boost::remove_const<typename boost::remove_reference<T2>::type>::type;

	public:
		using raw_type = typename derivative_detail::minus<t1_t, t2_t>::type;
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename T1, typename T2>
	typename result<subtraction_rule(T1, T2)>::type operator()(const T1& t1, const T2& t2) const
	{
		return derivative_detail::optimize()(t1 - t2);
	}
};

//! Multiplication
namespace derivative_detail {

	template <typename T1, typename T2>
	struct multiplies
	{
		using type = decltype(std::declval<T1>() * std::declval<T2>());
	};

	template <typename T1>
	struct multiplies<T1, derivative_constant<0>>
	{
		using type = derivative_constant<0>;
	};

	template <typename T2>
	struct multiplies<derivative_constant<0>, T2>
	{
		using type = derivative_constant<0>;
	};

	template <typename T1>
	struct multiplies<T1, derivative_constant<1>>
	{
		using type = T1;
	};

	template <typename T2>
	struct multiplies<derivative_constant<1>, T2>
	{
		using type = T2;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct product_rule_result_helper
	{
		using termA = decltype(std::declval<dArg1>() * std::declval<Arg2>());
		using termB = decltype(std::declval<Arg1>() * std::declval<dArg2>());
		using otermA = typename boost::result_of<optimize(termA)>::type;
		using otermB = typename boost::result_of<optimize(termB)>::type;
		using type = decltype(std::declval<otermA>() + std::declval<otermB>());
	};

}//! namespace derivative_detail;

struct product_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct result<This(Arg1, dArg1, Arg2, dArg2)>
	{
	private:

		using arg1_t = typename boost::remove_const<typename boost::remove_reference<Arg1>::type>::type;
		using darg1_t = typename boost::remove_const<typename boost::remove_reference<dArg1>::type>::type;
		using arg2_t = typename boost::remove_const<typename boost::remove_reference<Arg2>::type>::type;
		using darg2_t = typename boost::remove_const<typename boost::remove_reference<dArg2>::type>::type;
		using raw_type = typename derivative_detail::product_rule_result_helper<arg1_t, darg1_t, arg2_t, darg2_t>::type;

	public:
		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	typename result<product_rule(Arg1, dArg1, Arg2, dArg2)>::type operator()(const Arg1& a1, const dArg1& da1, const Arg2& a2, const dArg2& da2) const
	{
		using result_type = typename result<product_rule(Arg1, dArg1, Arg2, dArg2)>::type;
		derivative_detail::optimize opt;
		auto result = opt(opt(da1 * a2) + opt(a1 * da2));
		return result;
	}
};

//! Division
struct quotient_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	struct result<This(Arg1, dArg1, Arg2, dArg2)>
	{
	private:
		using termA_t = decltype(std::declval<dArg1>() * std::declval<Arg2>());
		using otermA_t = typename boost::result_of<derivative_detail::optimize(termA_t)>::type;
		using termB_t = decltype(std::declval<dArg2>() * std::declval<Arg1>());
		using otermB_t = typename boost::result_of<derivative_detail::optimize(termB_t)>::type;

		using num_t = typename boost::result_of<derivative_detail::optimize(decltype(std::declval<otermA_t>() - std::declval<otermB_t>()))>::type;
		using denom_t = typename boost::result_of<derivative_detail::optimize(decltype(std::declval<Arg2>() * std::declval<Arg2>()))>::type;
		using raw_type = decltype(std::declval<num_t>() / std::declval<denom_t>());
	public:

		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Arg1, typename dArg1, typename Arg2, typename dArg2>
	typename result<quotient_rule(Arg1, dArg1, Arg2, dArg2)>::type operator()(const Arg1& a1, const dArg1& da1, const Arg2& a2, const dArg2& da2) const
	{
		using result_type = typename result<quotient_rule(Arg1, dArg1, Arg2, dArg2)>::type;
		derivative_detail::optimize opt;
		auto result = opt( opt(opt(da1 * a2) - opt(da2 * a1)) / opt(a2 * a2) );
		return result;
	}
};

namespace derivative_detail
{
	template <typename Fn, typename Arg, typename dArg>
	struct chain_rule_result_helper;

	template <int Exp, typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<Exp>, Arg, dArg>
	{
		using type = decltype(static_cast<double>(Exp) * pow<Exp - 1>(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			return static_cast<double>(Exp) * pow<Exp - 1>(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<2>, Arg, dArg>
	{
		using type = decltype(2 * std::declval<Arg>() * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			return 2 * arg * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<pow_fun<1>, Arg, dArg>
	{
		using type = dArg;

		static type apply(const Arg& arg, const dArg& darg)
		{
			return darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<cos_fun, Arg, dArg>
	{
		using type = decltype( -1.0 * sin( std::declval<Arg>() ) * std::declval<dArg>() );

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::sin;
			return -1.0 * sin(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<sin_fun, Arg, dArg>
	{
		using type = decltype(cos(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::cos;
			return cos(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<exp_fun, Arg, dArg>
	{
		using type = decltype(exp(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::exp;
			return exp(arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<tan_fun, Arg, dArg>
	{
		using type = decltype(sec(std::declval<Arg>()) * sec(std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::cos;
			auto r = cos(arg);
			return (1.0 / (r * r)) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<log_fun, Arg, dArg>
	{
		using type = decltype( (1.0 / std::declval<Arg>()) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::log;
			return (1.0 / arg) * darg;
		}
	};

	template <typename Arg, typename dArg>
	struct chain_rule_result_helper<sqrt_fun, Arg, dArg>
	{
		using type = decltype((0.5 / sqrt(std::declval<Arg>())) * std::declval<dArg>());

		static type apply(const Arg& arg, const dArg& darg)
		{
			using std::sqrt;
			return (0.5 / sqrt(arg)) * darg;
		}
	};

}//! namespace derivative_detail;

//! Chain rule
struct chain_rule : boost::proto::callable
{
	template <typename Sig>
	struct result;

	template <typename This, typename Fn, typename Arg, typename dArg>
	struct result<This(Fn, Arg, dArg)>
	{
	private:
		using fn_t = typename boost::remove_const<typename boost::remove_reference<Fn>::type>::type;
		using arg_t = typename boost::remove_const<typename boost::remove_reference<Arg>::type>::type;
		using darg_t = typename boost::remove_const<typename boost::remove_reference<dArg>::type>::type;
		using raw_type = typename derivative_detail::chain_rule_result_helper<fn_t, arg_t, darg_t>::type;

	public:

		using type = typename boost::result_of<derivative_detail::optimize(raw_type)>::type;
	};

	template <typename Fn, typename Arg, typename dArg>
	typename result<chain_rule(Fn, Arg, dArg)>::type operator() (const Fn&, const Arg& arg, const dArg& darg) const
	{
		auto result = derivative_detail::optimize()(derivative_detail::chain_rule_result_helper<Fn, Arg, dArg>::apply(arg, darg));
		//boost::proto::display_expr(result, std::cout);
		return result;
	}
};

struct get_value
	: boost::proto::when<boost::proto::terminal<boost::proto::_>, boost::proto::_value>
{};

struct derivative_grammar
	: boost::proto::switch_<struct cases_>
{};

struct cases_
{
	template <typename Tag, int D=0>
	struct case_ : boost::proto::not_<boost::proto::_> {};

	template <int D>
	struct case_<boost::proto::tag::terminal, D>
		: boost::proto::or_
		  <
			boost::proto::when
			<
				boost::proto::terminal<x_var>
			,	derivative_variable_rule()
			>
		  , boost::proto::when
			<
				boost::proto::terminal<boost::proto::_>
			,	constant_rule()
			>
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::function, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	chain_rule(get_value(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::divides, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	quotient_rule(boost::proto::_left, derivative_grammar(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::multiplies, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	product_rule(boost::proto::_left, derivative_grammar(boost::proto::_left), boost::proto::_right, derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::plus, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	addition_rule(derivative_grammar(boost::proto::_left), derivative_grammar(boost::proto::_right))
		  >
	{};

	template <int D>
	struct case_<boost::proto::tag::minus, D>
		: boost::proto::when
		  <
			boost::proto::_
		  ,	subtraction_rule(derivative_grammar(boost::proto::_left), derivative_grammar(boost::proto::_right))
		  >
	{};
};


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

#ifndef SMOOTH__OPTIM_HPP_
#define SMOOTH__OPTIM_HPP_

/**
 * @file
 * @brief Non-linear least squares optimization on Manifolds.
 */

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <iostream>
#include <numeric>

#include "concepts.hpp"
#include "diff.hpp"
#include "internal/lmpar.hpp"
#include "internal/lmpar_sparse.hpp"
#include "internal/utils.hpp"

namespace smooth {

/**
 * @brief Optimization options.
 */
struct MinimizeOptions
{
  /// relative parameter tolerance for convergence
  double ptol{1e-6};
  /// relative function tolerance for convergence
  double ftol{1e-6};
  /// maximum number of iterations
  std::size_t max_iter{1000};
  /// solver verbosity level
  int verbosity{0};
};

/**
 * @brief Find a minimum of the non-linear least-squares problem
 *
 * \f[
 *  \min_{x} \sum_i \| f(x)_i \|^2
 * \f]
 *
 * @tparam Diff differentiation method to use in solver (see diff::Type in diff.hpp)
 * @param f residuals to minimize
 * @param x reference tuple of arguments to f
 * @param opts solver options
 *
 * All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<diff::Type Diff, typename _F, typename _Wrt>
void minimize(_F && f, _Wrt && x, const MinimizeOptions & opts = MinimizeOptions{})
{
  // evaluate residuals and jacobian at initial point
  auto [r, J] = diff::dr<Diff>(f, x);

  // extract some properties from jacobian
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<decltype(J)>, decltype(J)>;
  static constexpr int Nx = decltype(J)::ColsAtCompileTime;
  const int nx            = J.cols();

  // scaling parameters
  Eigen::Matrix<double, Nx, 1> d(nx);
  if constexpr (is_sparse) {
    d = (Eigen::Matrix<double, 1, -1>::Ones(J.rows()) * J.cwiseProduct(J)).cwiseSqrt().transpose();
  } else {
    d = J.colwise().stableNorm().transpose();
  }

  // ensure scaling parameters are non-zero
  for (auto i = 0u; i != d.size(); ++i) {
    if (d[i] == 0) { d[i] = 1; }
  }

  double r_norm = r.stableNorm();
  double Delta = 100. * d.stableNorm();  // TODO for Rn arguments we should multiply with norm(x)

  for (auto i = 0u; i != opts.max_iter; ++i) {
    // calculate step a via LM parameter algorithm
    Eigen::Matrix<double, Nx, 1> a(nx);
    double lambda;
    if constexpr (is_sparse) {
      std::tie(lambda, a) = detail::lmpar_sparse(J, d, r, Delta);
    } else {
      std::tie(lambda, a) = detail::lmpar(J, d, r, Delta);
    }

    // evaluate function and jacobian at x + a
    auto x_plus_a               = utils::tuple_plus(x, a);
    const auto [r_cand, J_cand] = diff::dr<Diff>(f, x_plus_a);

    const double r_cand_norm = r_cand.stableNorm();
    const double Da_norm     = d.cwiseProduct(a).stableNorm();

    // calculate actual to predicted reduction
    const double act_red  = 1. - Eigen::numext::abs2(r_cand_norm / r_norm);
    const double fra2     = Eigen::numext::abs2((J * a).stableNorm() / r_norm);
    const double fra3     = Eigen::numext::abs2(std::sqrt(lambda) * Da_norm / r_norm);
    const double pred_red = fra2 + 2. * fra3;
    const double rho      = act_red / pred_red;

    // update trust region following Moré (1978)
    if (rho < 0.25) {
      double mu;
      if (r_cand_norm <= r_norm) {
        mu = 0.5;
      } else if (r_cand_norm <= 10 * r_norm) {
        const double gamma = -fra2 - fra3;
        mu                 = std::clamp(gamma / (2. * gamma + act_red), 0.1, 0.5);
      } else {
        mu = 0.1;
      }
      Delta *= mu;
    } else if ((lambda == 0 && rho < 0.75) || rho > 0.75) {
      Delta = 2 * Da_norm;
    }

    //// TAKE STEP IF SUCCESSFUL ////

    if (rho > 1e-4) {
      x      = x_plus_a;
      r      = r_cand;
      J      = J_cand;
      r_norm = r_cand_norm;

      // update scaling
      if constexpr (is_sparse) {
        d = d.cwiseMax((Eigen::Matrix<double, 1, -1>::Ones(J.rows()) * J.cwiseProduct(J))
                         .cwiseSqrt()
                         .transpose());
      } else {
        d = d.cwiseMax(J.colwise().stableNorm().transpose());
      }
    }

    //// PRINT STATUS ////

    // TODO Pretty-print solver steps
    if (opts.verbosity > 0) { std::cout << "Step " << i << ": " << r.sum() << std::endl; }

    //// CHECK FOR CONVERGENCE ////

    // function tolerance
    if (std::abs(act_red) < opts.ftol && pred_red < opts.ftol && rho <= 2.) { break; }

    // parameter tolerance
    // TODO a.size() should be norm(x) for non-angle states
    if (Da_norm < opts.ptol * a.size()) { break; }
  }
}

/**
 * @brief Find a minimum of the non-linear least-squares problem
 *
 * \f[
 *  \min_{x} \sum_i \| f(x)_i \|^2
 * \f]
 *
 * @param f residuals to minimize
 * @param x reference tuple of arguments to f
 * @param opts solver options
 *
 * All arguments in x as well as the return type \f$f(x)\f$ must satisfy
 * the Manifold concept.
 */
template<typename _F, typename _Wrt>
void minimize(_F && f, _Wrt && x, const MinimizeOptions & opts = MinimizeOptions{})
{
  minimize<diff::Type::DEFAULT>(std::forward<_F>(f), std::forward<_Wrt>(x), opts);
}

}  // namespace smooth

#endif  // SMOOTH__OPTIM_HPP_


#pragma once

#include <boost/intrusive/list.hpp>
#include <boost/intrusive/slist.hpp>
#include <boost/intrusive/options.hpp>
#include "litiv/3rdparty/sospd/submodular-functions.hpp"

namespace sospd {

    enum class UBfn {
        chen,
        cvpr14,
    };

    enum class FlowAlgorithm {
        bidirectional,
        source,
        parametric
    };

    struct SubmodularIBFSParams {
        SubmodularIBFSParams() = default;
        SubmodularIBFSParams(sospd::FlowAlgorithm _alg) : alg(_alg) {}
        sospd::FlowAlgorithm alg = sospd::FlowAlgorithm::bidirectional;
        sospd::UBfn ub = sospd::UBfn::cvpr14;
        std::vector<bool> fixedVars;
    };

    template<typename ValueType, typename IndexType>
    class SubmodularIBFS;

    template<typename ValueType, typename IndexType>
    class FlowSolver {
    public:
        FlowSolver() = default;
        virtual ~FlowSolver() = default;
        virtual void Solve(SubmodularIBFS<ValueType,IndexType>* energy) = 0;
        FlowSolver(const FlowSolver&) = delete;
        FlowSolver(FlowSolver&&) = delete;
        FlowSolver& operator=(const FlowSolver&) = delete;
        FlowSolver& operator=(FlowSolver&&) = delete;
    };

    template<typename ValueType, typename IndexType>
    std::unique_ptr<FlowSolver<ValueType,IndexType>> GetSolver(const SubmodularIBFSParams& params);

    /** Graph structure and algorithm for sum-of-submodular IBFS
     */
    template<typename ValueType, typename IndexType>
    class SoSGraph {
        static_assert(std::is_arithmetic<ValueType>::value,"value type must be arithmetic");
        static_assert(std::is_integral<IndexType>::value,"index type must be integral");
        public:
            typedef ValueType REAL;
            typedef IndexType NodeId;
            typedef IndexType CliqueId;
            typedef std::vector<CliqueId> NeighborList;
            enum class NodeState : char {
                S, T, S_orphan, T_orphan, N
            };
            class IBFSEnergyTableClique;
            typedef std::tuple<sospd::UBfn,std::string,sospd::UpperBoundFunction<ValueType,IndexType>> UBParam;
            static const std::vector<UBParam> ubParamList;

            SoSGraph()
                : m_num_nodes(0),
                s(NodeId(-1)),
                t(NodeId(-1)),
                m_num_cliques(0)
            { }

            /** Add n new nodes to the base set V
             *
             * \return Index of first created node
             */
            NodeId AddNode(IndexType n = 1);

            /** Add weights to s-i and i-t edges, respectively
             */
            void AddTerminalWeights(NodeId n, REAL sCap, REAL tCap);

            /** Zero the capacities on the s-i and i-t edges
             */
            void ClearTerminals();

            // Add Clique defined by nodes and energy table given
            IBFSEnergyTableClique& AddClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energyTable);

            /* Clique: abstract base class for user-defined clique functions
             *
             * Clique stores the list of nodes associated with a clique.
             * Actual functionality is provided by the user writing a derived
             * class with Clique as the base, and which implements the
             * ComputeEnergy and ExchangeCapacity functions
             */
            class Clique {
                public:
                typedef std::vector<NodeId> NodeVec;
                Clique() : m_nodes(), m_alpha_Ci() { }
                Clique(const NodeVec& nodes)
                    : m_nodes(nodes),
                    m_alpha_Ci(nodes.size(), 0)
                { }
                ~Clique() = default;

                // Returns the energy of the given labeling for this clique function
                virtual REAL ComputeEnergy(const std::vector<int>& labels) const = 0;

                const NodeVec& Nodes() const { return m_nodes; }
                IndexType Size() const { return IndexType(m_nodes.size()); }
                std::vector<REAL>& AlphaCi() { return m_alpha_Ci; }
                const std::vector<REAL>& AlphaCi() const { return m_alpha_Ci; }
                IndexType GetIndex(NodeId i) const {
                    return IndexType(std::find(this->m_nodes.begin(), this->m_nodes.end(), i) - this->m_nodes.begin());
                }

                protected:
                NodeVec m_nodes; // The list of nodes in the clique
                std::vector<REAL> m_alpha_Ci; // The reparameterization variables for this clique

            };
            /*
             * IBFSEnergyTableClique: stores energy as a list of 2^k values for each subset
             */
            class IBFSEnergyTableClique : public Clique {
                public:
                    typedef uint32_t Assignment;

                    IBFSEnergyTableClique() : Clique(), m_energy(), m_alpha_energy(), m_min_tight_set() { }
                    IBFSEnergyTableClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energy)
                        : Clique(nodes),
                        m_energy(energy),
                        m_alpha_energy(energy),
                        m_min_tight_set(nodes.size(), (1u << nodes.size()) - 1)
                    {
                        ASSERT(nodes.size() <= 31);
                    }

                    virtual REAL ComputeEnergy(const std::vector<int>& labels) const;
                    REAL ComputeAlphaEnergy(const std::vector<int>& labels) const;
                    REAL ExchangeCapacity(IndexType u_idx, IndexType v_idx) const;
                    bool NonzeroCapacity(IndexType u_idx, IndexType v_idx) const;
                    void NormalizeEnergy(std::vector<REAL>& psi, REAL& constantTerm);

                    void Push(IndexType u_idx, IndexType v_idx, REAL delta);
                    void ComputeMinTightSets();
                    std::vector<REAL>& EnergyTable() { return m_energy; }
                    const std::vector<REAL>& EnergyTable() const { return m_energy; }
                    std::vector<REAL>& AlphaEnergy() { return m_alpha_energy; }
                    const std::vector<REAL>& AlphaEnergy() const { return m_alpha_energy; }

                    void ResetAlpha();

                protected:
                    std::vector<REAL> m_energy;
                    std::vector<REAL> m_alpha_energy;
                    std::vector<Assignment> m_min_tight_set;

            };
            struct ArcIterator {
                NodeId source;
                typename NeighborList::iterator cIter;
                IndexType cliqueIdx;
                IndexType cliqueSize;
                SoSGraph* graph;

                bool operator!=(const ArcIterator& a) {
                    return (cIter != a.cIter) || (cliqueIdx != a.cliqueIdx);
                }
                bool operator==(const ArcIterator& a) {
                    return !(*this != a);
                }
                bool operator<(const ArcIterator& a) {
                    ASSERT(source == a.source);
                    return (cIter == a.cIter) ? (source < a.source) : (cIter < a.cIter);
                }

                ArcIterator& operator++() {
                    //ASSERT(*cIter < static_cast<int>(graph->m_cliques.size()));
                    cliqueIdx++;
                    if (cliqueIdx == cliqueSize) {
                        cliqueIdx = IndexType(0);
                        cIter++;
                        if (cIter != graph->m_neighbors[source].end())
                            cliqueSize = graph->m_cliques[*cIter].Size();
                        else
                            cliqueSize = IndexType(0);
                    }
                    //ASSERT(cIter == graph->m_neighbors[source].end() || *cIter < static_cast<int>(graph->m_cliques.size()));
                    //ASSERT(cIter == graph->m_neighbors[source].end() || cliqueIdx < static_cast<int>(graph->m_cliques[*cIter].Nodes().size()));
                    return *this;
                }
                NodeId Source() const {
                    return source;
                }
                NodeId Target() const {
                    //ASSERT(*cIter < static_cast<int>(graph->m_cliques.size()));
                    //ASSERT(cliqueIdx < static_cast<int>(graph->m_cliques[*cIter].Nodes().size()));
                    return graph->m_cliques[*cIter].Nodes()[cliqueIdx];
                }
                IndexType SourceIdx() const { return graph->m_cliques[*cIter].GetIndex(source); }
                IndexType TargetIdx() const { return cliqueIdx; }
                CliqueId cliqueId() const { return *cIter; }
                ArcIterator Reverse() const {
                    auto newSource = Target();
                    auto newCIter = std::find(graph->m_neighbors[newSource].begin(), graph->m_neighbors[newSource].end(), *cIter);
                    auto newCliqueIdx = graph->GetCliques()[*newCIter].GetIndex(source);
                    return {newSource, newCIter, newCliqueIdx, graph->m_cliques[*newCIter].Size(), graph};
                }
            };

            typedef boost::intrusive::list_base_hook<boost::intrusive::link_mode<boost::intrusive::normal_link>> ListHook;
            typedef boost::intrusive::slist_base_hook<boost::intrusive::link_mode<boost::intrusive::normal_link>> OrphanListHook;
            struct Node : public ListHook, OrphanListHook {
                NodeId id;
                NodeState state;
                IndexType dis;
                ArcIterator parent_arc;
                NodeId parent;
                NeighborList cliques;
                explicit Node(NodeId _id)
                    : id(_id)
                    , state(NodeState::N)
                    , dis(std::numeric_limits<IndexType>::max())
                    , parent_arc()
                    , parent()
                    , cliques() { }
            };

            typedef boost::intrusive::list<Node> NodeQueue;
            typedef boost::intrusive::slist<Node, boost::intrusive::base_hook<OrphanListHook>, boost::intrusive::cache_last<true>> OrphanList;

            ArcIterator ArcsBegin(NodeId i) {
                auto cIter = m_neighbors[i].begin();
                if (cIter == m_neighbors[i].end())
                    return ArcsEnd(i);
                return {i, cIter, 0, m_cliques[*cIter].Size(), this};
            }
            ArcIterator ArcsEnd(NodeId i) {
                auto& neighborList = m_neighbors[i];
                return {i, neighborList.end(), 0, 0, this};
            }

            typedef std::vector<IBFSEnergyTableClique> CliqueVec;

            NodeId NumNodes() const { return m_num_nodes; }
            NodeId GetS() const { return s; }
            NodeId GetT() const { return t; }
            Node& node(NodeId i) { return m_nodes[i]; }
            const Node& node(NodeId i) const { return m_nodes[i]; }
            IBFSEnergyTableClique& clique(CliqueId c) { return m_cliques[c]; }
            const IBFSEnergyTableClique& clique(CliqueId c) const { return m_cliques[c]; }
            const std::vector<REAL>& GetC_si() const { return m_c_si; }
            const std::vector<REAL>& GetC_it() const { return m_c_it; }
            const std::vector<REAL>& GetPhi_si() const { return m_phi_si; }
            const std::vector<REAL>& GetPhi_it() const { return m_phi_it; }
            CliqueId GetNumCliques() const { return m_num_cliques; }
            const CliqueVec& GetCliques() const { return m_cliques; }
            CliqueVec& GetCliques() { return m_cliques; }
            const std::vector<NeighborList>& GetNeighbors() const { return m_neighbors; }
            std::vector<Node>& GetNodes() { return m_nodes; }
            const std::vector<Node>& GetNodes() const { return m_nodes; }

            REAL ResCap(const ArcIterator& arc, bool forwardArc);
            bool NonzeroCap(const ArcIterator& arc, bool forwardArc);
            void Push(ArcIterator& arc, bool forwardArc, REAL delta);

            void ResetFlow();
            typedef void(*BoundFn)(int, const std::vector<REAL>&, std::vector<REAL>&);
            struct NormStats {
                double L1 = 0;
                double L2 = 0;
                double LInfty = 0;
            };
            template<BoundFn fn>
            void UpperBoundCliques(const std::vector<bool>& fixedVars, NormStats* stats);
            void UpperBoundCliques(sospd::UBfn ub, NormStats* stats = 0);
            void UpperBoundCliques(sospd::UBfn ub, const std::vector<bool>& fixedVars, const std::vector<int>& labels, NormStats* stats = 0);

            NodeId m_num_nodes;
            NodeId s,t;
            std::vector<REAL> m_c_si;
            std::vector<REAL> m_c_it;
            std::vector<REAL> m_phi_si;
            std::vector<REAL> m_phi_it;

            CliqueId m_num_cliques;
            CliqueVec m_cliques;
            std::vector<NeighborList> m_neighbors;

        protected:
            std::vector<Node> m_nodes;
    };

} // namespace sospd

template<typename V, typename I>
const std::vector<typename sospd::SoSGraph<V,I>::UBParam> sospd::SoSGraph<V,I>::ubParamList = {
    UBParam{ sospd::UBfn::chen, "chen", sospd::ChenUpperBound<REAL> },
    UBParam{ sospd::UBfn::cvpr14, "cvpr14", sospd::UpperBoundCVPR14<REAL> },
};

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::NodeId sospd::SoSGraph<V,I>::AddNode(I n) {
    ASSERT(n >= I(1));
    ASSERT(s == I(-1));
    NodeId first_node = m_num_nodes;
    for(I i = 0; i < n; ++i) {
        m_nodes.push_back(Node(m_num_nodes));
        m_c_si.push_back(0);
        m_c_it.push_back(0);
        m_phi_si.push_back(0);
        m_phi_it.push_back(0);
        m_neighbors.push_back(NeighborList());
        m_num_nodes++;
    }
    return first_node;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::AddTerminalWeights(NodeId n, REAL sCap, REAL tCap) {
    m_c_si[n] += sCap;
    m_c_it[n] += tCap;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::ClearTerminals() {
    for (NodeId i = 0; i < m_num_nodes; ++i) {
        m_c_si[i] = m_c_it[i] = 0;
        m_phi_si[i] = m_phi_it[i] = 0;
    }
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::IBFSEnergyTableClique& sospd::SoSGraph<V,I>::AddClique(const std::vector<NodeId>& nodes, const std::vector<REAL>& energyTable) {
    ASSERT(s == I(-1));
    m_cliques.emplace_back(nodes, energyTable);
    for (NodeId i : nodes) {
        ASSERT(0 <= i && i < m_num_nodes);
        m_neighbors[i].push_back(m_num_cliques);
    }
    return m_cliques[m_num_cliques++];
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::ResetFlow() {
    // Initialize source, sink (only do once)
    if (s == I(-1)) {
        s = m_num_nodes; t = m_num_nodes + 1;
        m_nodes.push_back(Node(s));
        m_nodes.push_back(Node(t));
    }
    // reset distance, state and parent
    for (I i = 0; i < m_num_nodes + 2; ++i) {
        Node& node = m_nodes[i];
        node.dis = std::numeric_limits<I>::max();
        node.state = NodeState::N;
        node.parent = i;
        m_phi_si[i] = m_phi_it[i] = 0;
    }

    // Reset Clique parameters
    for (I cid = 0; cid < m_num_cliques; ++cid) {
        auto& c = m_cliques[cid];
        c.ResetAlpha();
        c.ComputeMinTightSets();
    }

}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::ResCap(const ArcIterator& arc, bool forwardArc) {
    ASSERT(arc.cliqueId() >= 0 && arc.cliqueId() < I(m_cliques.size()));
    if (forwardArc)
        return m_cliques[arc.cliqueId()].ExchangeCapacity(arc.SourceIdx(), arc.TargetIdx());
    else
        return m_cliques[arc.cliqueId()].ExchangeCapacity(arc.TargetIdx(), arc.SourceIdx());
}

template<typename V, typename I>
inline bool sospd::SoSGraph<V,I>::NonzeroCap(const ArcIterator& arc, bool forwardArc) {
    if (forwardArc)
        return m_cliques[arc.cliqueId()].NonzeroCapacity(arc.SourceIdx(), arc.TargetIdx());
    else
        return m_cliques[arc.cliqueId()].NonzeroCapacity(arc.TargetIdx(), arc.SourceIdx());
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::NormalizeEnergy(std::vector<REAL>& psi, REAL& constantTerm) {
    ASSERT(false /* Should not be calling this function*/);
    const I n = I(this->m_nodes.size());
    ASSERT(sospd::CheckSubmodular((int)n,m_energy));
    const Assignment num_assignments = 1u << n;
    REAL allOnes = m_energy[num_assignments - 1];
    constantTerm += allOnes;
    psi.resize(n);
    Assignment assgn = num_assignments - 1; // The all 1 assignment
    for (I i = 0; i < n; ++i) {
        Assignment next_assgn = assgn ^ (1u << i);
        psi[i] = (m_energy[assgn] - m_energy[next_assgn]);
        assgn = next_assgn;
    }

    for (Assignment a = 0; a < num_assignments; ++a) {
        m_energy[a] -= allOnes;
        for (I i = 0; i < n; ++i) {
            if (!(a & (1u << i))) m_energy[a] += psi[i];
        }
        ASSERT(m_energy[a] >= V(0));
        m_alpha_energy[a] = m_energy[a];
    }
    ComputeMinTightSets();
    ASSERT(sospd::CheckSubmodular((int)n,m_energy));
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeEnergy(const std::vector<int>& labels) const {
    Assignment assgn = 0;
    for (I i = 0; i < I(this->m_nodes.size()); ++i) {
        NodeId n = this->m_nodes[i];
        if (labels[n] == 1) {
            assgn |= 1u << i;
        }
    }
    return m_energy[assgn];
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeAlphaEnergy(const std::vector<int>& labels) const {
    Assignment assgn = 0;
    for (I i = 0; i < I(this->m_nodes.size()); ++i) {
        NodeId n = this->m_nodes[i];
        if (labels[n] == 1) {
            assgn |= 1u << i;
        }
    }
    return m_alpha_energy[assgn];
}

template<typename V, typename I>
inline typename sospd::SoSGraph<V,I>::REAL sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ExchangeCapacity(I u_idx, I v_idx) const {
    const I n = I(this->m_nodes.size());
    ASSERT(u_idx < n);
    ASSERT(v_idx < n);
    REAL min_energy = std::numeric_limits<REAL>::max();
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    const Assignment u_mask = 1u << u_idx;
    const Assignment v_mask = 1u << v_idx;
    const Assignment uv_mask = u_mask | v_mask;
    const Assignment subset_mask = bound & ~uv_mask;
    // Terrible bit-hacks to optimize the living hell out of this function
    // Iterate over all assignments without u_idx or v_idx set
    Assignment assgn = subset_mask;
    do {
        Assignment u_sep = assgn | u_mask;
        REAL energy = m_alpha_energy[u_sep];
        if (energy < min_energy) min_energy = energy;
        assgn = ((assgn - 1) & subset_mask);
    } while (assgn != subset_mask);

    return min_energy;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::Push(I u_idx, I v_idx, REAL delta) {
    ASSERT(u_idx < this->m_nodes.size());
    ASSERT(v_idx < this->m_nodes.size());
    Clique::m_alpha_Ci[u_idx] += delta;
    Clique::m_alpha_Ci[v_idx] -= delta;
    const I n = I(this->m_nodes.size());
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    const Assignment u_mask = 1u << u_idx;
    const Assignment v_mask = 1u << v_idx;
    const Assignment uv_mask = u_mask | v_mask;
    const Assignment subset_mask = bound & ~uv_mask;
    // Terrible bit-hacks to optimize the living hell out of this function
    // Iterate over all assignments without u_idx or v_idx set
    Assignment assgn = subset_mask;
    do {
        Assignment u_sep = assgn | u_mask;
        Assignment v_sep = assgn | v_mask;
        m_alpha_energy[u_sep] -= delta;
        m_alpha_energy[v_sep] += delta;
        assgn = ((assgn - 1) & subset_mask);
    } while (assgn != subset_mask);

    ComputeMinTightSets();
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ComputeMinTightSets() {
    I n = I(this->m_nodes.size());
    Assignment num_assgns = 1u << n;
    const Assignment bound = num_assgns-1;
    for (auto& a : m_min_tight_set)
        a = bound;
    for (Assignment assgn = bound-1; assgn >= 1; --assgn) {
        if (m_alpha_energy[assgn] == 0) {
            for (I i = 0; i < n; ++i) {
                //ASSERT(m_alpha_energy[m_min_tight_set[i] & assgn] == 0);
                //ASSERT(m_alpha_energy[m_min_tight_set[i] | assgn] == 0);
                if ((assgn & (1u << i)) != 0)
                    m_min_tight_set[i] = assgn;
            }
        }
    }
}

template<typename V, typename I>
inline bool sospd::SoSGraph<V,I>::IBFSEnergyTableClique::NonzeroCapacity(I u_idx, I v_idx) const {
    Assignment min_set = m_min_tight_set[u_idx];
    return (min_set & (1u << v_idx)) != 0;
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::IBFSEnergyTableClique::ResetAlpha() {
    for (auto& a : this->m_alpha_Ci) {
        a = 0;
    }
    const I n = I(this->m_nodes.size());
    const Assignment num_assignments = 1u << n;
    for (Assignment a = 0; a < num_assignments; ++a) {
        m_alpha_energy[a] = m_energy[a];
    }
}

template<typename V, typename I>
template<typename sospd::SoSGraph<V,I>::BoundFn UB>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(const std::vector<bool>& fixedVars, NormStats* stats) {
    std::vector<REAL> psi;
    //int nCliques = m_cliques.size();
    I cliquesDone = 0;
    /*
     *std::cout << "Upper Bounding Cliques: ";
     *std::cout.flush();
     */
    for (auto& c : m_cliques) {
        /*
         *if (cliquesDone % (nCliques/10) == 0) {
         *    std::cout << ".";
         *    std::cout.flush();
         *}
         */
        cliquesDone++;
        auto& newEnergy = c.AlphaEnergy();
        I k = I(c.Size());
        psi.resize(k);
        // Compute upper bound g of clique energy
        UB(k, c.EnergyTable(), newEnergy);

        if (!fixedVars.empty()) {
            I fixedSet = 0;
            for (I i = 0; i < k; ++i)
                fixedSet |= (fixedVars[c.Nodes()[i]] << i);
            sospd::ZeroMarginalSet(k, newEnergy, fixedSet);
        }

        if (stats) {
            stats->L1 += sospd::DiffL1(c.EnergyTable(), newEnergy);
            stats->L2 += sospd::DiffL2(c.EnergyTable(), newEnergy);
            stats->LInfty += sospd::DiffLInfty(c.EnergyTable(), newEnergy);
        }
        // Modify g, find psi so that g'(S) = g(S) + psi(S) >= 0
        sospd::Normalize(k, newEnergy, psi);
        /*
         *AddLinear(k, c.EnergyTable(), psi);
         */

        auto& alpha_Ci = c.AlphaCi();
        for (I i = 0; i < k; ++i) {
            alpha_Ci[i] = -psi[i];
            m_phi_it[c.Nodes()[i]] += psi[i];
        }
        c.ComputeMinTightSets();
    }
    /*
     *std::cout << "\n";
     *std::cout << "L1: " << diffL1 << "\tL2: " << diffL2 << "\tLInfty: " << diffLInfty << "\n";
     */
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(sospd::UBfn ub, NormStats* stats) {
    UpperBoundCliques(ub, std::vector<bool>{}, std::vector<int>{}, stats);
}

template<typename V, typename I>
inline void sospd::SoSGraph<V,I>::UpperBoundCliques(sospd::UBfn ub, const std::vector<bool>& fixedVars, const std::vector<int>& /*labels*/, NormStats* stats) {
    switch(ub) {
        case sospd::UBfn::chen:
            UpperBoundCliques<sospd::ChenUpperBound>(fixedVars, stats);
            break;
        case sospd::UBfn::cvpr14:
            UpperBoundCliques<sospd::UpperBoundCVPR14>(fixedVars, stats);
            break;
    }
}