/**
 * GeoDa TM, Copyright (C) 2011-2015 by Luc Anselin - all rights reserved
 *
 * This file is part of GeoDa.
 *
 * GeoDa is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * GeoDa is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <cmath>
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <wx/wxprec.h>
#include <wx/wx.h>
#include <wx/xrc/xmlres.h>
#include <wx/msgdlg.h>
#include "../Project.h"
#include "../DataViewer/TableInterface.h"
#include "../DataViewer/TimeState.h"
#include "../DataViewer/DataViewerAddColDlg.h"
#include "../GenUtils.h"
#include "../logger.h"
#include "FieldNewCalcSpecialDlg.h"
#include "FieldNewCalcUniDlg.h"
#include "FieldNewCalcBinDlg.h"
#include "FieldNewCalcLagDlg.h"
#include "FieldNewCalcRateDlg.h"

BEGIN_EVENT_TABLE( FieldNewCalcUniDlg, wxPanel )
	EVT_BUTTON( XRCID("ID_ADD_COLUMN"), FieldNewCalcUniDlg::OnAddColumnClick )
    EVT_CHOICE( XRCID("IDC_UNARY_RESULT"),
			   FieldNewCalcUniDlg::OnUnaryResultUpdated )
	EVT_CHOICE( XRCID("IDC_UNARY_RESULT_TM"),
		   FieldNewCalcUniDlg::OnUnaryResultTmUpdated )
	EVT_CHOICE( XRCID("IDC_UNARY_OPERATOR"),
			   FieldNewCalcUniDlg::OnUnaryOperatorUpdated )
	EVT_TEXT( XRCID("IDC_UNARY_OPERAND"),
			 FieldNewCalcUniDlg::OnUnaryOperandUpdated )
    EVT_COMBOBOX( XRCID("IDC_UNARY_OPERAND"),
				 FieldNewCalcUniDlg::OnUnaryOperandUpdated )
	EVT_CHOICE( XRCID("IDC_UNARY_OPERAND_TM"),
				 FieldNewCalcUniDlg::OnUnaryOperandTmUpdated )
END_EVENT_TABLE()

FieldNewCalcUniDlg::FieldNewCalcUniDlg(Project* project_s,
									   wxWindow* parent,
									   wxWindowID id, const wxString& caption,
									   const wxPoint& pos, const wxSize& size,
									   long style )
: all_init(false), op_string(9), project(project_s),
table_int(project_s->GetTableInt()),
m_valid_const(false), m_const(1), m_var_sel(wxNOT_FOUND),
is_space_time(project_s->GetTableInt()->IsTimeVariant())
{
	SetParent(parent);
    CreateControls();
    Centre();

	op_string[assign_op] = "ASSIGN";
	op_string[negate_op] = "NEGATIVE";
	op_string[invert_op] = "INVERT";
	op_string[sqrt_op] = "SQUARE ROOT";
	op_string[log_10_op] = "LOG (base 10)";
	op_string[log_e_op] = "LOG (base e)";
	op_string[dev_from_mean_op] = "DEVIATION FROM MEAN";
	op_string[standardize_op] = "STANDARDIZED";
	op_string[shuffle_op] = "SHUFFLE";

	for (int i=0, iend=op_string.size(); i<iend; i++) {
		m_op->Append(op_string[i]);
	}
	m_op->SetSelection(0);

	InitFieldChoices();
	all_init = true;
}

void FieldNewCalcUniDlg::CreateControls()
{
    wxXmlResource::Get()->LoadPanel(this, GetParent(), "IDD_FIELDCALC_UN");
    m_result = XRCCTRL(*this, "IDC_UNARY_RESULT", wxChoice);
	m_result_tm = XRCCTRL(*this, "IDC_UNARY_RESULT_TM", wxChoice);
	InitTime(m_result_tm);
    m_op = XRCCTRL(*this, "IDC_UNARY_OPERATOR", wxChoice);
    m_var = XRCCTRL(*this, "IDC_UNARY_OPERAND", wxComboBox);
	m_var_tm = XRCCTRL(*this, "IDC_UNARY_OPERAND_TM", wxChoice);
    InitTime(m_var_tm);
	m_text = XRCCTRL(*this, "IDC_EDIT1", wxTextCtrl);
	m_text->SetMaxLength(0);
}

void FieldNewCalcUniDlg::Apply()
{
	if (m_result->GetSelection() == wxNOT_FOUND) {
		wxString msg("Please choose a Result field.");
		wxMessageDialog dlg (this, msg, "Error", wxOK | wxICON_ERROR);
		dlg.ShowModal();
		return;
	}
	int result_col = col_id_map[m_result->GetSelection()];

	int var_col = wxNOT_FOUND;
	if (m_var_sel != wxNOT_FOUND) {
		var_col = col_id_map[m_var_sel];
	}
	if (var_col == wxNOT_FOUND && !m_valid_const) {
		wxString msg("Operation requires a valid field name or constant.");
		wxMessageDialog dlg (this, msg, "Error", wxOK | wxICON_ERROR);
		dlg.ShowModal();
		return;
	}

	if (is_space_time && var_col != wxNOT_FOUND &&
		!IsAllTime(result_col, m_result_tm->GetSelection()) &&
		IsAllTime(var_col, m_var_tm->GetSelection())) {
		wxString msg("When \"all times\" selected for variable, result "
					 "field must also be \"all times.\"");
		wxMessageDialog dlg (this, msg, "Error", wxOK | wxICON_ERROR);
		dlg.ShowModal();
		return;
	}

	TableState* ts = project->GetTableState();
	wxString grp_nm = table_int->GetColName(result_col);
	if (!Project::CanModifyGrpAndShowMsgIfNot(ts, grp_nm)) return;

	// Mersenne Twister random number generator, randomly seeded
	// with current time in seconds since Jan 1 1970.
	static boost::mt19937 rng(std::time(0));

	std::vector<int> time_list;
	if (IsAllTime(result_col, m_result_tm->GetSelection())) {
		int ts = project->GetTableInt()->GetTimeSteps();
		time_list.resize(ts);
		for (int i=0; i<ts; i++) time_list[i] = i;
	} else {
		int tm = IsTimeVariant(result_col) ? m_result_tm->GetSelection() : 0;
		time_list.resize(1);
		time_list[0] = tm;
	}

	int rows = table_int->GetNumberRows();
	std::vector<double> data(rows, 0);
	std::vector<bool> undefined(rows, false);
	if (var_col != wxNOT_FOUND &&
		!IsAllTime(var_col, m_var_tm->GetSelection())) {
		int tm = IsTimeVariant(var_col) ? m_var_tm->GetSelection() : 0;
		table_int->GetColData(var_col,tm, data);
		table_int->GetColUndefined(var_col, tm, undefined);
	} else {
		for (int i=0; i<rows; i++) data[i] = m_const;
	}
	std::vector<double> r_data(table_int->GetNumberRows(), 0);
	std::vector<bool> r_undefined(table_int->GetNumberRows(), false);

	for (int t=0; t<time_list.size(); t++) {
		if (var_col != wxNOT_FOUND &&
			IsAllTime(var_col, m_var_tm->GetSelection()))
		{
			table_int->GetColData(var_col, time_list[t], data);
			table_int->GetColUndefined(var_col, time_list[t], undefined);
		}
		for (int i=0; i<rows; i++) {
			r_data[i] = data[i];
			r_undefined[i] = undefined[i];
		}
		switch (m_op->GetSelection()) {
			case assign_op:
			{
			}
				break;
			case negate_op:
			{
				for (int i=0; i<rows; i++) {
					if (!undefined[i]) r_data[i] = -data[i];
				}
			}
				break;
			case invert_op:
			{
				for (int i=0; i<rows; i++) {
					if (!undefined[i] && data[i] != 0) {
						r_data[i] = 1.0 / data[i];
					} else {
						r_data[i] = 0;
						r_undefined[i] = true;
					}
				}
			}
				break;
			case sqrt_op:
			{
				for (int i=0; i<rows; i++) {
					if (!undefined[i] && data[i] >= 0) {
						r_data[i] = sqrt(data[i]);
					} else {
						r_data[i] = 0;
						r_undefined[i] = true;
					}
				}
			}
				break;
			case log_10_op:
			{
				for (int i=0; i<rows; i++) {
					if (!undefined[i] && data[i] > 0) {
						r_data[i] = log10(data[i]);
					} else {
						r_data[i] = 0;
						r_undefined[i] = true;
					}
				}
			}
				break;
			case log_e_op:
			{
				for (int i=0; i<rows; i++) {
					if (!undefined[i] && data[i] > 0) {
						r_data[i] = log(data[i]);
					} else {
						r_data[i] = 0;
						r_undefined[i] = true;
					}
				}
			}
				break;
			case dev_from_mean_op:
			{
				for (int i=0; i<rows; i++) {
					if (undefined[i]) {
						wxString msg;
						msg << "Observation " << i;
						msg << " is undefined. ";
						msg << "Operation aborted.";
						wxMessageDialog dlg (this, msg, "Error",
											 wxOK | wxICON_ERROR);
						dlg.ShowModal();
						return;
					}
					r_data[i] = data[i];
				}
				GenUtils::DeviationFromMean(r_data);
			}
				break;
			case standardize_op:
			{
				for (int i=0; i<rows; i++) {
					if (undefined[i]) {
						wxString msg;
						msg << "Observation ";
						msg << i << " is undefined. ";
						msg << "Operation aborted.";
						wxMessageDialog dlg (this, msg, "Error",
											 wxOK | wxICON_ERROR);
						dlg.ShowModal();
						return;
					}
					r_data[i] = data[i];
				}
				double ssum = 0.0;
				for (int i=0; i<rows; i++) ssum += r_data[i] * r_data[i];
				if (ssum == 0) {
					wxString msg("Standard deviation is 0, operation aborted.");
					wxMessageDialog dlg (this, msg, "Error", wxOK|wxICON_ERROR);
					dlg.ShowModal();
					return;
				}
				GenUtils::StandardizeData(r_data);
			}
				break;
			case shuffle_op:
			{
				for (int i=0; i<rows; i++) {
					r_data[i] = data[i];
					r_undefined[i] = undefined[i];
				}
				static boost::random::uniform_int_distribution<> X(0, rows-1);
				// X(rng) -> returns a uniform random number from 0 to rows-1;
				for (int i=0; i<rows; i++) {
					// swap each item in data with a random position in data.
					// This will produce a random permutation
					int r = X(rng);
					double d_t = r_data[r];
					bool u_t = r_undefined[r];
					r_data[r] = r_data[i];
					r_undefined[r] = r_undefined[i];
					r_data[i] = d_t;
					r_undefined[i] = u_t;
					if (undefined[i]) r_data[i] = 0;
					if (undefined[r]) r_data[r] = 0;
				}
			}
				break;
			default:
				return;
				break;
		}
		table_int->SetColData(result_col, time_list[t], r_data);
		table_int->SetColUndefined(result_col, time_list[t], r_undefined);

	}
}

void FieldNewCalcUniDlg::InitFieldChoices()
{
	wxString r_str_sel = m_result->GetStringSelection();
	int r_sel = m_result->GetSelection();
	int prev_cnt = m_result->GetCount();

	wxString var_val_orig = m_var->GetValue();
	m_result->Clear();
	{
		int sel_temp = m_var_sel;
		m_var->Clear();
		m_var_sel = sel_temp;
	}

	table_int->FillNumericColIdMap(col_id_map);
	m_var_str.resize(col_id_map.size());

	wxString r_tm, v_tm;
	if (is_space_time) {
		r_tm << " (" << m_result_tm->GetStringSelection() << ")";
		v_tm << " (" << m_var_tm->GetStringSelection() << ")";
	}
	for (int i=0, iend=col_id_map.size(); i<iend; i++) {
		if (is_space_time &&
			table_int->GetColTimeSteps(col_id_map[i]) > 1) {
			m_result->Append(table_int->GetColName(col_id_map[i]) + r_tm);
			m_var->Append(table_int->GetColName(col_id_map[i]) + v_tm);
			m_var_str[i] = table_int->GetColName(col_id_map[i]) + v_tm;
		} else {
			m_result->Append(table_int->GetColName(col_id_map[i]));
			m_var->Append(table_int->GetColName(col_id_map[i]));
			m_var_str[i] = table_int->GetColName(col_id_map[i]);
		}
	}

	if (m_result->GetCount() == prev_cnt) {
		// only the time field changed
		m_result->SetSelection(r_sel);
	} else {
		// a new variable might have been added, so find old string
		m_result->SetSelection(m_result->FindString(r_str_sel));
	}

	if (m_var->GetCount() == prev_cnt) {
		// only the time field changed
		if (m_var_sel != wxNOT_FOUND) {
			m_var->SetSelection(m_var_sel);
		} else {
			m_var->SetValue(var_val_orig);
		}
	} else {
		// a new variable might have been added, so find old string
		if (m_var_sel != wxNOT_FOUND) {
			m_var->SetSelection(m_var->FindString(var_val_orig));
			m_var_sel = m_var->GetSelection();
		} else {
			m_var->SetValue(var_val_orig);
		}
	}

	Display();
}

void FieldNewCalcUniDlg::UpdateOtherPanels()
{
	s_panel->InitFieldChoices();
	b_panel->InitFieldChoices();
	l_panel->InitFieldChoices();
	r_panel->InitFieldChoices();
}

void FieldNewCalcUniDlg::Display()
{
	if (!all_init) return;
	wxString s("");
	wxString lhs(m_result->GetStringSelection());
	wxString rhs("");
	wxString var("");
	if (m_var_sel != wxNOT_FOUND) var = m_var_str[m_var_sel];
	if (m_var_sel == wxNOT_FOUND && m_valid_const) {
		var = m_var->GetValue();
		var.Trim(false);
		var.Trim(true);
	}

	int op_sel = m_op->GetSelection();
	if (op_sel == assign_op) {
		rhs = var;
	} else if (op_sel == negate_op) {
		if (!var.IsEmpty()) rhs << "-" << var;
	} else if (op_sel == invert_op) {
		if (!var.IsEmpty()) rhs << "1/" << var;
	} else if (op_sel == sqrt_op) {
		if (!var.IsEmpty()) rhs << "sqrt( " << var << " )";
	} else if (op_sel == log_10_op) {
		if (!var.IsEmpty()) rhs << "log( " << var << " )";
	} else if (op_sel == log_e_op) {
		if (!var.IsEmpty()) rhs << "ln( " << var << " )";
	} else if (op_sel == dev_from_mean_op) {
		if (!var.IsEmpty()) rhs << "dev from mean of " << var;
	} else if (op_sel == standardize_op) {
		if (!var.IsEmpty()) rhs << "standardized dev from mean of " << var;
	} else { // op_sel == shuffle_op
		if (!var.IsEmpty()) rhs << "randomly permute values in " << var;
	}

	if (lhs.IsEmpty() && rhs.IsEmpty()) {
		s = "";
	} else if (!lhs.IsEmpty() && rhs.IsEmpty()) {
		s << lhs << " =";
	} else if (lhs.IsEmpty() && !rhs.IsEmpty()) {
		s << rhs;
	} else {
		// a good time to enable the apply button.
		s << lhs << " = " << rhs;
	}

	m_text->SetValue(s);
}

bool FieldNewCalcUniDlg::IsTimeVariant(int col_id)
{
	if (!is_space_time) return false;
	return (table_int->IsColTimeVariant(col_id));
}

bool FieldNewCalcUniDlg::IsAllTime(int col_id, int tm_sel)
{
	if (!is_space_time) return false;
	if (!table_int->IsColTimeVariant(col_id)) return false;
	return tm_sel == project->GetTableInt()->GetTimeSteps();
}

void FieldNewCalcUniDlg::OnUnaryResultUpdated( wxCommandEvent& event )
{
	int sel = m_result->GetSelection();
	m_result_tm->Enable(sel != wxNOT_FOUND &&
						IsTimeVariant(col_id_map[sel]));
    Display();
}

void FieldNewCalcUniDlg::OnUnaryResultTmUpdated( wxCommandEvent& event )
{
	InitFieldChoices();
    Display();
}

void FieldNewCalcUniDlg::OnUnaryOperatorUpdated( wxCommandEvent& event )
{
    Display();
}

void FieldNewCalcUniDlg::OnUnaryOperandUpdated( wxCommandEvent& event )
{
	if (!all_init) return;

	wxString var_val = m_var->GetValue();
	var_val.Trim(false);
	var_val.Trim(true);
	if (m_var->GetValue() != m_var->GetStringSelection()) {
		// User has typed something in manually.
		// if value matches some item on list, then set list to that
		// otherwise, set selection back to wxNOT_FOUND
		m_var_sel = wxNOT_FOUND;
		for (int i=0, i_end=m_var_str.size(); m_var_sel==-1 && i<i_end; i++) {
			if (var_val.IsSameAs(m_var_str[i], false)) m_var_sel = i;
		}
		if (m_var_sel != wxNOT_FOUND) {
			// don't use SetSelection because otherwise it will
			// be difficult to type in string names that have prefixes that
			// match someing in m_var_str
			//m_var->SetSelection(m_var_sel);
		} else {
			m_valid_const = var_val.ToDouble(&m_const);
		}
	} else {
		m_var_sel = m_var->GetSelection();
	}
	m_var_tm->Enable(m_var_sel != wxNOT_FOUND &&
					 table_int->GetColTimeSteps(col_id_map[m_var_sel]) > 1);
	Display();
}

void FieldNewCalcUniDlg::OnUnaryOperandTmUpdated( wxCommandEvent& event )
{
	InitFieldChoices();
    Display();
}

void FieldNewCalcUniDlg::OnAddColumnClick( wxCommandEvent& event )
{
	DataViewerAddColDlg dlg(project, this);
	if (dlg.ShowModal() != wxID_OK) return;
	InitFieldChoices();
	wxString sel_str = dlg.GetColName();
	if (table_int->GetColTimeSteps(dlg.GetColId()) > 1) {
		sel_str << " (" << m_result_tm->GetStringSelection() << ")";
	}
	m_result->SetSelection(m_result->FindString(sel_str));
	OnUnaryResultUpdated(event);
	UpdateOtherPanels();
}

void FieldNewCalcUniDlg::InitTime(wxChoice* time_list)
{
	time_list->Clear();
	for (int i=0; i<project->GetTableInt()->GetTimeSteps(); i++) {
		wxString t;
		t << project->GetTableInt()->GetTimeString(i);
		time_list->Append(t);
	}
	time_list->Append("all times");
	time_list->SetSelection(project->GetTableInt()->GetTimeSteps());
	time_list->Disable();
	time_list->Show(is_space_time);
}


//          Copyright Rein Halbersma 2014-2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_MPL_CFG_NO_PREPROCESSED_HEADERS
#define BOOST_MPL_LIMIT_VECTOR_SIZE 50

#include <xstd/bit_set.hpp>             // bit_set
#include <boost/container/flat_set.hpp> // flat_set
#include <boost/mpl/vector.hpp>         // vector
#include <boost/test/unit_test.hpp>     // BOOST_AUTO_TEST_SUITE, BOOST_AUTO_TEST_SUITE_END, BOOST_AUTO_TEST_CASE_TEMPLATE, BOOST_CHECK_EQUAL_COLLECTIONS
#include <algorithm>                    // copy
#include <cstdint>                      // uint8_t, uint16_t, uint32_t, uint64_t
#include <iterator>                     // inserter
#include <set>                          // set

BOOST_AUTO_TEST_SUITE(Implicit)

using namespace xstd;

using int_set_types = boost::mpl::vector
<       std::set<int>
,       boost::container::flat_set<int>
,       bit_set< 32, uint16_t>
,       bit_set< 33, uint16_t>
,       bit_set< 48, uint16_t>
,       bit_set< 32, uint32_t>
,       bit_set< 33, uint32_t>
,       bit_set< 64, uint32_t>
,       bit_set< 65, uint32_t>
,       bit_set< 96, uint32_t>
#if defined(__GNUG__) || defined(_MSC_VER) && defined(WIN64)
,       bit_set< 64, uint64_t>
,       bit_set< 65, uint64_t>
,       bit_set<128, uint64_t>
,       bit_set<129, uint64_t>
,       bit_set<192, uint64_t>
#endif
#if defined(__GNUG__)
,       bit_set<128, __uint128_t>
,       bit_set<129, __uint128_t>
,       bit_set<256, __uint128_t>
,       bit_set<257, __uint128_t>
,       bit_set<384, __uint128_t>
#endif
>;

class int_
{
        int m_value;
public:
        explicit(false) constexpr int_(int v) noexcept : m_value(v) {}
        explicit(false) constexpr operator int() const noexcept { return m_value; }
};

BOOST_AUTO_TEST_CASE_TEMPLATE(IntConstructible, T, int_set_types)
{
        auto src = T({ 2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31 });
        std::set<int_> dst;
        std::ranges::copy(src, std::inserter(dst, dst.end()));
        BOOST_CHECK_EQUAL_COLLECTIONS(src.begin(), src.end(), dst.begin(), dst.end());
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright Aleksey Gurtovoy 2001-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: upper_bound.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-10 23:26:17 -0700 (Fri, 10 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/upper_bound.hpp>

#include <boost/mpl/distance.hpp>
#include <boost/mpl/list_c.hpp>
#include <boost/mpl/less.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef list_c<int,1,2,3,3,3,5,8> numbers;
    typedef upper_bound< numbers, int_<3> >::type iter;

    MPL_ASSERT_RELATION( (mpl::distance< begin<numbers>::type,iter >::value), ==, 5 );
    MPL_ASSERT_RELATION( deref<iter>::type::value, ==, 5 );
}


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>

#include <boost/program_options.hpp>

namespace po = boost::program_options;
using std::cout;
using std::endl;

#include <jsCore/timer.hpp>

int main (int argc, char** argv)
{

  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("input,i", po::value<string>(),"path to input")
    ("output,o", po::value<string>(),"path to output")
    ("scale,s", po::value<float>(),"scale for normal extraction search radius")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    cout << desc << "\n";
    return 1;
  }

  float scale = 0.1;
  string inputPath = "./file.ply";
  string outputPath = "./out.ply";
  if(vm.count("input")) inputPath = vm["input"].as<string>();
  if(vm.count("output")) outputPath = vm["output"].as<string>();
  if(vm.count("scale")) scale = vm["scale"].as<float>();

  cout<< " extracting from "<<inputPath<<endl;
  cout<< " scale for normal estimation radius: "<<scale<<endl;
  cout<< " output to "<<outputPath<<endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PLYReader reader;
  if (reader.read(inputPath, *cloud))
    std::cout << "error reading " << inputPath << std::endl;
  else
    std::cout << "loaded pc from " << inputPath << ": " << cloud->width << "x"
      << cloud->height << std::endl;
//  if (pcl::io::loadPLYFile(inputPath, *cloud))
//  {
//    std::cout << "Cloud reading failed." << std::endl;
//    return (-1);
//  }

//  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("test_pcd.pcd", *cloud) == -1) //* load the file
//  {
//    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
//    return (-1);
//  }

  int w = cloud->width;
  int h = cloud->height;
  cout<<w << "x"<<h<<endl;

//  cloud->points[10+w*10] = pcl::PointXYZ(1.0,2.0,3.0);
//  cout<<cloud->points.data()<<endl;
//  float* data = cloud->points.data()->data;
//  cout<<data[(10+w*10)*3+0]<<" "<<data[(10+w*10)*3+1]<<" "<<data[(10+w*10)*3+2]<<endl;
//  cout<<cloud->points[10+w*10]<<endl;

  // estimate normals on a organized point cloud
  if(false)
  {
    jsc::Timer t0;
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); // 31ms
    //ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE); // 23ms
    //ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); // 47ms
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(10.0f);
    ne.setInputCloud(cloud);
    ne.compute(*normals);
    t0.toc();
    cout<<t0<<endl;
  }

  jsc::Timer t0;
  pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
//  ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); // 31ms
  //ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE); // 23ms
  //ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); // 47ms
//  ne.setMaxDepthChangeFactor(0.02f);
//  ne.setNormalSmoothingSize(10.0f);
  ne.setInputCloud(cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);
  ne.setRadiusSearch(scale);

  ne.compute(*normals);
  t0.toc();
  cout<<t0<<endl;

  cout<<normals->width<<" "<<normals->height<<endl;

  pcl::PointCloud<pcl::PointXYZRGBNormal> ptNormals(normals->width,
      normals->height);
  for(uint32_t i=0; i< ptNormals.size(); ++i) {
    ptNormals.at(i).x = cloud->points[i].x;
    ptNormals.at(i).y = cloud->points[i].y;
    ptNormals.at(i).z = cloud->points[i].z;
    if (normals->points[i].normal_x == normals->points[i].normal_x
        && normals->points[i].normal_y == normals->points[i].normal_y
        && normals->points[i].normal_z == normals->points[i].normal_z) {
      ptNormals.at(i).normal_x = normals->points[i].normal_x;
      ptNormals.at(i).normal_y = normals->points[i].normal_y;
      ptNormals.at(i).normal_z = normals->points[i].normal_z;
    } else {
      ptNormals.at(i).normal_x = 0;
      ptNormals.at(i).normal_y = 0;
      ptNormals.at(i).normal_z = 1;
    }
  }

  pcl::PLYWriter writer;
  writer.write(outputPath, ptNormals, false, false);

  return 0;

  // visualize normals
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");
  viewer.setBackgroundColor (0.3, 0.3, 0.3);
//  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color (cloud, 200, 10, 10);
//  viewer.addPointCloud(cloud,single_color,"cloud");
  viewer.addPointCloud(cloud,"cloud");
  viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(cloud, normals,10,0.05,"normals");

  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
  }

  return 0;
}


#define EIGENLINEARSOLVERS_EIGENCONJUGATEGRADIENT_CRS_CPP
#include <EigenLinearSolvers/EigenConjugateGradient[CRS].inl>

#if __has_include(<sofa/component/linearsolver/iterative/MatrixLinearSolver.inl>)
#include <sofa/component/linearsolver/iterative/MatrixLinearSolver.inl>
#else
#include <SofaBaseLinearSolver/MatrixLinearSolver.inl>
#endif

#include <sofa/core/ObjectFactory.h>

namespace EigenLinearSolvers
{

template class SOFA_EIGENLINEARSOLVERS_API EigenConjugateGradient< sofa::linearalgebra::CompressedRowSparseMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> >;
template class SOFA_EIGENLINEARSOLVERS_API EigenConjugateGradient<
    sofa::linearalgebra::CompressedRowSparseMatrix< sofa::type::Mat<3,3,SReal> >,
    sofa::linearalgebra::FullVector<SReal> >;

int EigenConjugateGradientCRSClass = sofa::core::RegisterObject("Linear system solver using the conjugate gradient iterative algorithm")
.add< EigenConjugateGradient< sofa::linearalgebra::CompressedRowSparseMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> > >()
.add< EigenConjugateGradient< sofa::linearalgebra::CompressedRowSparseMatrix< sofa::type::Mat<3,3,SReal> >, sofa::linearalgebra::FullVector<SReal> > >()
;

}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

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

*/

#ifndef _TESTELECTROMECHANICSEXACTSOLUTION_HPP_
#define _TESTELECTROMECHANICSEXACTSOLUTION_HPP_

#include <cxxtest/TestSuite.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign.hpp>

#include <cxxtest/TestSuite.h>
#include "BidomainProblem.hpp"
#include "PlaneStimulusCellFactory.hpp"
#include <petscvec.h>
#include "PetscSetupAndFinalize.hpp"
#include "CardiacElectroMechanicsProblem.hpp"
#include "LuoRudy1991.hpp"
#include "NonlinearElasticityTools.hpp"
#include "MooneyRivlinMaterialLaw.hpp"
#include "ExplicitCardiacMechanicsSolver.hpp"
#include "IncompressibleNonlinearElasticitySolver.hpp"
#include "ConstantActiveTension.hpp"

double MATERIAL_PARAM = 2.0;
double ACTIVE_TENSION = 1.5;
double ALPHA = 0.2;

// Body force (when active tension is constant and fibres in X-direction)
// corresponding to the deformation x = X+0.5*alpha*X^2, y=Y/(1+alpha*X), with p=2c
c_vector<double,2> MyBodyForce(c_vector<double,2>& rX, double t)
{
    assert(rX(0)>=0 && rX(0)<=1 && rX(1)>=0 && rX(1)<=1);

    c_vector<double,2> body_force;

    double lam = 1+ALPHA*rX(0);
    double mu = rX(1)*ALPHA/(lam*lam);
    double denom_sqd = pow(lam*lam + mu*mu,2);
    double aY = rX(1)*ALPHA;

    body_force(0) = -2*MATERIAL_PARAM * ALPHA                                - ACTIVE_TENSION*ALPHA*(5*mu*mu -lam*lam)/denom_sqd;
    body_force(1) = -2*MATERIAL_PARAM * 2*ALPHA*ALPHA*rX(1)/(lam*lam*lam)    - ACTIVE_TENSION*ALPHA*aY*(4.0/lam - 2*aY*aY*pow(lam,-7))/denom_sqd;

    return body_force;
}

// Surface traction (when active tension is constant and fibres in X-direction)
// on three sides of a cube, corresponding to x = X+0.5*alpha*X^2, y=Y/(1+alpha*X), with p=2c
c_vector<double,2> MyTraction(c_vector<double,2>& location, double t)
{
    c_vector<double,2> traction = zero_vector<double>(2);

    double lam = 1+ALPHA*location(0);
    double mu = location(1)*ALPHA/(lam*lam);
    double denom = lam*lam + mu*mu;

    if (fabs(location(0)-1.0) <= DBL_EPSILON) //Right edge
    {
        traction(0) =  2*MATERIAL_PARAM * (lam - 1.0/lam)              +  ACTIVE_TENSION*lam/denom;
        traction(1) = -2*MATERIAL_PARAM * location(1)*ALPHA/(lam*lam)  -  ACTIVE_TENSION*mu/denom;
    }
    else if (fabs(location(1))  <= DBL_EPSILON) //Bottom edge
    {
        traction(0) =  2*MATERIAL_PARAM * location(1)*ALPHA/(lam*lam);
        traction(1) = -2*MATERIAL_PARAM * (-lam + 1.0/lam);
    }
    else if (fabs(location(1) - 1.0) <= DBL_EPSILON)//Top edge
    {
        traction(0) = -2*MATERIAL_PARAM * location(1)*ALPHA/(lam*lam);
        traction(1) =  2*MATERIAL_PARAM * (-lam + 1.0/lam);
    }
    else
    {
        NEVER_REACHED;
    }

    return traction;
}

/**
 *  Test against an exact solution.
 *
 *  Choose x=X+0.5*alpha*X*X, y=Y/(1+alpha*X), p=2c, then F has determinant 1. Take
 *  the active tension to be constant.
 *
 *  Then S can be computed, and the boundary tractions and body needed to give this
 *  deformation as a solution can be computed, they are implemented above.
 *
 *  As active tension is constant, we really just testing cardiac mechanics
 *  here, not electrics solution, but electrics is tested elsewhere and mechanics does
 *  not affect electrics by default. Class is called TestElectroMechanicsExactSolution
 *  because we use the CardiacElectroMechanicsProblem interface)
 *
 *  See also equations and finite element implementation document.
 *
 */

class TestElectroMechanicsExactSolution : public CxxTest::TestSuite
{
public:
    void TestIncompressibleSolveWithExactSolution()
    {
        MechanicsEventHandler::Reset();

        PlaneStimulusCellFactory<CellLuoRudy1991FromCellML, 2> cell_factory(0.0);
        HeartConfig::Instance()->SetSimulationDuration(1.0);

        TetrahedralMesh<2,2> electrics_mesh;
        electrics_mesh.ConstructRegularSlabMesh(0.02, 1.0, 1.0);

        QuadraticMesh<2> mechanics_mesh;
        mechanics_mesh.ConstructRegularSlabMesh(0.05, 1.0, 1.0);

        MooneyRivlinMaterialLaw<2> law(MATERIAL_PARAM);

        std::vector<unsigned> fixed_nodes
            = NonlinearElasticityTools<2>::GetNodesByComponentValue(mechanics_mesh,0,0);

        std::vector<BoundaryElement<1,2>*> boundary_elems;
        for (TetrahedralMesh<2,2>::BoundaryElementIterator iter
              = mechanics_mesh.GetBoundaryElementIteratorBegin();
            iter != mechanics_mesh.GetBoundaryElementIteratorEnd();
            ++iter)
        {
            // get all boundary elems except those on X=0
            if (fabs((*iter)->CalculateCentroid()[0])>1e-6)
            {
                BoundaryElement<1,2>* p_element = *iter;
                boundary_elems.push_back(p_element);
            }
        }

        ElectroMechanicsProblemDefinition<2> problem_defn(mechanics_mesh);
        problem_defn.SetMaterialLaw(INCOMPRESSIBLE,&law);
        problem_defn.SetZeroDisplacementNodes(fixed_nodes);
        problem_defn.SetBodyForce(MyBodyForce);
        problem_defn.SetTractionBoundaryConditions(boundary_elems, MyTraction);
        problem_defn.SetContractionModel(CONSTANT,1.0/*ODE timestep, unused*/);
        problem_defn.SetSolverType(EXPLICIT);
        problem_defn.SetMechanicsSolveTimestep(1.0);

        CardiacElectroMechanicsProblem<2,1> problem(INCOMPRESSIBLE,
                                                    MONODOMAIN,
                                                    &electrics_mesh,
                                                    &mechanics_mesh,
                                                    &cell_factory,
                                                    &problem_defn,
                                                    "TestElectroMechanicsExactSolution");

        problem.Initialise();

        /////////////////////////////////////////////////////////////////
        //
        // Get access to the contraction models, to set the constant
        // to something other than 1.0. Difficult to do at the moment
        //
        //////////////////////////////////////////////////////////////////
        ExplicitCardiacMechanicsSolver<IncompressibleNonlinearElasticitySolver<2>,2>* p_solver
            = dynamic_cast<ExplicitCardiacMechanicsSolver<IncompressibleNonlinearElasticitySolver<2>,2>*>(problem.mpCardiacMechSolver);

        for (std::map<unsigned,DataAtQuadraturePoint>::iterator iter = p_solver->rGetQuadPointToDataAtQuadPointMap().begin();
             iter != p_solver->rGetQuadPointToDataAtQuadPointMap().end();
             iter++)
        {
            ConstantActiveTension* p_contraction_model = dynamic_cast<ConstantActiveTension*>(iter->second.ContractionModel);
            p_contraction_model->SetActiveTensionValue(ACTIVE_TENSION);
        }

        //////////////////////////////////////////////////////////////////
        //
        //  Solve and test
        //
        /////////////////////////////////////////////////////////////////
        problem.Solve();

        std::vector<c_vector<double,2> >& r_solution = problem.rGetDeformedPosition();

        for (unsigned i=0; i<mechanics_mesh.GetNumNodes(); i++)
        {
            double X = mechanics_mesh.GetNode(i)->rGetLocation()[0];
            double Y = mechanics_mesh.GetNode(i)->rGetLocation()[1];

            double exact_x = X + 0.5*ALPHA*X*X;
            double exact_y = Y/(1+ALPHA*X);

            TS_ASSERT_DELTA(r_solution[i](0), exact_x, 1e-3);
            TS_ASSERT_DELTA(r_solution[i](1), exact_y, 1e-3);
        }

        std::vector<double>& r_pressures = p_solver->rGetPressures();
        for (unsigned i=0; i<r_pressures.size(); i++)
        {
            TS_ASSERT_DELTA( r_pressures[i]/(2*MATERIAL_PARAM), 1.0, 5e-3);
        }

        MechanicsEventHandler::Headings();
        MechanicsEventHandler::Report();
    }
};

#endif //_TESTELECTROMECHANICSEXACTSOLUTION_HPP_


// My first test
//

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

#include <boost/graph/use_mpi.hpp>
#include <iostream>
#include <string>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/distributed/delta_stepping_shortest_paths.hpp>
#include <boost/graph/parallel/distribution.hpp>
#include <boost/parallel/append_buffer.hpp>

#include <am++/counter_coalesced_message_type.hpp>

//=======
#define PBGL_ACCOUNTING

#include <am++/am++.hpp>
#include <am++/mpi_transport.hpp>

#include <boost/graph/use_mpi.hpp>
#include <boost/graph/distributed/adjacency_list.hpp> // must precede delta_stepping...
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/distributed/delta_stepping_shortest_paths.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/graph/parallel/distribution.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/random.hpp>
#include <boost/test/minimal.hpp>
#include <boost/graph/iteration_macros.hpp>

#include <boost/parallel/append_buffer.hpp>

#include <am++/counter_coalesced_message_type.hpp>

//========

using namespace boost;

typedef int32_t weight_type;

struct WeightedEdge {
  WeightedEdge(weight_type weight = 0) : weight(weight) { }

  weight_type weight;
};

struct VertexProperties {
  VertexProperties(int d = 0) : distance(d) { }

  weight_type distance;
};

//================== MPT Structures ===================
// TODO what are we exactly doing here ?
namespace amplusplus {
  template<>
  struct make_mpi_datatype<WeightedEdge> : make_mpi_datatype_base {
    make_mpi_datatype<weight_type> dt1;
    scoped_mpi_datatype dt;
    make_mpi_datatype(): dt1() {
      int blocklengths[1] = {1};
      MPI_Aint displacements[1];
      WeightedEdge test_object;
      MPI_Aint test_object_ptr;
      MPI_Get_address(&test_object, &test_object_ptr);
      MPI_Get_address(&test_object.weight, &displacements[0]);
      displacements[0] -= test_object_ptr;
      MPI_Datatype types[1] = {dt1.get()};
      MPI_Type_create_struct(1, blocklengths, displacements, types, dt.get_ptr());
      MPI_Type_commit(dt.get_ptr());
    }
    MPI_Datatype get() const {return dt;}
  };

  template<>
  struct make_mpi_datatype<VertexProperties> : make_mpi_datatype_base {
    make_mpi_datatype<weight_type> dt1;
    scoped_mpi_datatype dt;
    make_mpi_datatype(): dt1() {
      int blocklengths[1] = {1};
      MPI_Aint displacements[1];
      VertexProperties test_object;
      MPI_Aint test_object_ptr;
      MPI_Get_address(&test_object, &test_object_ptr);
      MPI_Get_address(&test_object.distance, &displacements[0]);
      displacements[0] -= test_object_ptr;
      MPI_Datatype types[1] = {dt1.get()};
      MPI_Type_create_struct(1, blocklengths, displacements, types, dt.get_ptr());
      MPI_Type_commit(dt.get_ptr());
    }
    MPI_Datatype get() const {return dt;}
  };
}

//==============================================

//typedef adjacency_list<vecS, vecS, directedS, no_property, MyDistance, no_property, listS> Graph;
typedef adjacency_list<vecS, distributedS<>, directedS, VertexProperties, WeightedEdge> Graph;

void run_shortest_path_algorithm(amplusplus::transport trans, Graph g)
{
    // 0th vertex is the source
    Graph::vertex_descriptor source = vertex(0, g);

    // If g is connected run delta-stepping and verify
    boost::graph::distributed::delta_stepping_shortest_paths<Graph,
      typename boost::property_map<Graph, weight_type VertexProperties::*>::type,
      typename boost::property_map<Graph, weight_type WeightedEdge::*>::type>
          D(g, get(&VertexProperties::distance, g), get(&WeightedEdge::weight, g));

    D.set_source(source);

    int num_threads = 5;
    // Many threads now
    trans.set_nthreads(num_threads);

    boost::scoped_array<boost::thread> threads(new boost::thread[num_threads]);
    for (int i = 0; i < num_threads; ++i) {
        boost::thread thr(boost::ref(D), i);
        threads[i].swap(thr);
    }

    for (int i = 0; i < num_threads; ++i)
        threads[i].join();

    // Back to one thread
    trans.set_nthreads(1);

}

// function to generate a simple graph
//
void generate_graph(Graph& g)
{

   // where is add_edge defined ?
   add_edge(0, 1, WeightedEdge(10), g);
   add_edge(0, 2, WeightedEdge(5), g);
   add_edge(1, 3, WeightedEdge(1), g);
   add_edge(1, 2, WeightedEdge(2), g);
   add_edge(2, 1, WeightedEdge(3), g);
   add_edge(2, 3, WeightedEdge(9), g);
   add_edge(2, 4, WeightedEdge(2), g);
   add_edge(3, 4, WeightedEdge(4), g);
   add_edge(4, 3, WeightedEdge(6), g);
   add_edge(4, 0, WeightedEdge(7), g);

}

void print_graph(Graph& g)
{
    graph_traits < Graph >::vertex_iterator i, end;
    graph_traits < Graph >::adjacency_iterator ai, a_end;
    property_map < Graph, vertex_index_t >::type
    index_map = get(vertex_index, g); // index_map, vertex_index  where these are defined ? how do we find there definitions ? how do we know those are there for us to use ?

    for (boost::tie(i, end) = vertices(g); i != end; ++i) {
        std::cout << get(index_map, *i);
        boost::tie(ai, a_end) = adjacent_vertices(*i, g);

        if (ai == a_end)
            std::cout << " has no children";
        else
            std::cout << " is the parent of ";
        for (; ai != a_end; ++ai) {
            std::cout << get(index_map, *ai);
            if (boost::next(ai) != a_end)
                std::cout << ", ";
        }
        std::cout << std::endl;
    }

    property_map<Graph, weight_type WeightedEdge::*>::type weights = get(&WeightedEdge::weight, g);

    BGL_FORALL_VERTICES_T(v, g, Graph) {
      BGL_FORALL_OUTEDGES_T(v, e, g, Graph) {
          std::cout << " --" << 1 << "-->" <<  get(weights, e) << "  ";
      }

      std::cout << std::endl;
    }

    /*graph_traits<Graph>::vertex_iterator iv, endv;
    graph_traits<Graph>::out_edge_iterator ei, edge_end;
    property_map<Graph, weight_type WeightedEdge::*>::type weights = get(&WeightedEdge::weight, g); // TODO whats going on here ?

    for (boost::tie(iv, endv) = vertices(g); iv != endv; ++iv) {
        std::cout << *iv << " ";
        for (boost::tie(ei, edge_end) = out_edges(*iv, g); ei != edge_end; ++ei)
            std::cout << " --" << *ei << "--> " << weights[*ei] << "  ";

        std::cout << std::endl;
    }*/
    std::cout << std::endl;
}

int test_main(int argc, char* argv[])
{

  std :: cout << "Initializing AM++ ...." << std :: endl;

  amplusplus::environment env = amplusplus::mpi_environment(argc, argv, true);
  amplusplus::transport trans = env.create_transport();

  std :: cout << "The first test starting ...." << std :: endl;

  Graph g(trans);
  generate_graph(g);
  //print_graph(g);

  return 0;
}


#pragma once

#include <boost/python/make_function.hpp>
#include <boost/python/def_visitor.hpp>
#include <boost/python/signature.hpp>
#include <boost/mpl/at.hpp>
#include <pyublas/numpy.hpp>

namespace gd {

template <class F, class R>
struct vectorizer
{
    vectorizer(F fn) : fn(fn) {}

    template <class A0, class A1>
    pyublas::numpy_vector<A1> operator()(A0& a0, pyublas::numpy_vector<A1>& a1)
    {
		pyublas::numpy_vector<A1> outputvec(a1.size());
		//printf("size = %d\n", a1.size());
		A1* input = a1.data().begin();
		A1* input_end = a1.data().end();
		A1* output = outputvec.data().begin();
		while(input != input_end) {
			//printf("%f %f\n", *input, (a0.*fn)(*input));
			*output++ = (a0.*fn)(*input++);
		}
        return outputvec;
		//return 0;
    }
    F fn;
};

template <class F>
struct visitor : boost::python::def_visitor<visitor<F> >
{
    visitor(F fn)
      : fn(fn)
    {}

    template <class Class, class Options, class Signature>
    void visit_aux(
        Class& cl, char const* name
      , Options const& options, Signature const&) const
    {
	    typedef typename boost::mpl::at_c<Signature,0>::type	return_type;
	    typedef typename boost::mpl::at_c<Signature,2>::type	call_type;
	    typedef typename boost::mpl::at_c<Signature, 1>::type	call_class;
	    typedef pyublas::numpy_vector<return_type>				return_type_vec;
	    typedef pyublas::numpy_vector<call_type>				call_type_vec;
//float t = signature;
		//const boost::mpl::vector3<return_type_vec, gd::JeansAnisotropicConstant&, return_type_vec> sig_vec;
	    //boost::mpl::vector3<double, cls, double> sig_veqqc();
		//float t = signature;
	    //boost::mpl::vector3<return_type_vec, gd::JeansAnisotropicConstant&, return_type_vec> sig_vec;
	    //boost::mpl::vector3<double, gd::JeansAnisotropicConstant&, double> sig_test;
	    boost::mpl::vector3<return_type_vec, call_class, call_type_vec> sig_vec;
	    boost::mpl::vector3<return_type, call_class, call_type> sig_test;

        cl.def(
            name
          , boost::python::make_function(
                fn
              , options.policies()
              , options.keywords()
              , sig_test
            )
        );
		cl.def(
            name
          , boost::python::make_function(
                vectorizer<F, return_type>(fn)
              , options.policies()
              , options.keywords()
              , sig_vec
            )
        );
    }

    template <class Class, class Options>
    void visit(Class& cl, char const* name, Options const& options) const
    {
        this->visit_aux(
            cl, name, options
          , boost::python::detail::get_signature(fn, (typename Class::wrapped_type*)0)
        );
    }

    F fn;
};

// Member function adaptor that releases and aqcuires the GIL
// around the function call.
template <class F>
visitor<F> vectorize(F fn)
{
    return visitor<F>(fn);
}

}


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

/**
 * @file rcbdd.hpp
 *
 * @brief Data structure for BDD of a characteristic function to a reversible function
 *
 * @author Mathias Soeken
 * @since  2.0
 */

#ifndef RCBDD_HPP
#define RCBDD_HPP

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

#include <cuddObj.hh>

#include <reversible/circuit.hpp>

namespace cirkit
{

  using namespace boost::assign;

  class rcbdd
  {
  public:
    void initialize_manager();
    void create_variables( unsigned n, bool create_zs = true );
    BDD x( unsigned i ) const;
    BDD y( unsigned i ) const;
    BDD z( unsigned i ) const;

    const std::vector<BDD> xs() const;
    const std::vector<BDD> ys() const;
    const std::vector<BDD> zs() const;

    unsigned num_vars() const;
    const Cudd& manager() const;
    BDD chi() const;
    void set_chi( BDD f );
    void set_constant_value( bool v );
    bool constant_value() const;
    void set_num_inputs( unsigned n );
    void set_num_outputs( unsigned n );
    unsigned num_inputs() const;
    unsigned num_outputs() const;
    void set_input_labels( const std::vector<std::string>& labels );
    void set_output_labels( const std::vector<std::string>& labels );
    const std::vector<std::string> input_labels() const;
    const std::vector<std::string> output_labels() const;

    BDD compose(const BDD& left, const BDD& right) const;
    BDD cofactor( BDD f, unsigned var, bool input_polarity, bool output_polarity ) const;
    BDD move_xs_to_tmp( const BDD& f ) const;
    BDD move_ys_to_tmp( const BDD& f ) const;
    BDD move_tmp_to_ys( const BDD& f ) const;
    BDD move_ys_to_xs( const BDD& f) const;
    BDD remove_xs( const BDD& f ) const;
    BDD remove_ys( const BDD& f ) const;
    BDD remove_tmp( const BDD& f ) const;
    BDD invert( const BDD&f ) const;
    bool is_self_inverse( const BDD& f ) const;

    BDD create_from_gate( unsigned target, const BDD& controlf ) const;
    BDD create_from_gate( const gate& g ) const;
    BDD create_from_circuit( const circuit& circ ) const;

    void print_truth_table() const;
    void write_pla( const std::string& filename, bool full = false ) const;

  private:
    boost::optional<Cudd> _manager;
    BDD _chi;

    bool _constant_value = false;
    unsigned _num_inputs = 0u;
    unsigned _num_outputs = 0u;
    std::vector<std::string> _input_labels;
    std::vector<std::string> _output_labels;
    unsigned _n = 0u;
    std::vector<BDD> _xs;
    std::vector<BDD> _ys;
    std::vector<BDD> _zs;
  };

  void copy_meta_data( circuit& circ, const rcbdd& cf );
}

#endif

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


#include <mitkIOUtil.h>
#include <mitkITKImageImport.h>
#include <mitkImage.h>
#include <mitkImageAccessByItk.h>
#include <mitkImageCast.h>
#include <mitkTestingMacros.h>

#include <itkBinaryThresholdImageFilter.h>
#include <itkImageFileWriter.h>
#include <itkTestingComparisonImageFilter.h>

#include <mitkImagePixelReadAccessor.h>

//#include <boost/lexical_cast.hpp>

class DirectOverlayTestClass
{
public:
  template <typename TPixel, unsigned int VDimensions>
  static void InternalThreshold(const itk::Image<TPixel, VDimensions> *image,
                                mitk::Image::Pointer &output,
                                const double th[])
  {
    typedef itk::Image<TPixel, VDimensions> InputImageType;
    typedef itk::Image<unsigned int, VDimensions> OutputImageType;
    typedef itk::BinaryThresholdImageFilter<InputImageType, OutputImageType> BinaryThresholdFilterType;

    typename BinaryThresholdFilterType::Pointer thresholder = BinaryThresholdFilterType::New();
    thresholder->SetInput(image);
    thresholder->SetLowerThreshold(th[0]);
    thresholder->SetUpperThreshold(th[1]);
    thresholder->SetInsideValue(255);
    thresholder->SetOutsideValue(0);
    thresholder->Update();

    output = mitk::ImportItkImage(thresholder->GetOutput());
    // mitk::IOUtil::SaveImage( output, "/tmp/out.nii" );
    std::cout << "extra line";
  }

  template <typename TPixel, unsigned int VDimensions>
  static void InternalThreshold2(const itk::Image<TPixel, VDimensions> *image,
                                 itk::Image<unsigned int, 3>::Pointer &output,
                                 const double th[])
  {
    typedef itk::Image<TPixel, VDimensions> InputImageType;
    typedef itk::Image<unsigned int, VDimensions> OutputImageType;
    typedef itk::BinaryThresholdImageFilter<InputImageType, OutputImageType> BinaryThresholdFilterType;

    typename BinaryThresholdFilterType::Pointer thresholder = BinaryThresholdFilterType::New();
    thresholder->SetInput(image);
    thresholder->SetLowerThreshold(th[0]);
    thresholder->SetUpperThreshold(th[1]);
    thresholder->SetInsideValue(255);
    thresholder->SetOutsideValue(0);
    thresholder->Update();

    output = thresholder->GetOutput();
  }

  static void TestOverlay(mitk::Image::Pointer original,
                          mitk::Image::Pointer truth,
                          const double lower,
                          const double upper)
  {
    mitk::Image::Pointer overlayImage;
    const double th[] = {lower, upper};
    typedef itk::Image<unsigned int, 3> ImageType;
    ImageType::Pointer itkOverlayImage = ImageType::New();

    AccessByItk_2(original, InternalThreshold, overlayImage, th);
    /*
        AccessFixedDimensionByItk_2( original, InternalThreshold2,
                                     3, itkOverlayImage,
                                     th );

        overlayImage = mitk::ImportItkImage( itkOverlayImage );
    */
    // mitk::IOUtil::SaveImage(truth, "/tmp/truth_TestOverlay.nii");
    try
    {
      // mitk::Image::Pointer temp = overlayImage;
      mitk::IOUtil::SaveImage(overlayImage, "/tmp/overlayImage_TestOverlay.nrrd");
    }
    catch (const itk::ExceptionObject &e)
    {
      MITK_ERROR << "Save image: exception : " << e.what();
    }

    typedef itk::Image<unsigned int, 3> InputImageType;

    InputImageType::Pointer overlayItk;
    try
    {
      mitk::CastToItkImage(overlayImage, overlayItk);
    }
    catch (const mitk::Exception &e)
    {
      MITK_ERROR << "(CAST) Catched exception while creating accessor " << e.what();
      // MITK_TEST_FAILED_MSG("Exception for ouverlay image");
    }

    /*
        typedef  itk::ImageFileWriter< InputImageType  > WriterType;
        WriterType::Pointer writer = WriterType::New();
        writer->SetFileName("/tmp/overlayITK_TestOverlay.nii");
        writer->SetInput(overlayItk);
        writer->Update();
    */
    InputImageType::Pointer truthItk;
    mitk::CastToItkImage(truth, truthItk);

    bool difference = false;
    /*
        try
        {
          typedef unsigned int TPixel;

          itk::ImageRegionConstIteratorWithIndex< InputImageType > iter( truthItk, truthItk->GetLargestPossibleRegion()
       );
          iter.GoToBegin();

          mitk::ImagePixelReadAccessor< TPixel, 3 > readAccessor( overlayImage, overlayImage->GetVolumeData(0),
       mitk::ImageAccessorBase::ExceptionIfLocked );

          while( !iter.IsAtEnd() )
          {
            TPixel ref = iter.Get();
            TPixel val = readAccessor.GetPixelByIndex( iter.GetIndex() );

            difference |= ( ref != val );
            //if( difference )
            //{
              std::cout << iter.GetIndex() << ":" << ref << " ? " << val << "\n";
            //}
            ++iter;
          }
        }
        catch( const mitk::Exception &e)
        {
          MITK_ERROR << "Catched exception while creating accessor "<< e.what();
          //MITK_TEST_FAILED_MSG("Exception for ouverlay image");
        }

    */

    /*
        typedef itk::Testing::ComparisonImageFilter
          <InputImageType, InputImageType> ComparisonImageFilterType;
        ComparisonImageFilterType::Pointer comp =
          ComparisonImageFilterType::New();
        comp->SetValidInput(truthItk);
        comp->SetTestInput(overlayItk);
        try
        {
          comp->Update();
        }
        catch( const itk::ExceptionObject& e)
        {
          MITK_ERROR << "ITK Exception: " << e.what();
        }
    */
    typedef unsigned int TPixel;

    itk::ImageRegionConstIteratorWithIndex<InputImageType> iter(truthItk, truthItk->GetLargestPossibleRegion());
    itk::ImageRegionConstIteratorWithIndex<InputImageType> iter2(overlayItk, overlayItk->GetLargestPossibleRegion());
    iter.GoToBegin();

    unsigned int counter = 0;
    while (!iter.IsAtEnd() && !iter2.IsAtEnd())
    {
      TPixel ref = iter.Get();
      TPixel val = iter2.Get();

      if (ref != val)
      {
        counter++;
        //     std::cout << iter.GetIndex() << ":" << ref << " ? " << val << "\n";
      }

      ++iter;
      ++iter2;
    }

    std::cout << "Differs in " << counter << "voxels" << std::endl;

    MITK_TEST_CONDITION_REQUIRED(
      // comp->GetNumberOfPixelsWithDifferences() == 0,
      counter == 0,
      "Comparing overlay with ground truth")
  }

  static void TestDirectOverlay(char *in, char *gt, const int lower, const int upper)
  {
    mitk::Image::Pointer original = mitk::IOUtil::LoadImage(in);
    mitk::Image::Pointer truth = mitk::IOUtil::LoadImage(gt);

    if (original.IsNotNull() && original->GetDimension() == 3 && truth.IsNotNull() && truth->GetDimension() == 3 &&
        upper > lower)
    {
      TestOverlay(original, truth, lower, upper);
    }
    else
    {
      MITK_TEST_FAILED_MSG(<< "Invalid parameters");
    }
  }
};

int DirectOverlayTest(int argc, char *argv[])
{
  MITK_TEST_BEGIN("DirectOverlay")

  MITK_TEST_CONDITION_REQUIRED(argc >= 5, "File to load has been specified on the command line");

  unsigned int lower = 0, upper = 0;

  try
  {
    sscanf(argv[3], "%u", &lower);
    sscanf(argv[4], "%u", &upper);
    // lower = boost::lexical_cast<double>(argv[3]);
    // upper = boost::lexical_cast<double>(argv[4]);

    MITK_INFO << "Got values: " << lower << " : " << upper;
  }
  catch (std::exception &e)
  {
    MITK_TEST_FAILED_MSG(<< e.what());
  }

  DirectOverlayTestClass::TestDirectOverlay(argv[1], argv[2], lower, upper);

  MITK_TEST_END()
}