/// OpenGM. Copyright (c) 2010 by Bjoern Andres and Joerg Hendrik Kappes.
///
/// This software was developed by Bjoern Andres and Joerg Hendrik Kappes.
/// Enquiries shall be directed to:
/// bjoern.andres@iwr.uni-heidelberg.de, kappes@math.uni-heidelberg.de
///
/// Author(s) of this file: Bjoern Andres
///
/// All advertising materials mentioning features or use of this software must
/// display the following acknowledgement: ``This product includes the OpenGM
/// library developed by Bjoern Andres and Joerg Hendrik Kappes. Please direct
/// enquiries concerning OpenGM to bjoern.andres@iwr.uni-heidelberg.de,
/// kappes@math.uni-heidelberg.de''.
///
/// 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.
/// - All advertising materials mentioning features or use of this software must
///   display the following acknowledgement: ``This product includes the OpenGM
///   library developed by Bjoern Andres and Joerg Hendrik Kappes. Please direct
///   enquiries concerning OpenGM to bjoern.andres@iwr.uni-heidelberg.de,
///   kappes@math.uni-heidelberg.de''.
/// - The names of the authors must not be used to endorse or promote products
///   derived from this software without specific prior written permission.
///
/// THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``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 AUTHORS 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.
///
#pragma once
#ifndef OPENGM_GRAPHCUT_HXX
#define OPENGM_GRAPHCUT_HXX

#include <queue>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/kolmogorov_max_flow.hpp>

#include "opengm/explicitfactor.hxx"
#include "opengm/graphicalmodel.hxx"
#include "opengm/adder.hxx"
#include "opengm/minimizer.hxx"
#include "opengm/inference/inference.hxx"

namespace opengm {

// Graph Cut Optimizer.
//
// This optimizer minimizes the value of a graphical model
// of the type GraphicalModel<Factor, opengm::Adder>
//
template<class Factor>
class GraphCut
: Inference<GraphicalModel<Factor, opengm::Adder>,
            opengm::Minimizer>
{
public:
    typedef Factor factor_type;
    typedef typename factor_type::space_type space_type;
    typedef typename factor_type::value_type value_type;
    typedef typename space_type::state_type state_type;
    typedef GraphicalModel<Factor, opengm::Adder> gm_type;
    enum MaxFlowAlgorithm { PUSH_RELABEL, EDMONDS_KARP, KOLMOGOROV };

    // construction
    GraphCut(const gm_type&, const MaxFlowAlgorithm& = PUSH_RELABEL);

    // query
    std::string name() const;
    const gm_type& graphicalModel() const;

    // manipulation
    void setMaxFlowAlgorithm(const MaxFlowAlgorithm&);

    // inference
    InferenceTermination infer();
    InferenceTermination arg(std::vector<state_type>&, const size_t& = 1) const;

private:
    // boost graph library
    typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> graph_traits;
    typedef graph_traits::edge_descriptor edge_descriptor;
    typedef graph_traits::vertex_descriptor vertex_descriptor;
    struct Edge {
        Edge() : capacity(value_type()), residual(value_type()), reverse(edge_descriptor()) {}
        value_type capacity;
        value_type residual;
        edge_descriptor reverse;
    };
    typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, size_t, Edge> graph_type;
    typedef typename boost::graph_traits<graph_type>::edge_iterator edge_iterator;
    typedef typename boost::graph_traits<graph_type>::out_edge_iterator out_edge_iterator;

    void constructGraph();
    void printGraph();
    void cutPushRelabel();
    void cutEdmondsKarp();
    void cutKolmogorov();

    const gm_type& gm_;
    MaxFlowAlgorithm maxFlowAlgorithm_;
    graph_type graph_;
    std::vector<state_type> state_;
};

// public interface

template<class Factor>
GraphCut<Factor>::GraphCut
(
    const typename GraphCut::gm_type& gm,
    const MaxFlowAlgorithm& maxFlowAlgorithm
)
: gm_(gm),
  maxFlowAlgorithm_(maxFlowAlgorithm)
{
    for(size_t j=0; j<gm_.space().dimension(); ++j) {
        if(gm_.space().numberOfStates(j) != 2) {
            throw std::runtime_error("This implementation of the graph cut optimizer supports only binary variables.");
        }
    }
    for(size_t j=0; j<gm_.numberOfFactors(); ++j) {
        if(gm_[j].numberOfVariables() > 2) {
            throw std::runtime_error("This implementation of the graph cut optimizer supports only factors of order <= 2.");
        }
        if(!gm_[j].isSubmodular()) {
            throw std::runtime_error("This implementation of the graph cut optimizer supports only submodular factors.");
        }
    }
}

template<class Factor>
inline std::string
GraphCut<Factor>::name() const
{
    return "GraphCut";
}

template<class Factor>
inline const typename GraphCut<Factor>::gm_type&
GraphCut<Factor>::graphicalModel() const
{
    return gm_;
}

template<class Factor>
inline void
GraphCut<Factor>::setMaxFlowAlgorithm
(
    const MaxFlowAlgorithm& maxFlowAlgorithm
)
{
    maxFlowAlgorithm_ = maxFlowAlgorithm;
}

template<class Factor>
inline InferenceTermination
GraphCut<Factor>::infer()
{
    constructGraph();
    if(maxFlowAlgorithm_ == PUSH_RELABEL) {
        cutPushRelabel();
    }
    else if(maxFlowAlgorithm_ == EDMONDS_KARP) {
        cutEdmondsKarp();
    }
    else if(maxFlowAlgorithm_ == KOLMOGOROV) {
        cutKolmogorov();
    }
    return NORMAL;
}

template<class Factor>
inline InferenceTermination
GraphCut<Factor>::arg
(
    std::vector<state_type>& arg,
    const size_t& n
) const
{
    if(n > 1) {
        return UNKNOWN;
    }
    else {
        // skip source and sink
        arg.resize(state_.size() - 2);
        for(size_t j=2; j<state_.size(); ++j) {
            arg[j-2] = state_[j];
        }
        return NORMAL;
    }
}

// private member functions

template<class Factor>
void
GraphCut<Factor>::constructGraph()
{
    graph_ = graph_type(gm_.space().dimension()+2);
    // add source and sink vertex
    for(size_t j=0; j<gm_.numberOfFactors(); ++j) {
        const factor_type& factor = gm_[j];
        if(factor.numberOfVariables() == 0) {
            // constant factors
            // ignoring
        }
        else if(factor.numberOfVariables() == 1) {
            // 1st order factor
            if(factor(0) <= factor(1)) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(0, factor.variableIndex(0)+2, graph_); // from source
                graph_[e.first].capacity = factor(1) - factor(0); // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(factor.variableIndex(0)+2, 0, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
            else {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(factor.variableIndex(0)+2, 1, graph_); // to sink
                graph_[e.first].capacity = factor(0) - factor(1); // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(1, factor.variableIndex(0)+2, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
        }
        else if(factor.numberOfVariables() == 2) {
            // 2nd order factor
            const value_type& A = factor(0,0);
            const value_type& B = factor(0,1);
            const value_type& C = factor(1,0);
            const value_type& D = factor(1,1);
            // first variabe
            if(C > A) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(0, factor.variableIndex(0)+2, graph_); // from source
                graph_[e.first].capacity = C - A; // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(factor.variableIndex(0)+2, 0, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
            else if(C < A) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(factor.variableIndex(0)+2, 1, graph_); // to sink
                graph_[e.first].capacity = A - C; // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(1, factor.variableIndex(0)+2, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
            // second variable
            if(D > C) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(0, factor.variableIndex(1)+2, graph_); // from source
                graph_[e.first].capacity = D - C; // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(factor.variableIndex(1)+2, 0, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
            else if(D < C) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(factor.variableIndex(1)+2, 1, graph_); // to sink
                graph_[e.first].capacity = C - D; // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(1, factor.variableIndex(1)+2, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
            // submodular term
            if(B + C - A - D > 0) {
                // edge
                std::pair<edge_descriptor, bool> e =
                    add_edge(factor.variableIndex(0)+2, factor.variableIndex(1)+2, graph_);
                graph_[e.first].capacity = B + C - A - D; // cost
                // reverse edge
                std::pair<edge_descriptor, bool> er =
                    add_edge(factor.variableIndex(1)+2, factor.variableIndex(0)+2, graph_);
                graph_[e.first].reverse = er.first;
                graph_[er.first].reverse = e.first;
            }
        }
        else {
            // higher order factor
            throw std::runtime_error("This implementation of the graph cut optimizer does not support factors of order >2.");
        }
    }
}

template<class Factor>
void GraphCut<Factor>::cutEdmondsKarp()
{
    // compute max flow
    std::vector<boost::default_color_type> color(num_vertices(graph_));
    std::vector<edge_descriptor> pred(num_vertices(graph_));
    /*T flow = */edmonds_karp_max_flow(graph_, 0, 1,
        get(&Edge::capacity, graph_),
        get(&Edge::residual, graph_),
        get(&Edge::reverse, graph_),
        &color[0], &pred[0]
    );

    // find (s,t)-cut set
    state_ = std::vector<state_type>(num_vertices(graph_));
    for(size_t j=2; j<num_vertices(graph_); ++j) {
        if(color[j] == boost::black_color) {
            state_[j] = 0;
        }
        else if(color[j] == boost::white_color) {
            state_[j] = 1;
        }
        else {
            throw std::runtime_error("At least one vertex is labeled neither black nor white.");
        }
    }
}

template<class Factor>
void GraphCut<Factor>::cutPushRelabel()
{
    // compute max flow
    /*T flow = */push_relabel_max_flow(graph_, 0, 1,
        get(&Edge::capacity, graph_), get(&Edge::residual, graph_),
        get(&Edge::reverse, graph_), get(boost::vertex_index_t(), graph_));

    // find (s,t)-cut set
    state_ = std::vector<state_type>(num_vertices(graph_), 1);
    state_[0] = 0; // source
    state_[1] = 0; // sink
    typedef typename boost::property_map<graph_type, boost::vertex_index_t>::type VertexIndexMap;
    VertexIndexMap vertexIndexMap = get(boost::vertex_index, graph_);
    std::queue<vertex_descriptor> q;
    q.push(*(vertices(graph_).first)); // source
    while(!q.empty()) {
        out_edge_iterator current, end;
        tie(current, end) = out_edges(q.front(), graph_);
        q.pop();
        while(current != end) {
            if(graph_[*current].residual > 0) {
                vertex_descriptor v = target(*current, graph_);
                if(vertexIndexMap[v] > 1 && state_[vertexIndexMap[v]] == 1) {
                    state_[vertexIndexMap[v]] = 0;
                    q.push(v);
                }
            }
            ++current;
        }
    }
}

template<class Factor>
void GraphCut<Factor>::cutKolmogorov() {
    // compute max flow
    std::vector<boost::default_color_type> color(num_vertices(graph_));
    std::vector<edge_descriptor> pred(num_vertices(graph_));
    std::vector<vertex_descriptor> dist(num_vertices(graph_));
    /*T flow = */kolmogorov_max_flow(graph_,
        get(&Edge::capacity, graph_),
        get(&Edge::residual, graph_),
        get(&Edge::reverse, graph_),
        &pred[0],
        &color[0],
        &dist[0],
        get(boost::vertex_index, graph_),
        0, 1
        );

    // find (s,t)-cut set
    state_ = std::vector<state_type>(num_vertices(graph_));
    for(size_t j=2; j<num_vertices(graph_); ++j) {
        if(color[j] == boost::black_color || color[j] == boost::gray_color) {
            state_[j] = 0;
        }
        else if(color[j] == boost::white_color) {
            state_[j] = 1;
        }
    }
}

template<class Factor>
void
GraphCut<Factor>::printGraph()
{
    edge_iterator it, end;
    for(tie(it, end) = edges(graph_); it != end; ++it) {
        std::cout << '(' << source(*it, graph_) << ", " << target(*it, graph_) << "): "
            << "capacity=" << graph_[*it].capacity << ", residual=" << graph_[*it].residual << '.' << std::endl;
    }
}

} // namespace opengm

#endif // #ifndef OPENGM_GRAPHCUT_HXX


// #include "nabo/nabo.h"
#include "helpers.hpp"
#include <pcl/io/ply_io.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>
#include <omp.h>
#include <iostream>
#include <unordered_map>
#include <gdcpp.h>
#include <fstream>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>

// using namespace Nabo;
using namespace Eigen;
using namespace std;
using namespace TransformationUtilities;
using namespace InputUtilities;
using namespace InterfaceUtilities;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

std::ofstream outfile;
std::ofstream errorfile;

class Optimizer
{
    private:
        string config_file;
        vector<double> getTransVector(boost::property_tree::ptree &pt,string s);
    public:
        vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
        vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_downsampled;
        vector<MatrixXd> inverse_kinematics;
        std::vector<double> flange_transformation_initial;
        std::vector<double> flange_transformation;
        Eigen::Vector4f plane;
        Optimizer(string filename);
        Optimizer(){};
        void getInputs();
        double getError(vector<double>);
        void printError(vector<double>);
        double getError(vector<double>,vector<double>);
        unordered_map<int,int> mapping;
};

Optimizer::Optimizer(string filename)
{
    config_file = filename;
    flange_transformation = std::vector<double>(6,0);
    flange_transformation_initial = std::vector<double>(6,0);
}

vector<double> Optimizer::getTransVector(boost::property_tree::ptree &pt,string s)
{
    string angle_metric = pt.get<std::string>(s+".angle","radian");
    string camera_approx_trans_metric = pt.get<std::string>(s+".metric","m");
    double cam_approx_scale = 1.0;
    if(camera_approx_trans_metric=="cm")
        cam_approx_scale = 100.0;
    else if(camera_approx_trans_metric=="mm")
        cam_approx_scale=1000.0;
    string approx_transformation = pt.get<std::string>(s+".value","0,0,0,0,0,0");
    cout<<approx_transformation<<endl;
    vector<string> coords_str;
    boost::split(coords_str, approx_transformation , boost::is_any_of(","));
    vector<double> coords;
    for(int i=0;i<coords_str.size();i++)
        if(i<3)
            coords.push_back(stof(coords_str[i])/cam_approx_scale);
        else
        {
            double value = stof(coords_str[i]);
            if(angle_metric=="degree")
                value=degreeToRadian(value);
            coords.push_back(value);
        }
    for(auto x:coords)
        cout<<x<<" ";
    cout<<endl;
    return coords;
}

Eigen::Vector4f fitPlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
  Eigen::MatrixXd lhs (cloud->size(), 3);
  Eigen::VectorXd rhs (cloud->size());
  for (size_t i = 0; i < cloud->size(); ++i)
  {
    const auto& pt = cloud->points[i];
    lhs(i, 0) = pt.x;
    lhs(i, 1) = pt.y;
    lhs(i, 2) = 1.0;

    rhs(i) = -1.0 * pt.z;
  }
  Eigen::Vector3d params = lhs.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(rhs);
  Eigen::Vector3d normal (params(0), params(1), 1.0);
  auto length = normal.norm();
  normal /= length;
  params(2) /= length;
  return {normal(0), normal(1), normal(2), params(2)};
}

double pointToPlaneDistance(Eigen::Vector4f plane, vector<double> pt)
{
    return fabs(plane(0)*pt[0]+plane(1)*pt[1]+plane(2)*pt[2]+plane(3))/(sqrt(pow(plane(0),2)+pow(plane(1),2)+pow(plane(2),2)));
}

double pointToPlaneDistance(vector<double> plane, vector<double> pt)
{
    return fabs(plane[0]*pt[0]+plane[1]*pt[1]+plane[2]*pt[2]+plane[3])/(sqrt(pow(plane[0],2)+pow(plane[1],2)+pow(plane[2],2)));
}

string getSplit(string name, string character,int id)
{
    vector<string> values;
    boost::split(values,name,boost::is_any_of(character));
    if(id<0)
        id = values.size()+id;
    return values[id];
}

int getFileId(string filename)
{
    string file = getSplit(filename,"/",-1);
    file = getSplit(file,".",0);
    int number = stoi(getSplit(file,"_",1));
    return number;
}

void Optimizer::getInputs()
{
    ifstream file;
    std::cout<<"Config File: "<<config_file<<endl;
    file.open(config_file);
    using boost::property_tree::ptree;
    ptree pt;
    read_xml(file, pt);
    string camera_metric = pt.get<std::string>("data.camera.metric","m");
    int counter = 0;
    for (const auto &cloud : pt.get_child("data.camera.clouds"))
    {
        string filename = cloud.second.data();
        int cloud_id = getFileId(filename);
        cout<<"Cloud Number: "<<cloud_id<<endl;
        mapping[counter++] = cloud_id-1;
        PointCloudT::Ptr temp_cloud_t(new PointCloudT);
        InputUtilities::readPointCloud(filename,temp_cloud_t,camera_metric);
        PointCloudT::Ptr temp_cloud(new PointCloudT);
        for(auto pts:temp_cloud_t->points)
            if(pts.z<=0.8)
                temp_cloud->points.push_back(pts);
        // ifstream file(filename);
        // string line;
        // for(int c=0;c<14&&getline(file,line);c++);
        // PointCloudT::Ptr pointcloud(new PointCloudT);
        // while(getline(file,line))
        // {
        //     vector<string> values_from_file;
        //     boost::split(values_from_file, line, boost::is_any_of(" "));
        //     pcl::PointXYZRGB pt;
        //     pt.x = stof(values_from_file[0]);
        //     pt.y = stof(values_from_file[1]);
        //     pt.z = stof(values_from_file[2]);
        //     pt.r = stof(values_from_file[3]);
        //     pt.g = stof(values_from_file[4]);
        //     pt.b = stof(values_from_file[5]);
        //     pointcloud->points.push_back(pt);
        // }
        // PointCloudT::Ptr temp_cloud(new PointCloudT);
        // for(int i=0;i<pointcloud->points.size();i++)
        // {
        //     auto pt = pointcloud->points[i];
        //     if(pt.x==0.0&&pt.y==0.0&&pt.z==0.0)
        //         continue;
        //     temp_cloud->points.push_back(pt);
        // }
        clouds.push_back(temp_cloud);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_bw_temp(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::copyPointCloud(*temp_cloud,*cloud_bw_temp);
        pcl::VoxelGrid<pcl::PointXYZ> sor;
        sor.setInputCloud (cloud_bw_temp);
        constexpr double leaf = 0.02f;
        sor.setLeafSize (leaf, leaf, leaf);
        sor.filter (*cloud_filtered);
        cout<<"Filtered Clouds Size: "<<cloud_filtered->points.size()<<endl;
        cloud_downsampled.push_back(cloud_filtered);
    }
    string ik_filename  = pt.get<std::string>("data.camera.transformations.inverse_kinematics.location");
    cout<<ik_filename<<endl;
    string transformation_metric = pt.get<std::string>("data.camera.transformations.inverse_kinematics.metric","m");
    inverse_kinematics = readTransformations(ik_filename,true,transformation_metric);
    // string touch_points_file = pt.get<std::string>("data.plane.file","");
    // pcl::PointCloud<pcl::PointXYZ>::Ptr touch_points(new pcl::PointCloud<pcl::PointXYZ>);
    // ifstream file_h(touch_points_file);
    // string line;
    // while(getline(file_h,line)&&line.size())
    // {
    //     vector<string> v;
    //     split(v,line,boost::is_any_of(","));
    //     pcl::PointXYZ pt;
    //     pt.x = stof(v[0])/1000.0;
    //     pt.y = stof(v[1])/1000.0;
    //     pt.z = stof(v[2])/1000.0;
    //     touch_points->points.push_back(pt);
    // }
    // cout<<"Size of touch points: "<<touch_points->points.size()<<endl;
    // plane = fitPlane(touch_points);
    // cout<<"Plane Equation: "<<endl;
    // for(int i=0;i<4;i++)
        // cout<<plane(i)<<" ";
    // cout<<endl;
    // cout<<"Transformations Read"<<endl;
    auto transformation_initial_flange= getTransVector(pt,"data.camera.transformations.approximate_transformation");
    for(int i=0;i<transformation_initial_flange.size();i++)
    {
        flange_transformation[i]=transformation_initial_flange[i];
        flange_transformation_initial[i]=transformation_initial_flange[i];
    }

    cout<<"Plane Equation 2: "<<endl;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_combined (new pcl::PointCloud<pcl::PointXYZ>);
    int cl = 0;
    for(auto cloud:clouds)
    {
        for(int i=0;i<cloud->points.size();i++)
        {
            pcl::PointXYZ pt;
            pt.x = cloud->points[i].x;
            pt.y = cloud->points[i].y;
            pt.z = cloud->points[i].z;
            if(pt.z>0.8)
                continue;

            Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(flange_transformation);
            Eigen::MatrixXd transformation_temp = inverse_kinematics[mapping[cl]]*cam_T_flange;
            Eigen::Affine3d transformation;
            for(int a=0;a<3;a++)
                for(int b=0;b<4;b++)
                    transformation(a,b) = transformation_temp(a,b);

            float src[3];
            float out[3];
            src[0] = pt.x;
            src[1] = pt.y;
            src[2] = pt.z;
            apply_transformation_optimized(src,out,transformation);
            pt.x = out[0];
            pt.y = out[1];
            pt.z = out[2];
            cloud_combined->points.push_back(pt);
        }
        cl++;
    }
    auto new_plane = fitPlane(cloud_combined);
    plane = new_plane;
    for(int i=0;i<4;i++)
        cout<<new_plane(i)<<" ";
    cout<<endl;
}

double Optimizer::getError(vector<double> transformation)
{
    double error = 0.0;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    // vector<double> error_vec(clouds.size(),0);
    for(int j=0;j<clouds.size();j++)
    {
        double average = 0.0;
        double error_mx = -1e9;
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(transformation);
        Eigen::MatrixXd transformation = inverse_kinematics[mapping[j]]*cam_T_flange;
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        for(int i=0;i<cloud_downsampled[j]->points.size();i++)
        {
            auto pt = cloud_downsampled[j]->points[i];
            float src[3];
            float out[3];
            src[0] = pt.x;
            src[1] = pt.y;
            src[2] = pt.z;
            apply_transformation_optimized(src,out,trans);
            double distance = pointToPlaneDistance(plane,{out[0],out[1],out[2]});
            average+=distance;
            if(distance>error_mx)
                error_mx = distance;
        }
        error+=average/cloud_downsampled[j]->points.size();
        // error+=error_mx;
    }
    return error/clouds.size();
}
double Optimizer::getError(vector<double> transformation,vector<double> plane_local)
{
    double error = 0.0;
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    // vector<double> error_vec(clouds.size(),0);
    for(int j=0;j<clouds.size();j++)
    {
        double average = 0.0;
        double error_mx = -1e9;
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(transformation);
        Eigen::MatrixXd transformation = inverse_kinematics[mapping[j]]*cam_T_flange;
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        for(int i=0;i<cloud_downsampled[j]->points.size();i++)
        {
            auto pt = cloud_downsampled[j]->points[i];
            float src[3];
            float out[3];
            src[0] = pt.x;
            src[1] = pt.y;
            src[2] = pt.z;
            apply_transformation_optimized(src,out,trans);
            double distance = pointToPlaneDistance(plane_local,{out[0],out[1],out[2]});
            average+=distance;
            if(distance>error_mx)
                error_mx = distance;
        }
        error+=average/cloud_downsampled[j]->points.size();
        // error+=error_mx;
    }
    return error/clouds.size();
}
void Optimizer::printError(vector<double> transformation)
{
    Eigen::MatrixXd pts=Eigen::MatrixXd::Zero(1,3);
    for(int j=0;j<clouds.size();j++)
    {
        double average = 0.0;
        double error_mx = -1e9;
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(transformation);
        Eigen::MatrixXd transformation = inverse_kinematics[mapping[j]]*cam_T_flange;
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        for(int i=0;i<cloud_downsampled[j]->points.size();i++)
        {
            auto pt = cloud_downsampled[j]->points[i];
            float src[3];
            float out[3];
            src[0] = pt.x;
            src[1] = pt.y;
            src[2] = pt.z;
            apply_transformation_optimized(src,out,trans);
            double distance = pointToPlaneDistance(plane,{out[0],out[1],out[2]});
            average+=distance;
            if(distance>error_mx)
                error_mx = distance;
        }
        // error+=average/cloud_downsampled[j]->points.size();
        errorfile<<"Downsampled No: "<<j<<" Error Avg: "<<average/cloud_downsampled[j]->points.size()*1000.0<<" Errom Max: "<<error_mx*1000.0<<endl;
    }

    for(int j=0;j<clouds.size();j++)
    {
        double average = 0.0;
        double error_mx = -1e9;
        Eigen::MatrixXd cam_T_flange = vectorToTransformationMatrix(transformation);
        Eigen::MatrixXd transformation = inverse_kinematics[mapping[j]]*cam_T_flange;
        Eigen::Affine3d trans;
        for(int a=0;a<3;a++)
            for(int b=0;b<4;b++)
                trans(a,b) = transformation(a,b);
        for(int i=0;i<clouds[j]->points.size();i++)
        {
            auto pt = clouds[j]->points[i];
            float src[3];
            float out[3];
            src[0] = pt.x;
            src[1] = pt.y;
            src[2] = pt.z;
            apply_transformation_optimized(src,out,trans);
            double distance = pointToPlaneDistance(plane,{out[0],out[1],out[2]});
            average+=distance;
            if(distance>error_mx)
                error_mx = distance;
        }
        // error+=average/cloud_downsampled[j]->points.size();
        errorfile<<"Real No: "<<j<<" Error Avg: "<<average/clouds[j]->points.size()*1000.0<<" Errom Max: "<<error_mx*1000.0<<endl;
    }
    errorfile<<"--------------------------------------------"<<endl;
}

Optimizer opti;

void gradientDescent()
{
    struct Functor
    {
        Functor()
        {
        }

        double operator()(const Eigen::VectorXd &xval, Eigen::VectorXd &) const
        {
            vector<double> trans(6);
            for(int i=0;i<6;i++){
                trans[i]=xval(i);
            }
            return opti.getError(trans);
        }
    };
    gdc::GradientDescent<double, Functor,
        gdc::WolfeBacktracking<double>> optimizer;

    optimizer.setMaxIterations(10000);
    optimizer.setMinGradientLength(1e-6);
    optimizer.setMinStepLength(1e-6);
    optimizer.setMomentum(0.4);
    optimizer.setVerbosity(4);
    Eigen::VectorXd initialGuess = Eigen::VectorXd::Zero(6);
    for(int i=0;i<6;i++)
    {
        initialGuess(i) = opti.flange_transformation[i];
    }
    cout<<"Plane Value"<<endl;
    for(int i=0;i<4;i++)
        cout<<opti.plane(i)<<" ";
    cout<<endl;
    auto result = optimizer.minimize(initialGuess);
    std::cout << "Done! Converged: " << (result.converged ? "true" : "false")
        << " Iterations: " << result.iterations << std::endl;
    std::cout << "Final fval: " << result.fval << std::endl;
    std::cout << "Final xval: " << result.xval.transpose() << std::endl;
    std::cout<<"---------------------------------------------------------"<<endl;
    outfile<<"Gradient Descent on Flange Transformation..."<<endl;
    outfile<<"Iterations: "<<result.iterations<<" Converged: "<<(result.converged ? "true" : "false")<<" Final fval: "<<result.fval<<endl;
    outfile<<"Flange Transformation"<<endl;
    auto result_flange_trans = result.xval.transpose().head(6);
    for(int i=0;i<5;i++)
        outfile<<result_flange_trans(i)<<", ";
    outfile<<result_flange_trans(5)<<endl;
    outfile<<"Plane Equation"<<endl;
    for(int i=0;i<3;i++)
        outfile<<opti.plane(i)<<", ";
    outfile<<opti.plane(3)<<endl;
    for(int i=0;i<6;i++)
        opti.flange_transformation[i] = result_flange_trans(i);
    opti.printError({result_flange_trans(0),result_flange_trans(1),result_flange_trans(2),result_flange_trans(3),result_flange_trans(4),result_flange_trans(5)});
}
void gradientDescentWithPlane()
{
    struct Functor
    {
        Functor()
        {
        }

        double operator()(const Eigen::VectorXd &xval, Eigen::VectorXd &) const
        {
            vector<double> trans(6);
            vector<double> plane(4);
            for(int i=0;i<6;i++){
                trans[i]=xval(i);
            }
            for(int i=6;i<10;i++)
                plane[i-6]=xval(i);
            return opti.getError(trans,plane);
        }
    };
    gdc::GradientDescent<double, Functor,
        gdc::WolfeBacktracking<double>> optimizer;

    optimizer.setMaxIterations(10000);
    optimizer.setMinGradientLength(1e-6);
    optimizer.setMinStepLength(1e-6);
    optimizer.setMomentum(0.4);
    optimizer.setVerbosity(4);
    Eigen::VectorXd initialGuess = Eigen::VectorXd::Zero(10);
    for(int i=0;i<6;i++)
    {
        initialGuess(i) = opti.flange_transformation[i];
    }
    cout<<"Plane Value"<<endl;
    for(int i=0;i<4;i++)
        cout<<opti.plane(i)<<" ";
    cout<<endl;
    for(int i=0;i<4;i++)
        initialGuess(i+6) = opti.plane(i);
    auto result = optimizer.minimize(initialGuess);
    std::cout << "Done! Converged: " << (result.converged ? "true" : "false")
        << " Iterations: " << result.iterations << std::endl;
    std::cout << "Final fval: " << result.fval << std::endl;
    std::cout << "Final xval: " << result.xval.transpose() << std::endl;
    std::cout<<"---------------------------------------------------------"<<endl;
    outfile<<"Gradient Descent on Plane..."<<endl;
    outfile<<"Iterations: "<<result.iterations<<" Converged: "<<(result.converged ? "true" : "false")<<" Final fval: "<<result.fval<<endl;
    outfile<<"Flange Transformation"<<endl;
    auto result_flange_trans = result.xval.transpose().head(6);
    for(int i=0;i<5;i++)
        outfile<<result_flange_trans(i)<<", ";
    outfile<<result_flange_trans(5)<<endl;
    outfile<<"Plane Equation"<<endl;
    auto result_plane = result.xval.transpose().tail(4);
    for(int i=0;i<3;i++)
        outfile<<result_plane(i)<<", ";
    outfile<<result_plane(3)<<endl;
    opti.printError({result_flange_trans(0),result_flange_trans(1),result_flange_trans(2),result_flange_trans(3),result_flange_trans(4),result_flange_trans(5)});
}

void discreteCombinatorialOptimization()
{
    vector<double> transformation = opti.flange_transformation;
    cout<<"True: "<<opti.getError(transformation);
    vector<double> transformation_best = opti.flange_transformation;
    double error_min = 1e9;
    double t_min = -20, t_max = 20, r_min = -5, r_max = 5;
    for(double x = t_min;x<=t_max;x+=4)
        for(double y = t_min;y<=t_max;y+=4)
            for(double z = t_min;z<=t_max;z+=4)
                for(double zt=r_min;zt<=r_max;zt++)
                    for(double yt=r_min;yt<=r_max;yt++)
                        for(double xt=r_min;xt<=r_max;xt++)
                        {
                            vector<double> temp_transformation = {transformation[0]+x/1000.0,transformation[1]+y/1000.0,transformation[2]+z/1000.0,transformation[3]+degreeToRadian(zt),transformation[4]+degreeToRadian(yt),transformation[5]+degreeToRadian(xt)};
                            double total_error = opti.getError(temp_transformation);
                            if(total_error<error_min)
                            {
                                cout<<"Error: "<<total_error<<endl;
                                transformation_best = temp_transformation;
                                error_min = total_error;
                            }
                        }
    cout<<"Best Transformation:"<<endl;
    for(auto x:transformation_best)
        cout<<x<<" ";
    cout<<endl;
    outfile<<"Flange Transformation From Discrete Optimization"<<endl;
    for(int i=0;i<5;i++)
        outfile<<transformation_best[i]<<", ";
    outfile<<transformation_best[5]<<endl;
    opti.flange_transformation = transformation_best;
}

int main(int argc, char** argv)
{
    if(argc<2)
    {
        std::cout<<"Usage: optimizer_test <config file>"<<endl;
        exit(-1);
    }
    outfile.open("results.txt", std::ios_base::app); // append instead of overwrite
    string config_filename = argv[1];
    errorfile.open("new_experiments_errors.txt", std::ios_base::app); // append instead of overwrite
    config_filename = argv[1];
    outfile<<"Results: "<<config_filename<<endl;
    errorfile<<"Results: "<<config_filename<<endl;
    opti = Optimizer(config_filename);
    opti.getInputs();
    cout<<"Starting optimization"<<endl;
    discreteCombinatorialOptimization();
    gradientDescent();
    gradientDescentWithPlane();
    outfile<<"------------------------------------------------------"<<endl;
    return 0;
}


/************************************************************************/
/*                                                                      */
/*               Copyright 2007-2019 by Hans Meine                      */
/*                                                                      */
/*    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.                                   */
/*                                                                      */
/************************************************************************/

#include "mydebug.hxx"
#include "debugimage.hxx"
#include "crop.hxx"

#include "foureightsegmentation.hxx"
#include "cellconfigurations.hxx"

#include <boost/format.hpp>

#include <iostream>
#include <algorithm>
#include <set>

#if !defined(NDEBUG) && !defined(_MSC_VER)
#  warning Consistency checks will be done after every Euler operation!
#endif

namespace vigra {

namespace cellimage {

#ifndef NDEBUG
inline void validateDart(const GeoMap::DartTraverser &dart)
{
    vigra_precondition(dart.neighborCirculator().center()->type() == CellTypeVertex,
                       "dart is not attached to a node");
    vigra_precondition(dart.startNode().initialized(),
                       "dart's startNode is not valid (initialized())");
    if(!dart.isSingular())
        vigra_precondition(dart.edge().initialized(),
                           "dart's edge is not valid (initialized())");
}
#else
// separate def. to prevent compiler warning due to unused param:
inline void validateDart(const GeoMap::DartTraverser &) {}
#endif

struct FindMaxLabel
{
    CellLabel maxLabel_;

    FindMaxLabel() : maxLabel_(0) {}

    void operator()(const CellPixel &p)
    {
        if(p.label() > maxLabel_)
            maxLabel_ = p.label();
    }

    CellLabel operator()() const
    { return maxLabel_; }
};

GeoMap::GeoMap(const CellImage &importImage)
: initialized_(false)
{
    cellImage.resize(importImage.size());
    copyImage(srcImageRange(importImage), destImage(cellImage));

    cells = cellImage.upperLeft() + Diff2D(2, 2);

    FindMaxLabel findMaxNodeLabel;
    inspectImageIf(srcImageRange(cellImage),
                   maskImage(cellImage, CellTypeEquals<CellTypeVertex>()),
                   findMaxNodeLabel);
    CellLabel maxNodeLabel = findMaxNodeLabel();
    std::cerr << "  found maxNodeLabel: " << maxNodeLabel << "\n";

    FindMaxLabel findMaxEdgeLabel;
    inspectImageIf(srcImageRange(cellImage),
                   maskImage(cellImage, CellTypeEquals<CellTypeLine>()),
                   findMaxEdgeLabel);
    CellLabel maxEdgeLabel = findMaxEdgeLabel();
    std::cerr << "  found maxEdgeLabel: " << maxEdgeLabel << "\n";

    FindMaxLabel findMaxFaceLabel;
    inspectImageIf(srcImageRange(cellImage),
                   maskImage(cellImage, CellTypeEquals<CellTypeRegion>()),
                   findMaxFaceLabel);
    CellLabel maxFaceLabel = findMaxFaceLabel();
    std::cerr << "  found maxFaceLabel: " << maxFaceLabel << "\n";

    nodeCount_ = edgeCount_ = faceCount_ = 0;

//    if(maxNodeLabel+maxEdgeLabel+maxFaceLabel>25000)
//        std::cerr << maxNodeLabel/nodeCount_;

    std::cerr << "  initializing nodeList..\n";
    initNodeList(maxNodeLabel);
    std::cerr << "  initializing edgeList..\n";
    initEdgeList(maxEdgeLabel);

    std::cerr << "  creating temporary contourImage..\n";
    BImage contourImage(cellImage.size());
    contourImage = 1;
    initImageIf(destImageRange(contourImage),
                maskImage(cellImage, CellTypeEquals<CellTypeRegion>()),
                0);
    std::cerr << "  initializing faceList..\n";
    initFaceList(contourImage, maxFaceLabel);

    initialized_ = true;
}

unsigned int GeoMap::findComponentAnchor(
    const FaceInfo &face, const DartTraverser & dart)
{
    unsigned int result = 0;
    const ContourComponents &contours = face.contours;

    if(contours.size() == 1)
        return result;

    if(dart.isSingular())
    {
        // look for startNodeLabel
        for(ConstContourComponentsIterator contour= contours.begin();
            contour != contours.end(); ++contour, ++result)
            if(contour->startNodeLabel() == dart.startNodeLabel())
                return result;
    }
    else
    {
        // look for edgeLabel
        for(ConstContourComponentsIterator contour= contours.begin();
            contour != contours.end(); ++contour, ++result)
            if(!contour->isSingular() &&
               (contour->edgeLabel() == dart.edgeLabel()))
                return result;

        // we have to circulate through all contours now.. :-(
        result = 0;
        for(ConstContourComponentsIterator contour= contours.begin();
            contour != contours.end(); ++contour, ++result)
        {
            DartTraverser dart2(*contour);
            while(dart2.nextPhi() != *contour)
            {
                if(dart2.edgeLabel() == dart.edgeLabel())
                    return result;
            }
        }
    }

    std::cerr << "DART NOT FOUND IN CONTOURS OF FACE " << face.label
              << ": " << dart << "\n";
    result = 1;
    for(ConstContourComponentsIterator contour= contours.begin();
        contour != contours.end(); ++contour, ++result)
    {
        std::cerr << "contour " << result << ":\n";
        DartTraverser dart2(*contour);
        do {
            std::cerr << "  " << dart2 << "\n";
        } while(dart2.nextPhi() != *contour);
    }

    vigra_fail("findComponentAnchor: dart not found in any contour");
    return 0;
}

void GeoMap::removeNodeFromContours(ContourComponents &contours,
                                    CellLabel nodeLabel)
{
    // find contour anchor to be changed if it points to this edge
    for(ContourComponentsIterator contour= contours.begin();
        contour != contours.end(); ++contour)
        if(contour->startNodeLabel() == nodeLabel)
            contour->nextPhi();
}

GeoMap::FaceInfo &GeoMap::removeIsolatedNode(const DartTraverser & dart)
{
    //std::cerr << "removeIsolatedNode(" << dart << ")\n";
    validateDart(dart);
    vigra_precondition(dart.isSingular(),
                       "removeIsolatedNode: node is not singular");

    NodeInfo &node= dart.startNode();
    FaceInfo &face= dart.leftFace();

    face.contours.erase(face.contours.begin() + findComponentAnchor(face, dart));

    for(CellScanIterator it= nodeScanIterator(node.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeRegion, face.label);

    // updating bounds not necessary since all of the node's neighbors
    // are already face pixels, so the bounds should not change
    face.size += node.size;

    node.uninitialize();
    --nodeCount_;
#ifndef NDEBUG
    try
    {
        checkConsistency();
    }
    catch(vigra::StdException &)
    {
        std::cerr << "OPERATION: removeIsolatedNode(" << dart << ")\n";
        throw;
    }
#endif
    return face;
}

GeoMap::FaceInfo &GeoMap::mergeFaces(const DartTraverser & dart)
{
    //std::cerr << "mergeFaces(" << dart << ")\n";
    validateDart(dart);
    // merge smaller face into larger one:
    DartTraverser removedDart = dart;
    if(dart.leftFace().bounds.area() < dart.rightFace().bounds.area())
        removedDart.nextAlpha();

    EdgeInfo &mergedEdge= removedDart.edge();
    FaceInfo &survivor= removedDart.leftFace();
    FaceInfo &mergedFace= removedDart.rightFace();
    NodeInfo &node1= mergedEdge.start.startNode();
    NodeInfo &node2= mergedEdge.end.startNode();

    vigra_precondition(survivor.label != mergedFace.label,
        "GeoMap::mergeFaces(): dart is singular or edge is a bridge");

    // find indices of contour components to be merged
    const unsigned int contour1(findComponentAnchor(survivor, removedDart));
    const unsigned int contour2(findComponentAnchor(mergedFace, removedDart));

    // re-use an old anchor for the merged contour
    if(survivor.contours[contour1].edgeLabel() == mergedEdge.label)
    {
        survivor.contours[contour1].nextPhi();
        if(survivor.contours[contour1].edgeLabel() == mergedEdge.label)
        {
            survivor.contours[contour1] = mergedFace.contours[contour2];
            if(survivor.contours[contour1].edgeLabel() == mergedEdge.label)
                survivor.contours[contour1].nextPhi();
        }
    }
    vigra_postcondition(
        (survivor.contours[contour1].edgeLabel() != mergedEdge.label)
        || (node1.label == node2.label),
        "did not find an anchor for merged contour!\n");

    // update contours
    for(unsigned int i = 0; i < mergedFace.contours.size(); ++i)
        if(i != contour2)
            survivor.contours.push_back(mergedFace.contours[i]);

    // relabel cells in cellImage:
    for(CellScanIterator it= edgeScanIterator(mergedEdge.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeRegion, survivor.label);
    for(CellScanIterator it= faceScanIterator(mergedFace.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeRegion, survivor.label);

    // turn node anchors if they pointed to the removed edge and
    // there's another left
    if(--node1.degree && node1.anchor.isSingular())
        node1.anchor.carefulNextSigma();
    if(--node2.degree && node2.anchor.isSingular())
        node2.anchor.carefulNextSigma();

    // update bounds and sizes:
    survivor.bounds |= mergedEdge.bounds;
    survivor.size += mergedEdge.size;
    survivor.bounds |= mergedFace.bounds;
    survivor.size += mergedFace.size;

    mergedEdge.uninitialize();
    --edgeCount_;
    mergedFace.uninitialize();
    --faceCount_;
    // FIXME: also update maxFaceLabel / maxEdgeLabel
#ifndef NDEBUG
    try
    {
        checkConsistency();
    }
    catch(vigra::StdException &)
    {
        std::cerr << "OPERATION: mergeFaces(" << dart << ")\n";
        throw;
    }
#endif
    return survivor;
}

GeoMap::FaceInfo &GeoMap::removeBridge(const DartTraverser & dart)
{
    //std::cerr << "removeBridge(" << dart << ")\n";
    validateDart(dart);
    vigra_precondition(!dart.isSingular(), "removeBridge: dart does not point to any edge");
    EdgeInfo &edge= dart.edge();
    FaceInfo &face= dart.leftFace();
    NodeInfo &node1= edge.start.startNode();
    NodeInfo &node2= edge.end.startNode();

    vigra_precondition(face.label == dart.rightFaceLabel(),
                       "GeoMap::removeBridge(): edge is not a bridge");

    // prepare new anchors
    DartTraverser newAnchor1(edge.start);
    newAnchor1.prevSigma();
    DartTraverser newAnchor2(edge.end);
    newAnchor2.prevSigma();

    const unsigned int contourIndex(findComponentAnchor(face, dart));

#ifndef NDEBUG
    vigra_invariant(
        (newAnchor1 != newAnchor2) &&
        (newAnchor1.startNodeLabel() == node1.label) &&
        (newAnchor2.startNodeLabel() == node2.label),
        "GeoMap::removeBridge(): new anchors unusable");

#endif // NDEBUG

    // update anchors
    face.contours[contourIndex] = newAnchor1;
    face.contours.push_back(newAnchor2);

    // relabel cell in cellImage:
    for(CellScanIterator it= edgeScanIterator(edge.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeRegion, face.label);

    // turn node anchors if they pointed to the removed edge and
    // there's another one left
    if(--node1.degree && node1.anchor.isSingular())
        node1.anchor.carefulNextSigma();
    if(--node2.degree && node2.anchor.isSingular())
        node2.anchor.carefulNextSigma();

    // update bounds and size:
    face.bounds |= edge.bounds;
    face.size += edge.size;

    edge.uninitialize();
    --edgeCount_;
#ifndef NDEBUG
    try
    {
        checkConsistency();
    }
    catch(vigra::StdException &)
    {
        std::cerr << "OPERATION: removeBridge(" << dart << ")\n";
        throw;
    }
#endif
    return face;
}

GeoMap::EdgeInfo &GeoMap::mergeEdges(const DartTraverser & dart)
{
    //std::cerr << "mergeEdges(" << dart << ")\n";
    // merge smaller edge (mergedEdge) into larger one (survivor):
    validateDart(dart);
    DartTraverser dart1(dart);
    dart1.nextSigma();
    bool firstIsSmaller =
        dart.edge().bounds.area() < dart1.edge().bounds.area();

    if(firstIsSmaller)
        dart1.nextSigma(); // dart1 _temporarily_ points to smaller one

    DartTraverser dart2(dart1);
    EdgeInfo &mergedEdge = dart2.edge();
    NodeInfo &node = dart1.startNode();
    dart1.nextSigma();
    EdgeInfo &survivor = dart1.edge();

    vigra_precondition((firstIsSmaller ? dart2 : dart1) == dart,
        "GeoMap::mergeEdges(): node has degree > 2");
    vigra_precondition(survivor.label != mergedEdge.label,
        "GeoMap::mergeEdges(): node has degree one or is loop");

    // update contours of neighbored faces if necessary
    removeNodeFromContours(dart1.leftFace().contours, dart1.startNodeLabel());
    if(dart1.leftFaceLabel() != dart1.rightFaceLabel())
        removeNodeFromContours(dart1.rightFace().contours, dart1.startNodeLabel());

    // update start, end
    dart1.nextAlpha();
    dart2.nextAlpha();
    survivor.start = dart1;
    survivor.end = dart2;

    // relabel cells in cellImage:
    for(CellScanIterator it= nodeScanIterator(node.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeLine, survivor.label);
    for(CellScanIterator it= edgeScanIterator(mergedEdge.label, cells, false);
        it.inRange(); ++it)
        *it= CellPixel(CellTypeLine, survivor.label);

    // update bounds:
    survivor.bounds |= node.bounds;
    survivor.size += node.size;
    survivor.bounds |= mergedEdge.bounds;
    survivor.size += mergedEdge.size;

    node.uninitialize();
    --nodeCount_;
    mergedEdge.uninitialize();
    --edgeCount_;
#ifndef NDEBUG
    try
    {
        checkConsistency();
    }
    catch(vigra::StdException &)
    {
        std::cerr << "OPERATION: mergeEdges(" << dart << ")\n";
        throw;
    }
#endif
    return survivor;
}

/********************************************************************/

GeoMap &GeoMap::deepCopy(const GeoMap &other)
{
    cellImage = other.cellImage;
    cells = cellImage.upperLeft() + Diff2D(2,2);

    nodeList_ = other.nodeList_;
    nodeCount_ = other.nodeCount_;
    edgeList_ = other.edgeList_;
    edgeCount_ = other.edgeCount_;
    faceList_ = other.faceList_;
    faceCount_ = other.faceCount_;

    for(NodeIterator it= nodesBegin(); it.inRange(); ++it)
    {
        it->anchor.reparent(this);
    }

    for(EdgeIterator it= edgesBegin(); it.inRange(); ++it)
    {
        it->start.reparent(this);
        it->end.reparent(this);
    }

    for(FaceIterator it= facesBegin(); it.inRange(); ++it)
    {
        for(ContourComponentsIterator contour= it->contours.begin();
            contour != it->contours.end(); ++contour)
        {
            contour->reparent(this);
        }
    }

    initialized_ = other.initialized_; // should be always true

    return *this;
}

/********************************************************************/

void GeoMap::checkConsistency()
{
    bool consistent = true;
    // std::cerr << "checking nodes...\n";
    for(NodeIterator it= nodesBegin(); it != nodesEnd(); ++it)
    {
        // std::cerr << "  node " << it->label << "\r";
        unsigned int realDegree = 0;
        if(it->anchor.neighborCirculator().center()->type() != CellTypeVertex)
        {
            consistent = false;
            std::cerr << "node " << it->label << "'s anchor is broken: "
					  << it->anchor << "\n";
        }
        if(!it->anchor.isSingular())
        {
            DartTraverser dart(it->anchor);
            do { ++realDegree; }
            while(dart.nextSigma() != it->anchor);
        }
        if(realDegree != it->degree)
        {
            consistent = false;
            std::cerr << "node " << it->label << " has degree stored as "
                      << it->degree << " but seems to be of degree "
                      << realDegree << "\n  anchor:"
					  << it->anchor << "\n";
            std::cerr << "  sigma-successors:\n";
            for(DartTraverser dart(it->anchor); dart.nextSigma() != it->anchor; )
            {
                std::cerr << "   " << dart << "\n";
            }
        }

        FindBoundingRectangle actualBounds;
        Point2D coord(0, 0);
        inspectCell(nodeScanIterator(it->label, coord, false), actualBounds);
        Rect2D ab(actualBounds.upperLeft, actualBounds.lowerRight);
        if(ab != it->bounds)
        {
            consistent = false;
            std::cerr << "node " << it->label << "'s bounds are stored as "
                      << it->bounds << " but seem to be " << ab << "\n";
        }
    }

    // std::cerr << "checking edges...\n";
    for(EdgeIterator it= edgesBegin(); it != edgesEnd(); ++it)
    {
        // std::cerr << "  edge " << it->label << "\r";
        FindBoundingRectangle actualBounds;
        Point2D coord(0, 0);
        inspectCell(edgeScanIterator(it->label, coord, false), actualBounds);
        Rect2D ab(actualBounds.upperLeft, actualBounds.lowerRight);
        if(ab != it->bounds)
        {
            consistent = false;
            std::cerr << "edge " << it->label << "'s bounds are stored as "
                      << it->bounds << " but seem to be " << ab << "\n";
        }
        if(it->start.neighborCirculator().center()->type() != CellTypeVertex)
        {
            consistent = false;
            std::cerr << "edge " << it->label << "'s start is broken: "
					  << it->start << "\n";
        }
        if(it->start.edgeLabel() != it->label)
        {
            consistent = false;
            std::cerr << "edge " << it->label << "'s start points to edge "
                      << it->start.edgeLabel() << "\n";
        }
        if(it->end.neighborCirculator().center()->type() != CellTypeVertex)
        {
            consistent = false;
            std::cerr << "edge " << it->label << "'s end is broken: "
					  << it->end << "\n";
        }
        if(it->end.edgeLabel() != it->label)
        {
            consistent = false;
            std::cerr << "edge " << it->label << "'s end points to edge "
                      << it->end.edgeLabel() << "\n";
        }
    }

    // std::cerr << "checking faces...\n";
    for(FaceIterator it= facesBegin(); it != facesEnd(); ++it)
    {
        // std::cerr << "  face " << it->label << "\r";
        FindBoundingRectangle actualBounds;
        Point2D coord(0, 0);
        inspectCell(faceScanIterator(it->label, coord, false), actualBounds);
        Rect2D ab(actualBounds.upperLeft, actualBounds.lowerRight);
        if(ab != it->bounds)
        {
            consistent = false;
            std::cerr << "face " << it->label << "'s bounds are stored as "
                      << it->bounds << " but seem to be " << ab << "\n";
        }

        for(ContourComponentsIterator cc(it->contours.begin());
            cc != it->contours.end(); ++cc)
        {
            if((cc->neighborCirculator().center()->type() != CellTypeVertex)
               || (cc->isSingular() && (cc->startNode().degree > 0)))
            {
                consistent = false;
                std::cerr << "face " << it->label << "'s anchor at position "
                          << (cc - it->contours.begin()) << " is broken: "
						  << *cc << "\n";
            }
            DartTraverser dart(*cc);
            dart.nextSigma();
            dart.prevSigma();
            if(dart != *cc)
            {
                consistent = false;
                std::cerr << "face " << it->label << "'s contours are broken:\n"
                          << "  anchor:  " << *cc << "\n"
						  << "  becomes: " << dart << "\n";
            }
            int maxSteps= 10000;
            do
            {
                if(dart.leftFaceLabel() != it->label)
                {
                    std::cerr << "dart.leftFace() is no longer our face!\n";
                    maxSteps = 0;
                    break;
                }
            }
            while((dart.nextPhi() != *cc) && --maxSteps);
            if(!maxSteps)
            {
                consistent = false;
                std::cerr << "face " << it->label << "'s contours are broken:\n"
                          << "  anchor: " << *cc << "\n"
						  << "  did not return after " << maxSteps << " steps: "
						  << dart << "\n";
            }

//  for(unsigned int i = 1; i < face.contours.size(); ++i)
//      for(unsigned int j = 0; j < i; ++j)
//          if(face.contours[i] == face.contours[j])
//          {
//                 consistent = false;
//                 std::cerr << "face " << face.label << "'s contours contains one dart twice:\n  ";
//              debugDart(face.contours[i]);
//          }
        }
    }

    vigra_postcondition(consistent, "consistency check failed");
}

/********************************************************************/

struct ChooseCellConfiguration
{
    CellType * cornerTypeLut_;
    static CellType preferVertex[6], preferEdge[6];

    ChooseCellConfiguration(CellType cornerType)
    : cornerTypeLut_(cornerType == CellTypeVertex ? preferVertex : preferEdge)
    {}

    CellType operator()(CellType c) const
    {
        return cornerTypeLut_[c];
    }
};

CellType ChooseCellConfiguration::preferVertex[6] = {
    CellTypeRegion, CellTypeLine, CellTypeVertex, CellTypeError, CellTypeVertex, CellTypeError };
CellType ChooseCellConfiguration::preferEdge[6] = {
    CellTypeRegion, CellTypeLine, CellTypeVertex, CellTypeError, CellTypeLine, CellTypeLine };

void GeoMap::initCellImage(BImage & contourImage, CellType cornerType)
{
    CellType cellConf[256];
    std::transform(cellConfigurations, cellConfigurations+256, cellConf,
                   ChooseCellConfiguration(cornerType));

    CellPixel regionPixel(CellTypeRegion, 0);

    BImage::traverser rawLine = contourImage.upperLeft();
    CellImage::traverser cellLine = cellImage.upperLeft();
    for(int y=-2; y < cellImage.height()-2; ++y, ++rawLine.y, ++cellLine.y)
    {
        BImage::traverser raw = rawLine;
        CellImage::traverser cell = cellLine;
        for(int x=-2; x < cellImage.width()-2; ++x, ++raw.x, ++cell.x)
        {
            if(*raw == 0)
            {
                *cell = regionPixel;
            }
            else
            {
                vigra::NeighborhoodCirculator<BImage::traverser, EightNeighborCode>
                    neighbors(raw, EightNeighborCode::SouthEast);
                vigra::NeighborhoodCirculator<BImage::traverser, EightNeighborCode>
                    end = neighbors;
                int conf = 0;
                do
                {
                    conf = (conf << 1) | *neighbors;
                }
                while(--neighbors != end);

                if(cellConf[conf] == CellTypeError)
                {
                    debugImage(crop(srcImageRange(contourImage),
                                    Rect2D(x, y, x+5, y+5)),
                               std::cerr);
                    vigra_precondition(0, (boost::format(
                        "GeoMap::init(): Configuration at (%1%, %2%) must be thinned further "
                        "(found configuration %3%)") % x % y % conf).str());
                }

                cell->setType(cellConf[conf], 0);
            }
        }
    }

    // FIXME: this special-handling of the boundary is only necessary
    // because the containment hierarchy is determined in a strange
    // way (faceList_[0].anchor is directly initialized from this
    // position)
    cellImage(1, 1).setType(CellTypeVertex, 0);
#ifndef NDEBUG
    std::cerr << "*** RUN-TIME WARNING: slowness results from missing NDEBUG flag during compilation of foureightsegmentation.cxx! ***\n";
#endif
}

/********************************************************************/

CellLabel GeoMap::labelNodes()
{
    BImage nodeImage(cellImage.size());
    BImage::traverser nodes = nodeImage.upperLeft() + Diff2D(2,2);

    for(int y=-2; y < cellImage.height()-2; ++y)
    {
        CellImage::traverser cell = cells + Diff2D(-2, y);

        for(int x=-2; x < cellImage.width()-2; ++x, ++cell.x)
        {
            if(cell->type() != CellTypeVertex)
            {
                nodes(x,y) = 0;
            }
            else
            {
                nodes(x,y) = 1;
#if 0
                // test for forbidden configuration
                CellImageEightCirculator n(cell);
                CellImageEightCirculator nend = n;

                do
                {
                    vigra_precondition((n->type() != CellTypeLine || n[1].type() || CellTypeLine),
                                       (boost::format("labelNodes(): Node at (%1%, %2%) has two "
                                                      "incident edgels from the same edge (direction: %3%)")
                                        % x % y % (n - nend)).str());
                }
                while(++n != nend);
#endif
            }
        }
    }

    return labelImageWithBackground(
        srcImageRange(nodeImage),
        destImage(cellImage, LabelWriter<CellTypeVertex>()), true, 0);
}

/********************************************************************/

CellLabel GeoMap::labelEdges(CellLabel maxNodeLabel)
{
    std::vector<bool> nodeProcessed(maxNodeLabel + 1, false);

    CellLabel maxEdgeLabel = 0;

    for(int y=-1; y < cellImage.height()-3; ++y)
    {
        CellImage::traverser cell = cells + Diff2D(-1, y);

        for(int x=-1; x < cellImage.width()-3; ++x, ++cell.x)
        {
            if(cell->type() != CellTypeVertex)
                continue;
            if(nodeProcessed[cell->label()])
                continue;

            nodeProcessed[cell->label()] = true;

            DartTraverser dart(
                this, CellImageEightCirculator(cell, EightNeighborCode::West));
            DartTraverser dend = dart;

            if(dart.isSingular())
                continue;

            do
            {
                if(dart.edgeLabel() != 0)
                    continue;

                labelEdge(dart.neighborCirculator(), ++maxEdgeLabel);
            }
            while(dart.nextSigma() != dend);
        }
    }

    return maxEdgeLabel;
}

/********************************************************************/

CellLabel GeoMap::labelFaces(BImage & contourImage)
{
    // labelImageWithBackground() starts with label 1, so don't
    // include outer border (infinite regions shall have label 0)
    return labelImageWithBackground(
        srcIterRange(contourImage.upperLeft() + Diff2D(1,1),
                     contourImage.lowerRight() - Diff2D(1,1)),
        destIter(cellImage.upperLeft() + Diff2D(1,1),
                 LabelWriter<CellTypeRegion>()),
        false, // eight_neighbors = false -> 4-neighborhood
        1);    // background: contour value
}

/********************************************************************/

void GeoMap::labelSelfLoops(CellLabel & maxNodeLabel, CellLabel & maxEdgeLabel)
{
    for(int y=-1; y < cellImage.height()-3; ++y)
    {
        CellImage::traverser cell = cells + Diff2D(-1, y);

        for(int x=-1; x < cellImage.width()-3; ++x, ++cell.x)
        {
            if(cell->label() != 0)
                continue;

            // found a circle (not labeled by previous steps)

            // mark first point as node
            (*cell) = CellPixel(CellTypeVertex, ++maxNodeLabel);

            CellImageEightCirculator circ(cell);
            CellImageEightCirculator circEnd = circ;

            do
            {
                if(circ->type() != CellTypeLine)
                    continue;
                if(circ->label() != 0)
                    continue;

                labelEdge(circ, ++maxEdgeLabel);
            }
            while(++circ != circEnd);
        }
    }
}

/********************************************************************/

void GeoMap::labelEdge(CellImageEightCirculator circ,
                       CellLabel newLabel)
{
    EdgelIterator edge(circ);

    // follow the edge and relabel it
    for(; !edge.atEnd(); ++edge)
    {
        edge->setLabel(newLabel, CellTypeLine);
    }
}

/********************************************************************/

struct HoleRemover
{
    const CellPixel &p_;
    mutable unsigned int size_;

    HoleRemover(const CellPixel &p) : p_(p), size_(0) {}

    const CellPixel &operator()(const CellPixel &) const
    {
        ++size_;
        return p_;
    }

    unsigned int size() const
    {
        return size_;
    }
};

void GeoMap::initNodeList(CellLabel maxNodeLabel)
{
    nodeList_.resize(maxNodeLabel + 1);
    std::vector<unsigned int> crackCirculatedAreas(maxNodeLabel + 1, 0);

    for(Point2D pos= Point2D(-1, -1); pos.y < cellImage.height()-3; ++pos.y)
    {
        CellImage::traverser cell = cells + Diff2D(-1, pos.y);

        for(pos.x=-1; pos.x < cellImage.width()-3; ++pos.x, ++cell.x)
        {
            if(cell->type() != CellTypeVertex)
                continue;

            CellLabel nodeLabel = cell->label();
            vigra_precondition(nodeLabel < nodeList_.size(),
                               "nodeList_ must be large enough");

            if(!nodeList_[nodeLabel].initialized())
            {
                nodeList_[nodeLabel].label = nodeLabel;
                ++nodeCount_;

                nodeList_[nodeLabel].degree = 0;
                nodeList_[nodeLabel].anchor = DartTraverser(
                    this, CellImageEightCirculator(cell,
                                                   EightNeighborCode::West));
                if(!nodeList_[nodeLabel].anchor.isSingular())
                {
                    DartTraverser dart(nodeList_[nodeLabel].anchor);
                    do
                    {
                        ++nodeList_[nodeLabel].degree;
                    }
                    while(dart.nextSigma() != nodeList_[nodeLabel].anchor);
                }

                // calculate area from following the outer contour of the node
                CrackContourCirculator<CellImage::traverser> crack(cell);
                CrackContourCirculator<CellImage::traverser> crackend(crack);
                do
                {
                    crackCirculatedAreas[nodeLabel] += crack.diff().x * crack.pos().y -
                                                   crack.diff().y * crack.pos().x;
                }
                while(++crack != crackend);

                crackCirculatedAreas[nodeLabel] /= 2;
            }

            nodeList_[nodeLabel].bounds |= pos;
            ++nodeList_[nodeLabel].size;
        }
    }

    for(NodeIterator node= nodesBegin(); node.inRange(); ++node)
    {
        // methods to calculate the area must yield identical values
        if(crackCirculatedAreas[node->label] != node->size)
        {
//             std::cerr << "GeoMap::initNodeList(): "
//                       << "Node " << node->label << " has a "
//                       << (crackCirculatedAreas[node->label] - node->size)
//                       << "-pixel hole, stuffing..\n";

            CellImage::traverser bul(cells + node->bounds.upperLeft());
            CellImage::traverser blr(cells + node->bounds.lowerRight());

            CellImage::traverser anchor(bul);
            CellPixel nodePixel(CellTypeVertex, node->label);
            while(*anchor != nodePixel)
                ++anchor.x;

            std::set<CellLabel> outerRegions;

            CrackContourCirculator<CellImage::traverser> crack(anchor);
            CrackContourCirculator<CellImage::traverser> crackend(crack);
            do
            {
                if(crack.outerPixel()->type() == CellTypeRegion)
                    outerRegions.insert(crack.outerPixel()->label());
            }
            while(++crack != crackend);

//             std::cerr << "found " << outerRegions.size() << " outer regions:\n";
//             for(std::set<CellLabel>::iterator it= outerRegions.begin();
//                 it != outerRegions.end(); ++it)
//             {
//                 std::cerr << *it << ", ";
//             }
//             std::cerr << "\n";
//             debugImage(crop(srcIterRange(cells, cells), node->bounds),
//                        std::cerr, 4);

            for(bul = cells + node->bounds.upperLeft(); bul.y < blr.y; ++bul.y)
            {
                for(anchor= bul; anchor.x < blr.x-1; ++anchor.x)
                {
                    if(*anchor == nodePixel)
                    {
                        ++anchor.x;
                        if((anchor->type() == CellTypeRegion) &&
                           !outerRegions.count(anchor->label()))
                        {
//                             std::cerr << "found hole label: " << *anchor;
                            HoleRemover holeRemover(nodePixel);
                            transformImageIf(crop(srcIterRange(cells, cells), node->bounds),
                                             crop(maskIter(cells, CellMask(*anchor)), node->bounds),
                                             crop(destIter(cells), node->bounds),
                                             holeRemover);

                            node->size += holeRemover.size();
//                             std::cerr << " (" << holeRemover.size() << " pixels)\n";

                            if(crackCirculatedAreas[node->label] == node->size)
                                goto HolesStuffed; // double break
                        }
                        else
                            --anchor.x;
                    }
                }
            }

        HolesStuffed:
            ;
        }
    }
}

/********************************************************************/

void GeoMap::initEdgeList(CellLabel maxEdgeLabel)
{
    edgeList_.resize(maxEdgeLabel + 1);

    for(NodeIterator n = nodesBegin(); n.inRange(); ++n)
    {
        DartTraverser dart = n->anchor;
        if(dart.isSingular())
            continue;
        DartTraverser dartEnd = dart;

        int debugCount = 0;
        do
        {
            CellLabel label = dart.edgeLabel();
            vigra_precondition(label < edgeList_.size(),
                               "edgeList_ must be large enough");
            if(!edgeList_[label].initialized())
            {
                edgeList_[label].label = label;
                ++edgeCount_;
                edgeList_[label].start = dart;
                edgeList_[label].end = dart;
                edgeList_[label].end.nextAlpha();
                // correct size and bounds will be collected by initFaceList()
                edgeList_[label].size = 0;
            }

            if(++debugCount == 40)
            {
                std::cerr << "node " << n->label << " has degree > 40??\n"
                          << "  bounds: " << n->bounds << "\n";
                Rect2D dr(n->bounds);
                dr |= Point2D(dartEnd.neighborCirculator().base() - cells);
                dr |= Point2D(dart.neighborCirculator().base() - cells);
                std::cerr << "debugging rect " << dr << " including "
                          << Point2D(dartEnd.neighborCirculator().base() - cells) << "\n";
                debugImage(crop(srcIterRange(cells, cells), dr),
                           std::cerr, 4);
                std::cerr << "dartEnd: " << dartEnd << "\n";
                std::cerr << "dart:    " << dart << "\n";
            }
        }
        while(dart.nextSigma() != dartEnd);
    }
}

/********************************************************************/

void GeoMap::initFaceList(BImage & contourImage, CellLabel maxFaceLabel)
{
    faceList_.resize(maxFaceLabel + 1);

    IImage contourLabelImage(cellImage.size());
    contourLabelImage = 0;
    int contourComponentsCount =
        labelImageWithBackground(srcImageRange(contourImage),
                                 destImage(contourLabelImage), true, 0);
    IImage::traverser contourLabel =
        contourLabelImage.upperLeft() + Diff2D(2, 2);

    std::vector<bool> contourProcessed(contourComponentsCount + 1, false);

    // process outer face
    faceList_[0].label= 0;
    ++faceCount_;
    faceList_[0].bounds |= Point2D(-2, -2);
    faceList_[0].bounds |= Point2D(-2, -2) + cellImage.size() - Diff2D(1, 1);
    faceList_[0].size = NumericTraits<unsigned int>::max();
    DartTraverser anchor(this, CellImageEightCirculator(cells + Diff2D(-1, -1),
                                                        EightNeighborCode::West));
    faceList_[0].contours.push_back(anchor.prevSigma());
    contourProcessed[contourLabel(-1, -1)] = true;

    for(Point2D pos= Point2D(-1, -1); pos.y < cellImage.height()-3; ++pos.y)
    {
        pos.x= -1;
        CellImage::traverser cell = cells + pos;
        CellImage::traverser leftNeighbor = cell - Diff2D(1, 0);

        for(; pos.x < cellImage.width()-3; ++pos.x, ++cell.x, ++leftNeighbor.x)
        {
            if(cell->type() != CellTypeRegion)
            {
                if(cell->type() == CellTypeLine)
                {
                    edgeList_[cell->label()].bounds |= pos;
                    edgeList_[cell->label()].size += 1;
                }
                continue;
            }

            CellLabel faceLabel = cell->label();
            vigra_precondition(faceLabel < faceList_.size(),
                               "faceList_ must be large enough");

            if(!faceList_[faceLabel].initialized())
            {
                //std::cerr << "found face " << faceLabel << " at " << pos.x << "," << pos.y << "\n";
                faceList_[faceLabel].label = faceLabel;
                ++faceCount_;

                // determine anchor (either via node or via edge):
                if(leftNeighbor->type() == CellTypeVertex)
                {
                    DartTraverser anchor(this,
                                         CellImageEightCirculator(leftNeighbor));
                    anchor.prevSigma();

                    vigra_invariant(anchor.leftFaceLabel() == faceLabel,
                                    "GeoMap::initFaceList()");

                    faceList_[faceLabel].contours.push_back(anchor);
                }
                else
                {
                    vigra_precondition(leftNeighbor->type() == CellTypeLine,
                                       "leftNeighbor expected to be an edge");

                    CellLabel edgeIndex = leftNeighbor->label();

                    vigra_precondition(edgeList_[edgeIndex].initialized(),
                                       (boost::format("EdgeInfo %1% expected to be initialized") % edgeIndex).str());

                    DartTraverser anchor = edgeList_[edgeIndex].start;
                    if(anchor.leftFaceLabel() != faceLabel)
                        anchor.nextAlpha();

                    vigra_invariant(anchor.leftFaceLabel() == faceLabel,
                                    "GeoMap::initFaceList()");

                    faceList_[faceLabel].contours.push_back(anchor);
                }
            }
            else // faceLabel seen before
            {
                // look for inner contours
                CellImageEightCirculator neighbor(cell);
                CellImageEightCirculator nend = neighbor;

                do
                {
                    int contourIndex = contourLabel[neighbor.base() - cells];
                    if(contourIndex == 0 || contourProcessed[contourIndex])
                        continue;

                    // found an inner contour
                    contourProcessed[contourIndex] = true;

                    // find incident node
                    if(neighbor->type() == CellTypeVertex)
                    {
                        // this is the node
                        CellImageEightCirculator n = neighbor;
                        n.swapCenterNeighbor();

                        DartTraverser anchor(this, n);
                        anchor.prevSigma();

                        vigra_invariant(anchor.leftFaceLabel() == faceLabel,
                                        "GeoMap::initFaceList()");

                        faceList_[faceLabel].contours.push_back(anchor);
                    }
                    else
                    {
                        vigra_precondition(neighbor->type() == CellTypeLine,
                                           "neighbor expected to be an edge");

                        CellLabel edgeIndex = neighbor->label();

                        vigra_precondition(edgeList_[edgeIndex].initialized(),
                                           (boost::format("EdgeInfo %1% should be initialized") % edgeIndex).str());

                        DartTraverser anchor = edgeList_[edgeIndex].start;
                        if(anchor.leftFaceLabel() != faceLabel)
                            anchor.nextAlpha();

                        vigra_invariant(anchor.leftFaceLabel() == faceLabel,
                                        "GeoMap::initFaceList()");

                        faceList_[faceLabel].contours.push_back(anchor);
                    }
                }
                while(++neighbor != nend);
            }

            faceList_[faceLabel].bounds |= pos;
            ++faceList_[faceLabel].size;
        }
    }
}

std::ostream &
operator<<(std::ostream & out,
		   const vigra::cellimage::GeoMap::DartTraverser & d)
{
    static const char * const dirStr[] = {
        " E",
        "NE",
        " N",
        "NW",
        " W",
        "SW",
        " S",
        "SE"
    };

    vigra::Point2D pos(d.neighborCirculator().center() -
                       d.segmentation()->cells);

    out << "DartTraverser["
        << dirStr[(int)d.neighborCirculator().direction()]
        << " from " << pos.x << ", " << pos.y;

    bool singularDart = false;
    if(d.neighborCirculator().center()->type() == CellTypeVertex)
    {
        CellLabel nodeLabel(d.neighborCirculator().center()->label());
        out << " (node " << nodeLabel;
        if(nodeLabel > d.segmentation()->maxNodeLabel())
            out << ", LABEL INVALID!";
        else
        {
            const GeoMap::NodeInfo &node(d.segmentation()->node(nodeLabel));
            if(!node.initialized())
                out << ", UNINITIALIZED";
            out << ", " << node.size << " pixels, deg. " << node.degree;
            singularDart = (node.degree == 0);
        }
        out << ")";
    }
    else
    {
        if(d.neighborCirculator().center()->type() == CellTypeLine)
            out << " (*EDGE* ";
        else
            out << " (*FACE* ";
        out << d.neighborCirculator().center()->label() << ")";
    }

    out << " to "; /*****************************************************/

    if(d.neighborCirculator().base()->type() == CellTypeLine)
    {
        CellLabel edgeLabel(d.neighborCirculator().base()->label());
        out << "edge " << edgeLabel;
        if(edgeLabel > d.segmentation()->maxEdgeLabel())
            out << ", LABEL INVALID!";
        else
        {
            const GeoMap::EdgeInfo &edge(d.segmentation()->edge(edgeLabel));
            if(!edge.initialized())
                out << ", UNINITIALIZED";
            out << ", " << edge.size << " pixels";
        }
    }
    else
    {
        if(d.neighborCirculator().base()->type() == CellTypeVertex)
            out << "*NODE* ";
        else
            out << (singularDart ? "face " : "*FACE* ");
        out << d.neighborCirculator().base()->label();
    }

    out << "]";
    return out;
}

} // namespace cellimage

} // namespace vigra


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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.
 */

#include "transposition_based_synthesis.hpp"

#include <limits.h>

#include <iostream>
#include <iterator>   // for std::distance
#include <string>

#include <boost/assign/std/set.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/dynamic_bitset.hpp>

#include <core/functor.hpp>
#include <core/utils/timer.hpp>

#include <reversible/circuit.hpp>
#include <reversible/gate.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/functions/add_circuit.hpp>
#include <reversible/functions/copy_metadata.hpp>
#include <reversible/functions/transposition_to_circuit.hpp>
#include <reversible/io/read_realization.hpp>
#include <reversible/io/print_circuit.hpp>

#include "synthesis_utils_p.hpp"

using namespace boost::assign;

namespace cirkit
{

  bool transposition_based_synthesis( circuit& circ, const binary_truth_table& spec,
      properties::ptr settings, properties::ptr statistics )
  {
    // Settings parsing
    // Run-time measuring
    properties_timer t( statistics );

    unsigned bw = spec.num_outputs();
    circ.set_lines( bw );
    copy_metadata( spec, circ );

    std::map<unsigned, unsigned> values_map;

    for ( binary_truth_table::const_iterator it = spec.begin(); it != spec.end(); ++it )
    {
      binary_truth_table::cube_type in( it->first.first, it->first.second );
      binary_truth_table::cube_type out( it->second.first, it->second.second );

      values_map.insert( std::make_pair( truth_table_cube_to_number( in ), truth_table_cube_to_number( out ) ) );
    }

    // Set of cycles
    std::vector<std::vector<unsigned> > cycles;

    while ( !values_map.empty() )
    {
      unsigned start_value = values_map.begin()->first; // first key in values_map

      std::vector<unsigned> cycle;
      unsigned target = start_value;

      do
      {
        cycle += target;
        unsigned old_target = target;
        target = values_map[target];
        values_map.erase( old_target ); // erase this entry
      } while ( target != start_value );

      cycles += cycle;
    }

    for ( auto& cycle : cycles )
    {
      unsigned max_distance = 0u;
      unsigned max_index = 0u;

      for ( unsigned i = 0u; i < cycle.size(); ++i )
      {
        unsigned first = cycle.at(i);
        unsigned second = cycle.at( (i + 1u) % cycle.size() );

        unsigned distance = hamming_distance( first, second );

        if ( distance > max_distance )
        {
          max_distance = distance;
          max_index = i;
        }
      }

      std::vector<unsigned> tmp;
      std::copy( cycle.begin() + ( max_index + 1u ), cycle.end(), std::back_inserter( tmp ) );
      std::copy( cycle.begin(), cycle.begin() + ( max_index + 1u ), std::back_inserter( tmp ) );
      std::copy( tmp.begin(), tmp.end(), cycle.begin() );
      std::reverse( cycle.begin(), cycle.end() );
    }

    // TODO create transpositions function
    for ( auto& cycle : cycles )
    {
      for ( unsigned i = 0u; i < cycle.size() - 1; ++i )
      {
        circuit transposition_circ( spec.num_inputs() );

        unsigned first = cycle.at(i);
        unsigned second = cycle.at( (i + 1) % cycle.size() );

        boost::dynamic_bitset<> first_bits( spec.num_inputs(), first );
        boost::dynamic_bitset<> second_bits( spec.num_inputs(), second );

        transposition_to_circuit( transposition_circ, first_bits, second_bits );

        append_circuit( circ, transposition_circ);
      }
    }

    return true;
  }

  truth_table_synthesis_func transposition_based_synthesis_func( properties::ptr settings, properties::ptr statistics )
  {
    truth_table_synthesis_func f = [&settings, &statistics]( circuit& circ, const binary_truth_table& spec ) {
      return transposition_based_synthesis( circ, spec, settings, statistics );
    };
    f.init( settings, statistics );
    return f;
  }

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


/***************************************************************************
 * Copyright (C) 2012, Naomasa Matsubayashi
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ***************************************************************************/

#include <boost/cstdint.hpp>
#include <vector>
#include <string>
#include <boost/spirit/include/qi.hpp>
#include <boost/lexical_cast.hpp>
#include <tommath.h>

#include <hermit/mpint.hpp>

namespace hermit {
  boost::optional< std::string > is_hex( const std::string &temp ) {
    using boost::spirit::qi::parse;
    using boost::spirit::ascii::char_;
    using boost::spirit::ascii::space;
    using boost::spirit::ascii::digit;
    auto iter = temp.begin();
    std::vector< char > result;
    if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> "0x" >> +( digit|char_( "a-fA-F" ) ), result ) && iter == temp.end() )
      return std::string( result.begin(), result.end() );
    return boost::optional< std::string >();
  }
  boost::optional< std::string > is_dec( const std::string temp ) {
    using boost::spirit::qi::parse;
    using boost::spirit::ascii::char_;
    using boost::spirit::ascii::space;
    using boost::spirit::ascii::digit;
    auto iter = temp.begin();
    std::vector< char > result;
    if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> +digit, result ) && iter == temp.end() ) {
      return std::string( result.begin(), result.end() );
    }
    return boost::optional< std::string >();

  }
  boost::optional< std::string > is_oct( const std::string temp ) {
    using boost::spirit::qi::parse;
    using boost::spirit::ascii::char_;
    using boost::spirit::ascii::space;
    using boost::spirit::ascii::digit;
    auto iter = temp.begin();
    std::vector< char > result;
    if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> '0' >> +char_( "0-7" ), result ) && iter == temp.end() )
      return std::string( result.begin(), result.end() );
    return boost::optional< std::string >();
  }

  mpint::mpint( const std::string &value_ ) {
    MPINT_SAFE_CALL( mp_init( &value ) );
    mp_zero( &value );
    boost::optional< std::string > transformed = is_oct( value_ );
    if( transformed ) {
      from_string( value_, 8 );
    }
    else {
      transformed = is_hex( value_ );
      if( transformed ) {
        from_string( *transformed, 16 );
      }
      else {
        transformed = is_dec( value_ );
        if( transformed ) {
          std::string foo = *transformed;
          from_string( *transformed, 10 );
        }
        else
          throw std::runtime_error( "Invalid integer format" );
      }
    }
  }
  std::string mpint::to_string( int radix ) const {
    int length;
    MPINT_SAFE_CALL( mp_radix_size( const_cast< mp_int* >( &value ), radix, &length ) );
    std::vector< char > temp( length );
    std::fill( temp.begin(), temp.end(), 0 );
    MPINT_SAFE_CALL( mp_toradix_n( const_cast< mp_int* >( &value ), &temp.front(), radix, length ) );
    std::string str( &temp.front() );
    return str;
  }

  std::ostream &operator<<( std::ostream &stream, const mpint &mp_value ) {
    if( stream.flags() & std::ios_base::dec )
      stream << mp_value.to_string( 10 );
    else if( stream.flags() & std::ios_base::hex )
      stream << mp_value.to_string( 16 );
    else if( stream.flags() & std::ios_base::oct )
      stream << mp_value.to_string( 8 );
    else
      stream << mp_value.to_string( 10 );
    return stream;
  }

  std::istream &operator>>( std::istream &stream, mpint &mp_value ) {
    using boost::spirit::qi::parse;
    using boost::spirit::ascii::char_;
    using boost::spirit::ascii::space;
    using boost::spirit::ascii::digit;
    if( stream.flags() & std::ios_base::dec ) {
      while( stream.good() && !stream.eof() ) {
        std::string temp;
        stream >> temp;
        auto iter = temp.begin();
        std::vector< char > result;
        if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> +digit, result ) && iter == temp.end() ) {
          mp_value.from_string( temp, 10 );
          break;
        }
      }
    }
    else if( stream.flags() & std::ios_base::hex ) {
      while( stream.good() && !stream.eof() ) {
        std::string temp;
        stream >> temp;
        auto iter = temp.begin();
        std::vector< char > result;
        if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> +( digit|char_( "a-fA-F" ) ), result ) && iter == temp.end() ) {
          mp_value.from_string( temp, 16 );
          break;
        }
      }
    }
    else if( stream.flags() & std::ios_base::oct ) {
      while( stream.good() && !stream.eof() ) {
        std::string temp;
        stream >> temp;
        auto iter = temp.begin();
        std::vector< char > result;
        if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> +char_( "0-7" ), result ) && iter == temp.end() ) {
          mp_value.from_string( temp, 8 );
          break;
        }
      }
    }
    else {
      while( stream.good() && !stream.eof() ) {
        std::string temp;
        stream >> temp;
        auto iter = temp.begin();
        std::vector< char > result;
        if( parse( iter, temp.end(), -( char_('-')|char_('+') ) >> +digit, result ) && iter == temp.end() ) {
          mp_value.from_string( temp, 10 );
          break;
        }
      }
    }
    return stream;
  }

  mpint sqrt( const mpint &value ) {
    mpint temp;
    MPINT_SAFE_CALL( mp_sqrt( const_cast< mp_int* >( &value.get_raw() ), const_cast< mp_int* >( &temp.get_raw() ) ) );
    return temp;
  }

  void swap( mpint &left, mpint &right ) {
    mp_exch( const_cast< mp_int* >( &left.get_raw() ), const_cast< mp_int* >( &right.get_raw() ) );
  }

}