#include "mapper.hpp"

#include <boost/program_options.hpp>
#include <regex>
#include <math.h>

namespace po = boost::program_options;

/**
 * Global variables
 */
architecture   arch;
unsigned long  ngates                  = 0;
unsigned long  current_depth           = 0;
unsigned int   nqubits                 = 0;

std::set<edge>                                                               graph;
std::vector<std::vector<QASMparser::gate>>                                   layers;
unique_priority_queue<node, cleanup_node, node_cost_greater, node_func_less> nodes;

double get_pi_div(double val) {
	if(val == 0) {
		return 0;
	}
	const int precision = 10000;
	return round(M_PI / val * precision) / precision;
}

int main(int argc, char** argv) {
	bool   verbose     = false;
	bool   real_format = false;
	std::string input,  input_coupling;
	std::string output, output_statistics;

	// argument handling
	try {
		po::options_description desc{"Options"};
    	desc.add_options()
			("help,h",                                                        "help screen")
			("input,i",         po::value<std::string>(&input)->required(),   "input file")
			("output,o",        po::value<std::string>(&output),              "output file")
			("statistic,s",     po::value<std::string>(&output_statistics),   "output statistics file")
			("coupling_file,c", po::value<std::string>(&input_coupling),      "coupling graph - file")
			("verbose,v",       po::bool_switch(&verbose),                    "verbose")
			("real,r",          po::bool_switch(&real_format),                "output the circuit in the real format");

		po::positional_options_description p;
		p.add("input",     1);
		p.add("statistic", 1);
		p.add("output",    1);

		po::variables_map vm;
		po::store(po::command_line_parser(argc, argv).
			options(desc).positional(p).run(), vm);

		if (vm.count("help")) {
			std::cout << desc << std::endl;
			return 0;
		}
		po::notify(vm);
	} catch (const po::error &ex) {
		std::cerr << ex.what() << std::endl;
		exit(ERROR);
	}

	if(verbose) {
		std::cout << "Input:        " << input             << std::endl;
		std::cout << "Output:       " << output            << std::endl;
		std::cout << "Statistic:    " << output_statistics << std::endl;
		std::cout << "CouplingFile: " << input_coupling    << std::endl;
		std::cout << "Verbose:      " << verbose           << std::endl;
	}

	// parsing
	QASMparser* parser = new QASMparser(input.c_str());
	parser->Parse();

	std::vector<QASMparser::gate> gates = parser->getGates();
	nqubits = parser->getNqubits();
	ngates  = parser->getNgates();

	parser->clear();
	delete parser;

	// graph handling
	if(!create_architecture_properties(input_coupling)) {
		std::cout << "Error while generating the graph" << std::endl;
		exit(ERROR);
	}
	if(arch.positions > 0 ? nqubits > (unsigned int)arch.positions : (int)nqubits > arch.positions) {
        std::cerr << "ERROR before mapping: more logical qubits than physical ones!" << std::endl;
        exit(ERROR);
    }

	// print infos
	const char* bName = basename(input.c_str());
	if(verbose) {
		std::cout << "Circuit name: " << bName << " (requires " << nqubits << " qubits)" << std::endl;
		std::cout << std::endl;
		std::cout << "Before mapping: "                      << std::endl;
		std::cout << "  elementary gates: " << ngates        << std::endl;
		std::cout << "  depth:            " << layers.size() << std::endl;
	} else {
    	std::cout << bName << ',' << nqubits << ',' << ngates << ',' << layers.size() << ',' << std::flush;
	}

	// start mapping algorithm
	clock_t begin_time = clock();

	int                                        total_swaps = 0;
	circuit_properties                         properties  = create_circuit_properties();
    std::vector<QASMparser::gate>              all_gates;
	std::vector<std::vector<QASMparser::gate>> mapped_circuit;

	mapping(gates, mapped_circuit, all_gates, total_swaps, properties);

	double    time     = double(clock() - begin_time) / CLOCKS_PER_SEC;
	int       depth    = mapped_circuit.size();
	int       cost     = all_gates.size()-total_swaps;
#if SPECIAL_OPT
	long long workload = workload_cost(properties.workload);
	double    fidelity = fidelity_cost(properties.fidelities);
#else
	long long workload = 0;
	double    fidelity = 0;
#endif
	// print statistics
	if(verbose) {
		std::cout << std::endl << "After mapping (no post mapping optimizations are conducted): " << std::endl;
		std::cout << "  elementary gates: " << cost  << std::endl;
		std::cout << "  depth:            " << depth << std::endl;

		std::cout << "\nThe mapping required " << time << " seconds" << std::endl;

		std::cout << "\nInitial mapping of the logical qubits (q) to the physical qubits (Q) of the IBM QX5 architecture: " << std::endl;

		for(unsigned int i = 0; i < nqubits; i++) {
			std::cout << "  q" << i << " is initially mapped to Q" << properties.locations[i] << std::endl;
		}
	} else {
    	std::cout << time << ',' << cost << ',' << depth << "," << fidelity << std::endl;
	}

	// dump resulting circuit
	if(!output.empty()) {
		std::ofstream of(output);
		if(real_format) {
			of << ".numvars "   << nqubits << std::endl;
			of << ".variables";
			for(unsigned int i = 0; i < nqubits; i++) {
				of << " q" << i;
			}
			of << std::endl;
			of << ".constants ";
			for(unsigned int i = 0; i < nqubits; i++) {
				of << "0";
			}
			of << std::endl;
			of << ".begin" << std::endl;
			for (std::vector<std::vector<QASMparser::gate> >::iterator it = mapped_circuit.begin();
					it != mapped_circuit.end(); it++) {
				std::vector<QASMparser::gate> v = *it;
				for (std::vector<QASMparser::gate>::iterator it2 = v.begin(); it2 != v.end(); it2++) {
					std::string hadamard = "U(pi/2,0,pi)";
					if(it2->control != -1) {
						of << "t2 " << "q" << it2->control << " q" << it2->target << std::endl;
					} else if(hadamard.compare(it2->type) == 0) {
						of << "h1 q" << it2->target << std::endl;
					} else {
						std::string s(it2->type);
						std::regex rgx("U\\(([+-]?([0-9]*[.])?[0-9]+), ([+-]?([0-9]*[.])?[0-9]+), ([+-]?([0-9]*[.])?[0-9]+)\\).*");
						std::smatch match;

						if(std::regex_search(s, match, rgx)) {
							double theta = stof(match[1]);
							double phi   = stof(match[3]);
							double delta = stof(match[5]);

							double theta_div = get_pi_div(theta);
							double phi_div   = get_pi_div(phi);
							double delta_div = get_pi_div(delta);

							// conversion to rotation gates
							if(phi_div == 0) {
								of << "rz1:" << 1                     << " q" << it2->target << std::endl; //1.0 / 3
							} else {
								of << "rz1:" << (int)(phi_div / (1 + 3 * phi_div)) << " q" << it2->target << std::endl;
							}
							of << "rx1:" << 2                               << " q" << it2->target << std::endl;
							if(theta_div == 0) {
								of << "rz1:" << 1                           << " q" << it2->target << std::endl;
							} else {
								of << "rz1:" << (int)(theta_div / (1 + theta_div)) << " q" << it2->target << std::endl;
							}
							of << "rx1:" << 2                               << " q" << it2->target << std::endl;
							if(delta_div != 0) {
								of << "rz1:" << delta_div                   << " q" << it2->target << std::endl;
							}
						}
					}
				}
			}
			of << ".end" << std::endl;
		} else {
			of << "OPENQASM 2.0;"              << std::endl;
			of << "include \"qelib1.inc\";"    << std::endl;
			of << "qreg q[" << nqubits << "];" << std::endl;
			of << "creg c[" << nqubits << "];" << std::endl;

			for (std::vector<std::vector<QASMparser::gate> >::iterator it = mapped_circuit.begin();
				it != mapped_circuit.end(); it++) {
				for (std::vector<QASMparser::gate>::iterator it2 = it->begin(); it2 != it->end(); it2++) {
					of << it2->type << " ";
					if (it2->control != -1) {
						of << "q[" << it2->control << "],";
					}
					of << "q[" << it2->target << "];" << std::endl;
				}
			}
		}
	}

	// store timing
	if(!output_statistics.empty()) {
		std::ofstream ofstat (output_statistics, std::ofstream::app);
		//ofstat << bName << " : " << time << " " << depth << " " << cost << " " << workload << " " << alloc_tries << " " << total_swaps << std::endl;
		ofstat << bName << " : " << time << " " << depth << " " << cost << " " << workload << " " << total_swaps << " " << fidelity << std::endl;
	}

	delete_circuit_properties(properties);
	delete_architecture_properties();

	return 0;
}


#include <boost/hana/fwd/cartesian_product.hpp>


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/view_graph/orientations_from_maximum_spanning_tree.h"

#include <Eigen/Core>
#include <ceres/rotation.h>

#include <algorithm>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>

#include "theia/math/graph/minimum_spanning_tree.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/util/map_util.h"

namespace theia {
namespace {
typedef std::pair<TwoViewInfo, ViewIdPair> HeapElement;

bool SortHeapElement(const HeapElement& h1, const HeapElement& h2) {
  return h1.first.num_verified_matches > h2.first.num_verified_matches;
}

// Computes the orientation of the neighbor camera based on the orientation of
// the source camera and the relative rotation between the cameras.
Eigen::Vector3d ComputeOrientation(const Eigen::Vector3d& source_orientation,
                                   const TwoViewInfo& two_view_info,
                                   const ViewId source_view_id,
                                   const ViewId neighbor_view_id) {
  Eigen::Matrix3d source_rotation_mat, relative_rotation;
  ceres::AngleAxisToRotationMatrix(
      source_orientation.data(),
      ceres::ColumnMajorAdapter3x3(source_rotation_mat.data()));
  ceres::AngleAxisToRotationMatrix(
      two_view_info.rotation_2.data(),
      ceres::ColumnMajorAdapter3x3(relative_rotation.data()));

  const Eigen::Matrix3d neighbor_orientation =
      (source_view_id < neighbor_view_id)
          ? (relative_rotation * source_rotation_mat).eval()
          : (relative_rotation.transpose() * source_rotation_mat).eval();

  Eigen::Vector3d orientation;
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(neighbor_orientation.data()),
      orientation.data());
  return orientation;
}

// Adds all the edges of view_id to the heap. Only edges that do not already
// have an orientation estimation are added.
void AddEdgesToHeap(
    const ViewGraph& view_graph,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const ViewId view_id,
    std::vector<HeapElement>* heap) {
  const std::unordered_set<ViewId>* edge_ids =
      view_graph.GetNeighborIdsForView(view_id);
  for (const ViewId edge_id : *edge_ids) {
    // Only add edges to the heap that contain a vertex that has not been seen.
    if (ContainsKey(orientations, edge_id)) {
      continue;
    }

    heap->emplace_back(*view_graph.GetEdge(view_id, edge_id),
                       ViewIdPair(view_id, edge_id));
    std::push_heap(heap->begin(), heap->end(), SortHeapElement);
  }
}

}  // namespace

bool OrientationsFromMaximumSpanningTree(
    const ViewGraph& view_graph,
    std::unordered_map<ViewId, Eigen::Vector3d>* orientations) {
  CHECK_NOTNULL(orientations);

  // Compute the largest connected component of the input view graph since the
  // MST is only valid on a single connected component.
  std::unordered_set<theia::ViewId> largest_cc;
  view_graph.GetLargestConnectedComponentIds(&largest_cc);
  ViewGraph largest_cc_subgraph;
  view_graph.ExtractSubgraph(largest_cc, &largest_cc_subgraph);

  // Compute maximum spanning tree.
  const auto& all_edges = largest_cc_subgraph.GetAllEdges();
  MinimumSpanningTree<ViewId, int> mst_extractor;
  for (const auto& edge : all_edges) {
    // Since we want the *maximum* spanning tree, we negate all of the edge
    // weights in the *minimum* spanning tree extractor.
    mst_extractor.AddEdge(
        edge.first.first, edge.first.second, -edge.second.num_verified_matches);
  }

  std::unordered_set<ViewIdPair> mst;
  if (!mst_extractor.Extract(&mst)) {
    VLOG(2)
        << "Could not extract the maximum spanning tree from the view graph";
    return false;
  }

  // Create an MST view graph.
  ViewGraph mst_view_graph;
  for (const ViewIdPair& edge : mst) {
    mst_view_graph.AddEdge(
        edge.first,
        edge.second,
        *largest_cc_subgraph.GetEdge(edge.first, edge.second));
  }

  // Chain the relative rotations together to compute orientations.  We use a
  // heap to determine the next edges to add to the minimum spanning tree.
  std::vector<HeapElement> heap;

  // Set the root value.
  const ViewId root_view_id = mst.begin()->first;
  (*orientations)[root_view_id] = Eigen::Vector3d::Zero();
  AddEdgesToHeap(mst_view_graph, *orientations, root_view_id, &heap);

  while (!heap.empty()) {
    const HeapElement next_edge = heap.front();
    // Remove the best edge.
    std::pop_heap(heap.begin(), heap.end(), SortHeapElement);
    heap.pop_back();

    // If the edge contains two vertices that have already been added then do
    // nothing.
    if (ContainsKey(*orientations, next_edge.second.second)) {
      continue;
    }

    // Compute the orientation for the vertex.
    (*orientations)[next_edge.second.second] =
        ComputeOrientation(FindOrDie(*orientations, next_edge.second.first),
                           next_edge.first,
                           next_edge.second.first,
                           next_edge.second.second);

    // Add all edges to the heap.
    AddEdgesToHeap(
        mst_view_graph, *orientations, next_edge.second.second, &heap);
  }
  return true;
}

}  // namespace theia


/*
   For more information, please see: http://software.sci.utah.edu

   The MIT License

   Copyright (c) 2012 Scientific Computing and Imaging Institute,
   University of Utah.

   License for the specific language governing rights and limitations under
   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.

   Author: Moritz Dannhauer, Spencer Frisby
   Date:   September 2014
*/
///@file SetupRHSforTDCSandTMS
///@brief
/// This module sets up TDCS problem by providing the right hand side vector (controlled by GUI) and inputs for the modules: AddKnownsToLinearSystem, BuildTDCSMatrix.
///
///@author
/// Moritz Dannhauer, Spencer Frisby
///
///@details
/// .The module gets 4 inputs the tetrahedral mesh (first), triangle surfaces (as part of the first input) for the scalp (second) and electrodes sponges (third) as well as the a matrix (output of ElectrodeCoilSetup)
///  that contains the center location and thickness of the electrode sponge. The first output contains the knowns (reference zeros node) of the linear system (third input of AddKnownsToLinearSystem).
///  The zero potential node is set to be the first mesh node by default, which can be chosen arbitrarily in the GUI by the user. The second until fifth output prepares the inputs for the BuildTDCSMatrix module that
///  combines the FEM stiffness matrix with the boundary conditions of the complete electrode model. The electrical impedances can be provided in the GUI for each electrode individually.
///  The module needs to identify the surface of the sponge which makes up the major part of the algorithmic module implementation.
///  The sixth output creates the right hand side (RHS) vector that can be provided by the GUI. The electrical charges (ingoing and outgoing current intensities) need to sum up zero.
///  If you execute the module once the module will graphically only present the number of electrodes defined in the input data.
///  The complete electrode model solves the nodal and electrode potentials in an agumented linear system the potential solution (output of SolveLinearSystem) needs to adjuested.
///  Therefore, a seventh output is provided that contains the relevant indeces to be used for the module 'SelectSubMatrix'.
///  Since the identification of the contact surface between electrode sponge and the electrode itself (the sponge surface top is assumed to be that) is a difficult geometrical process the eighth
///  output provides a triangle mesh that can be used for visual inspection (->ShowField->ViewScene).

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/BrainStimulator/SetupRHSforTDCSandTMSAlgorithm.h>
#include <Core/GeometryPrimitives/Vector.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/SparseRowMatrixFromMap.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>
#include <Core/Datatypes/MatrixComparison.h>
#include <boost/lexical_cast.hpp>
#include <Core/Algorithms/Legacy/Fields/MeshDerivatives/SplitByConnectedRegion.h>
#include <Core/Algorithms/Legacy/Fields/DomainFields/SplitFieldByDomainAlgo.h>

using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Algorithms::Fields;
using namespace SCIRun;

ALGORITHM_PARAMETER_DEF(BrainStimulator, ElectrodeTableValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ImpedanceTableValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, SurfaceAreaValues);
ALGORITHM_PARAMETER_DEF(BrainStimulator, ELECTRODE_VALUES);
ALGORITHM_PARAMETER_DEF(BrainStimulator, IMPEDANCE_VALUES);
ALGORITHM_PARAMETER_DEF(BrainStimulator, refnode);
ALGORITHM_PARAMETER_DEF(BrainStimulator, normal_dot_product_bound);
ALGORITHM_PARAMETER_DEF(BrainStimulator, pointdistancebound);
ALGORITHM_PARAMETER_DEF(BrainStimulator, number_of_electrodes);

AlgorithmInputName SetupTDCSAlgorithm::MESH("MESH");
AlgorithmInputName SetupTDCSAlgorithm::SCALP_TRI_SURF_MESH("SCALP_TRI_SURF_MESH");
AlgorithmInputName SetupTDCSAlgorithm::ELECTRODE_TRI_SURF_MESH("ELECTRODE_TRI_SURF_MESH");
AlgorithmInputName SetupTDCSAlgorithm::ELECTRODE_SPONGE_LOCATION_AVR("ELECTRODE_SPONGE_LOCATION_AVR");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT("ELECTRODE_ELEMENT");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT_TYPE("ELECTRODE_ELEMENT_TYPE");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_ELEMENT_DEFINITION("ELECTRODE_ELEMENT_DEFINITION");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_CONTACT_IMPEDANCE("ELECTRODE_CONTACT_IMPEDANCE");
AlgorithmOutputName SetupTDCSAlgorithm::RHS("RHS");
AlgorithmOutputName SetupTDCSAlgorithm::LHS_KNOWNS("LHS_KNOWNS");
AlgorithmOutputName SetupTDCSAlgorithm::ELECTRODE_SPONGE_SURF("ELECTRODE_SPONGE_SURF");
AlgorithmOutputName SetupTDCSAlgorithm::SELECTMATRIXINDECES("SELECTMATRIXINDECES");

AlgorithmParameterName SetupTDCSAlgorithm::ElectrodeParameterName(int i) { return AlgorithmParameterName(Name("elc" + boost::lexical_cast<std::string>(i))); }
AlgorithmParameterName SetupTDCSAlgorithm::ElectrodeImpedanceParameterName(int i) { return AlgorithmParameterName(Name("imp_elc" + boost::lexical_cast<std::string>(i))); }

const double SetupTDCSAlgorithm::electode_current_summation_bound = 1e-6;
const int SetupTDCSAlgorithm::special_label = 4321;
const int SetupTDCSAlgorithm::max_number_of_electrodes = 512;

SetupTDCSAlgorithm::SetupTDCSAlgorithm()
{
  addParameter(Parameters::SurfaceAreaValues, 0);
  addParameter(Parameters::ELECTRODE_VALUES, 0); // just a default value, will be replaced with vector
  addParameter(Parameters::IMPEDANCE_VALUES, 0);
  addParameter(Parameters::number_of_electrodes, max_number_of_electrodes);
  addParameter(Parameters::refnode, 0);
  addParameter(Parameters::normal_dot_product_bound, 0.7);
  addParameter(Parameters::pointdistancebound, 0.0001);
}

AlgorithmOutput SetupTDCSAlgorithm::run_generic(const AlgorithmInput& input) const
{
  auto mesh = input.get<Field>(MESH);

  auto all_elc_values = get(Parameters::ELECTRODE_VALUES).toVector();
  auto all_imp_elc_values = get(Parameters::IMPEDANCE_VALUES).toVector();

  if (all_elc_values.size()!=all_imp_elc_values.size())
  {
    THROW_ALGORITHM_PROCESSING_ERROR("Internal Error: electrode information could not retrieved from GUI.");
  }

  /// consistency checks to validate the GUI<->Algorithm data transfer
  for (int i=0; i<all_elc_values.size(); i++)
  {
    auto elecName = all_elc_values[i].nameForXml();
   // auto elecValue = all_elc_values[i].getDouble(); // why is not that used?
    auto expectedElecName = ElectrodeParameterName(i); // ElecrodeParameterName(i);
    if(elecName.name_.compare(expectedElecName.name_) != 0) // if so, electrodes are being stored out of order.
      THROW_ALGORITHM_PROCESSING_ERROR("Values are being stored out of order!");

    auto impelecName = all_imp_elc_values[i].nameForXml();
    auto expectedImpElecName = ElectrodeImpedanceParameterName(i);
    if(elecName.name_.compare(expectedElecName.name_) != 0) // if so, electrodes are being stored out of order.
      THROW_ALGORITHM_PROCESSING_ERROR("Impedance values are being stored out of order!");
  }

  auto scalp_tri_surf = input.get<Field>(SCALP_TRI_SURF_MESH);
  auto elc_tri_surf = input.get<Field>(ELECTRODE_TRI_SURF_MESH);

  if(!elc_tri_surf)
  {
    THROW_ALGORITHM_PROCESSING_ERROR("Input Error: electrode triangle surface is not allocated.");
  }

  if(!scalp_tri_surf)
  {
    THROW_ALGORITHM_PROCESSING_ERROR("Input Error: Scalp triangle surface is not allocated.");
  }

  // obtaining number of electrodes
  DenseMatrixHandle elc_sponge_location = matrix_convert::to_dense(input.get<Matrix>(ELECTRODE_SPONGE_LOCATION_AVR));
  if (!elc_sponge_location)
  {
   THROW_ALGORITHM_PROCESSING_ERROR("Electrode sponges matrix (center locations) is not allocated.");
  }
  if (elc_sponge_location->ncols()!=4 || elc_sponge_location->nrows()<2)
  {
   THROW_ALGORITHM_PROCESSING_ERROR("Electrode sponges matrix needs to have dimension #sponges x 4 (#sponges>=2)");
  }

  int num_of_elc = elc_sponge_location->nrows();

  AlgorithmOutput output;

  DenseMatrixHandle lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, rhs, selectmatrixind;
  FieldHandle elec_sponge_surf;
  std::vector<double> electrode_sponge_areas;
  boost::tie(lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, rhs, elec_sponge_surf, selectmatrixind, electrode_sponge_areas) = run(mesh, all_elc_values, all_imp_elc_values, num_of_elc, scalp_tri_surf, elc_tri_surf, elc_sponge_location);

  output[LHS_KNOWNS] = lhs_knowns;
  output[ELECTRODE_ELEMENT] = elc_element;
  output[ELECTRODE_ELEMENT_TYPE] = elc_element_typ;
  output[ELECTRODE_ELEMENT_DEFINITION] = elc_element_def;
  output[ELECTRODE_CONTACT_IMPEDANCE] = elc_contact_imp;
  output[RHS] = rhs;
  output[SELECTMATRIXINDECES] = selectmatrixind;
  output[ELECTRODE_SPONGE_SURF] = elec_sponge_surf;

  Variable::List surface_area;
  for (long i=0; i<electrode_sponge_areas.size(); i++)
  {
    surface_area.push_back(makeVariable("surf_area_" + boost::lexical_cast<std::string>(i), electrode_sponge_areas[i]));
  }
  VariableHandle var_surface_area(new Variable(Name("surf_areas"), surface_area));
  output.setAdditionalAlgoOutput(var_surface_area);
  return output;
}

/// replace this code with calls to splitfieldbyconnectedregion, clipfieldby* if available for SCIRun5
boost::tuple<DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, FieldHandle, std::vector<double>> SetupTDCSAlgorithm::create_lhs(FieldHandle mesh, const std::vector<Variable>& impelc, FieldHandle scalp_tri_surf, FieldHandle elc_tri_surf, DenseMatrixHandle elc_sponge_location) const
{
 VMesh*  mesh_vmesh = mesh->vmesh();
 VMesh::size_type mesh_num_nodes = mesh_vmesh->num_nodes();
 DenseMatrixHandle lhs_knows, elc_elem, elc_elem_typ, elc_elem_def, elc_con_imp;

 index_type refnode_number = get(Parameters::refnode).toInt();

 double normal_dot_product_bound_ = get(Parameters::normal_dot_product_bound).toDouble();
 double identical_node_location_differce = get(Parameters::pointdistancebound).toDouble();

 if (identical_node_location_differce<=0 || refnode_number<0 || normal_dot_product_bound_<0)   /// check for valid input data
 {
  THROW_ALGORITHM_PROCESSING_ERROR(" Gui values (refnode, point dis., surf shrink) should be > 0. ");
 }

 SplitFieldByConnectedRegionAlgo algo;
 algo.set(SplitFieldByConnectedRegionAlgo::SortDomainBySize(), false);
 algo.set(SplitFieldByConnectedRegionAlgo::SortAscending(), false);
 std::vector<FieldHandle> result = algo.run(elc_tri_surf);

 if (result.size()<=0)
 {
    THROW_ALGORITHM_PROCESSING_ERROR(" Splitting input mesh into connected regions failed. ");
 }

 if (elc_sponge_location->ncols()!=4)
 {
   THROW_ALGORITHM_PROCESSING_ERROR(" ELECTRODE_SPONGE_LOCATION_AVR (4th module input) needs to have 4 columns (x,y,z,elc. sponge height).");
 }

 if( result.size() != elc_sponge_location->nrows())
 {
   THROW_ALGORITHM_PROCESSING_ERROR(" The number of electrode sponges (4th module input) does not match number of a splitted electrode surfaces (3rd module input).");
 }

 if ( refnode_number > mesh_num_nodes || refnode_number<0)
 {
    THROW_ALGORITHM_PROCESSING_ERROR(" Reference node exceeds number of FEM nodes. ");
 }

 /// prepare LHS_KNOWNS which is the an input to addknownstolinearsystem (called x) besides the stiffness matrix
 lhs_knows=boost::make_shared<DenseMatrix>(mesh_num_nodes,1);
 for(VMesh::Node::index_type idx=0; idx<mesh_num_nodes; idx++)
 {
   (*lhs_knows)(idx,0)=std::numeric_limits<double>::quiet_NaN();
 }
 (*lhs_knows)(refnode_number,0)=0;

 FieldInformation fieldinfo("TriSurfMesh", CONSTANTDATA_E, "int");
 FieldHandle elc_sponge_surf = CreateField(fieldinfo);
 VMesh* elc_sponge_surf_vmesh = elc_sponge_surf->vmesh();
 VField* elc_sponge_surf_vfld  = elc_sponge_surf->vfield();
 std::vector<double> field_values, impedances;

 /// map the electrode sponge center (CreateElectrodeCoil) to generated tDCS electrode geometry (Cleaver),
 /// this mapping is meant to map the GUI inputs to actual electrode geometry by having a lookup table
 int electrode_sponges=elc_sponge_location->nrows();
 DenseMatrixHandle lookup(new DenseMatrix(electrode_sponges, 1));
 DenseMatrixHandle distances(new DenseMatrix(electrode_sponges, 1));
 VMesh::Node::index_type didx;
 double distance=0;
 for (long i=0;i<electrode_sponges;i++)
 {
  Point elc((*elc_sponge_location)(i,0),(*elc_sponge_location)(i,1),(*elc_sponge_location)(i,2)),r;
  double min_dis=std::numeric_limits<double>::infinity();
  long found_index=std::numeric_limits<double>::quiet_NaN();
  for(long j=0;j<result.size();j++)
  {
   VMesh*  tmp_mesh = result[j]->vmesh();
   tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);

   tmp_mesh->find_closest_node(distance,r,didx,elc);
   if (distance<min_dis)
   {
     min_dis=distance;
     found_index=j;
   }
  }
  (*lookup)(i,0)=found_index;
  (*distances)(i,0)=min_dis;
 }

 /// throw error if the geometry is too far away from predicted location - further away than sponge thickness
 for (long i=0;i<elc_sponge_location->nrows();i++)
 {
   if ((*distances)(i,0)>(*elc_sponge_location)(i,3))
   {
    std::ostringstream ostr1;
    ostr1 << " distance to electrode " << i << " sponge  = " <<  (*distances)(i,0) << " exceeds defined limit of " << (*elc_sponge_location)(i,3) << std::endl;
    THROW_ALGORITHM_PROCESSING_ERROR(ostr1.str());
   }
 }

 /// determine intersection of electrode sponge and scalp; as well as center of electrode sponge/scalp surface
 FieldInformation fi("PointCloudMesh",0,"double");
 FieldHandle sponge_geometry_centers=CreateField(fi);
 VMesh* mesh_sponge_geometry_centers = sponge_geometry_centers->vmesh();

 VMesh* mesh_scalp_tri_surf  = scalp_tri_surf->vmesh();
 Point p,q; Vector norm;
 VMesh::Node::index_type node_ind;
 distance=std::numeric_limits<double>::infinity();
 DenseMatrixHandle sponge_center_pojected_onto_scalp_index(new DenseMatrix(result.size(), 1));
 DenseMatrixHandle sponge_center_pojected_onto_scalp_normal(new DenseMatrix(result.size(), 3));
 DenseMatrixHandle sponge_center_pojected_onto_scalp(new DenseMatrix(result.size(), 3));

 mesh_scalp_tri_surf->synchronize(Mesh::NODE_LOCATE_E);

 for(long i=0;i<result.size();i++)
 {
  VMesh*  tmp_mesh = result[i]->vmesh();
  tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);
  double avr_x=0,avr_y=0,avr_z=0;
  long number_electrode_nodes=tmp_mesh->num_nodes();

  FieldInformation fi2("PointCloudMesh",0,"double");
  FieldHandle tmp_field=CreateField(fi2);
  VMesh* tmp_field_mesh = tmp_field->vmesh();

  for (VMesh::Node::index_type idx=0; idx<number_electrode_nodes; idx++)
  {
    tmp_mesh->get_center(p,idx);
    avr_x+=p.x();
    avr_y+=p.y();
    avr_z+=p.z();
    if(mesh_scalp_tri_surf->find_closest_node(distance,q,node_ind,p))
    {
     if (distance<identical_node_location_differce)
     {
      tmp_field_mesh->add_point(q);
     }
    }
  }

  avr_x/=(number_electrode_nodes);
  avr_y/=(number_electrode_nodes);
  avr_z/=(number_electrode_nodes);

  Point avr_sponge_geometry(avr_x,avr_y,avr_z);
  mesh_sponge_geometry_centers->add_point(avr_sponge_geometry);
  tmp_field_mesh->synchronize(Mesh::NODE_LOCATE_E);
  tmp_field_mesh->find_closest_node(distance,q,node_ind,avr_sponge_geometry);

  (*sponge_center_pojected_onto_scalp)(i,0)=q.x();
  (*sponge_center_pojected_onto_scalp)(i,1)=q.y();
  (*sponge_center_pojected_onto_scalp)(i,2)=q.z();

  mesh_scalp_tri_surf->find_closest_node(distance,p,node_ind,q);
  (*sponge_center_pojected_onto_scalp_index)(i,0)=node_ind;
 }

 /// determine normal of scalp/electrode sponge inferface center
 mesh_scalp_tri_surf->synchronize(Mesh::NORMALS_E);
 VMesh::Elem::array_type ca;
 for(long j=0;j<result.size();j++)
 {
  VMesh::Node::index_type ii((*sponge_center_pojected_onto_scalp_index)(j,0));
  mesh_scalp_tri_surf->get_normal(norm,ii);
  double x=norm.x(),y=norm.y(),z=norm.z();

  double normal_mag=sqrt(x*x+y*y+z*z); /// normalize vector normal
  x/=normal_mag;
  y/=normal_mag;
  z/=normal_mag;

  (*sponge_center_pojected_onto_scalp_normal)(j,0)=x;
  (*sponge_center_pojected_onto_scalp_normal)(j,1)=y;
  (*sponge_center_pojected_onto_scalp_normal)(j,2)=z;
 }

 VMesh* mesh_elc_tri_surf = scalp_tri_surf->vmesh();
 mesh_elc_tri_surf->synchronize(Mesh::NODE_LOCATE_E);

 int nr_elc_sponge_triangles=0;

 FieldInformation fi3("PointCloudMesh",0,"double");
 FieldHandle estimated_sponge_top_center_points=CreateField(fi3);
 VMesh*  estimated_sponge_top_center_points_vmesh = estimated_sponge_top_center_points->vmesh();

 /// retrieve electrode thickness from last input by using the lookup table
 for(int k=0;k<sponge_center_pojected_onto_scalp_normal->nrows();k++)
 {
  bool normal_traveling_direction_needs_to_be_positive=true;
  double elc_thickness=0;
  bool found=false;

  for (long j=0;j<lookup->nrows();j++)
  {
    if ((*lookup)(j,0)==k)
      {
        elc_thickness=(*elc_sponge_location)((*lookup)(j,0),3);
        found=true;
      }
  }

  if (!found)
  {
   THROW_ALGORITHM_PROCESSING_ERROR("Internal error: Lookup of mesh defined electrode (first input) and definition (fourth input) do not match - check your inputs!");
  }

  /// determine the outgoing normal of the scalp to find sponge/electrode surface
  Point o1((*sponge_center_pojected_onto_scalp)(k,0)+(*sponge_center_pojected_onto_scalp_normal)(k,0)*elc_thickness/2,(*sponge_center_pojected_onto_scalp)(k,1)+(*sponge_center_pojected_onto_scalp_normal)(k,1)*elc_thickness/2,(*sponge_center_pojected_onto_scalp)(k,2)+(*sponge_center_pojected_onto_scalp_normal)(k,2)*elc_thickness/2);
  Point o2((*sponge_center_pojected_onto_scalp)(k,0)-(*sponge_center_pojected_onto_scalp_normal)(k,0)*elc_thickness/2,(*sponge_center_pojected_onto_scalp)(k,1)-(*sponge_center_pojected_onto_scalp_normal)(k,1)*elc_thickness/2,(*sponge_center_pojected_onto_scalp)(k,2)-(*sponge_center_pojected_onto_scalp_normal)(k,2)*elc_thickness/2);

  Vector sp_o1(o1.x()-(*sponge_center_pojected_onto_scalp)(k,0),o1.y()-(*sponge_center_pojected_onto_scalp)(k,1),o1.z()-(*sponge_center_pojected_onto_scalp)(k,2));
  Vector sp_o2(o2.x()-(*sponge_center_pojected_onto_scalp)(k,0),o2.y()-(*sponge_center_pojected_onto_scalp)(k,1),o2.z()-(*sponge_center_pojected_onto_scalp)(k,2));
  Vector current_scalp_normal_at_elec((*sponge_center_pojected_onto_scalp_normal)(k,0),(*sponge_center_pojected_onto_scalp_normal)(k,1),(*sponge_center_pojected_onto_scalp_normal)(k,2));

  double dot_sp_o1 = Dot(sp_o1, current_scalp_normal_at_elec);
  double dot_sp_o2 = Dot(sp_o2, current_scalp_normal_at_elec);

  double x=0,y=0,z=0;

  if( (dot_sp_o1<0 && dot_sp_o2<0) || (dot_sp_o1>0 && dot_sp_o2>0) )
  {
    THROW_ALGORITHM_PROCESSING_ERROR("Internal error: Criteria to find sponge top and bottom failed!");
  } else
  if (dot_sp_o1>0 && dot_sp_o2<0)
  {
    normal_traveling_direction_needs_to_be_positive=true;
    x=(*sponge_center_pojected_onto_scalp)(k,0)+(*sponge_center_pojected_onto_scalp_normal)(k,0)*elc_thickness;
    y=(*sponge_center_pojected_onto_scalp)(k,1)+(*sponge_center_pojected_onto_scalp_normal)(k,1)*elc_thickness;
    z=(*sponge_center_pojected_onto_scalp)(k,2)+(*sponge_center_pojected_onto_scalp_normal)(k,2)*elc_thickness;
  } else
  if (dot_sp_o1<0 && dot_sp_o2>0)
  {
    normal_traveling_direction_needs_to_be_positive=false;
    x=(*sponge_center_pojected_onto_scalp)(k,0)-(*sponge_center_pojected_onto_scalp_normal)(k,0)*elc_thickness;
    y=(*sponge_center_pojected_onto_scalp)(k,1)-(*sponge_center_pojected_onto_scalp_normal)(k,1)*elc_thickness;
    z=(*sponge_center_pojected_onto_scalp)(k,2)-(*sponge_center_pojected_onto_scalp_normal)(k,2)*elc_thickness;
  }

  VMesh*  tmp_mesh = result[k]->vmesh();
  VField* tmp_fld = result[k]->vfield();
  tmp_mesh->synchronize(Mesh::NORMALS_E);

  Point o3(x,y,z),o4; //electrode surface point = additional criterial to identify the sponge top/electrode surface
  tmp_mesh->synchronize(Mesh::NODE_LOCATE_E);
  tmp_mesh->find_closest_node(distance,o4,node_ind,o3);
  estimated_sponge_top_center_points_vmesh->add_point(o4);

  Vector sponge_outwards_normal;
  if (normal_traveling_direction_needs_to_be_positive) /// force normals to point towards electrode
  {
   sponge_outwards_normal.x((*sponge_center_pojected_onto_scalp_normal)(k,0));
   sponge_outwards_normal.y((*sponge_center_pojected_onto_scalp_normal)(k,1));
   sponge_outwards_normal.z((*sponge_center_pojected_onto_scalp_normal)(k,2));
  } else
  {
   sponge_outwards_normal.x(-(*sponge_center_pojected_onto_scalp_normal)(k,0));
   sponge_outwards_normal.y(-(*sponge_center_pojected_onto_scalp_normal)(k,1));
   sponge_outwards_normal.z(-(*sponge_center_pojected_onto_scalp_normal)(k,2));
  }

  /// determine which sponge electrode triangles make up sponge top (using the scalp outgoing normal)
  /// the critia to control this can be adjusted in GUI "elc. normal criteria", -1..1, smaller values more electrode surface
  VMesh::coords_type center;
  VMesh::Face::size_type nrows;
  tmp_mesh->size(nrows);

  for(VMesh::Face::index_type idx=0; idx<nrows; idx++)
  {
   tmp_mesh->get_element_center(center);
   tmp_mesh->get_normal(norm,center,VMesh::Elem::index_type(idx));
   double dot_product=Dot(norm, sponge_outwards_normal);

   if (dot_product>=normal_dot_product_bound_)
   {
    Point o1,o2,o3,o0;
    VMesh::Node::array_type onodes(3);
    tmp_mesh->get_nodes(onodes, idx);

    tmp_mesh->get_center(o1,onodes[0]);
    tmp_mesh->get_center(o2,onodes[1]);
    tmp_mesh->get_center(o3,onodes[2]);
    double dis1=std::numeric_limits<double>::quiet_NaN(),
           dis2=std::numeric_limits<double>::quiet_NaN(),
	   dis3=std::numeric_limits<double>::quiet_NaN();
    mesh_scalp_tri_surf->find_closest_node(dis1,o0,node_ind,o1);
    mesh_scalp_tri_surf->find_closest_node(dis2,o0,node_ind,o2);
    mesh_scalp_tri_surf->find_closest_node(dis3,o0,node_ind,o3);

    if (dis1>identical_node_location_differce && dis2>identical_node_location_differce && dis3>identical_node_location_differce)
    {
      tmp_fld->set_value(static_cast<int>(special_label), idx);
    }
   } else
   {
    tmp_fld->set_value(static_cast<int>(0), idx);
   }
  }

  /// use SplitFieldByDomainAlgo to find only labeled (based on elc. normal criteria) surfaces
  VMesh::Elem::index_type c_ind=0;
  SplitFieldByDomainAlgo algo2;
  algo2.setLogger(getLogger());
  FieldList output;
  algo2.set(SplitFieldByDomainAlgo::SortBySize, true);
  algo2.set(SplitFieldByDomainAlgo::SortAscending, false);
  algo2.runImpl(result[k], output);

  bool found_sponge_surface=false;
  int electrode_surface=-1;

  mesh_scalp_tri_surf->synchronize(Mesh::NODE_LOCATE_E);
  /// find out which of the selected triangles (from previous step) do not coincide with scalp -> those should belong to the electrode sponge top
  for(long o=0;o<output.size();o++)
  {
   VField* tmp_splitbydomain_tri_field = output[o]->vfield();

   int tmp_val=0;
   tmp_splitbydomain_tri_field->get_value(tmp_val,c_ind);

   if( tmp_val == special_label )
   {
       found_sponge_surface=true;
       electrode_surface=o;
   }
  }

  /// use SplitFieldByConnectedRegionAlgo to make sure we have the right triangle surface
  if (!found_sponge_surface || electrode_surface==-1)
  {
    remark(" At least for one electrode the electrode/sponge interface could not be identified.");
    continue;
  } else
  {
    SplitFieldByConnectedRegionAlgo algo3;
    algo3.set(SplitFieldByConnectedRegionAlgo::SortDomainBySize(), true);
    algo3.set(SplitFieldByConnectedRegionAlgo::SortAscending(), false);
    std::vector<FieldHandle> result3 = algo3.run(output[electrode_surface]);
    if (result3.size()>1)
    {
     remark(" This module found two electrode sponge surfaces that are not connected and took the one that has more elements. Visualize the 8th (Field) output to make sure otherwise the simulation could be wrong!");
    }
    if (result3.size()==0)
    {
     THROW_ALGORITHM_PROCESSING_ERROR("Internal error: Cannot find any electrode sponge surface. ");
    }

    /// create eightht output
    FieldHandle tri_mesh = result3[0]; /// use largest surface
    VMesh* tri_mesh_vmesh = tri_mesh->vmesh();
    VMesh::Node::array_type onodes(3);
    for (VMesh::Node::index_type l=0; l<tri_mesh_vmesh->num_nodes(); l++)
    {
      Point point;
      tri_mesh_vmesh->get_center(p,l);
      elc_sponge_surf_vmesh->add_point(p);
    }

    for (VMesh::Elem::index_type l=0; l<tri_mesh_vmesh->num_elems(); l++)
    {
     VMesh::Node::array_type onodes(3);
     tri_mesh_vmesh->get_nodes(onodes, l);
     onodes[0]+=nr_elc_sponge_triangles;
     onodes[1]+=nr_elc_sponge_triangles;
     onodes[2]+=nr_elc_sponge_triangles;
     elc_sponge_surf_vmesh->add_elem(onodes);
     field_values.push_back(k);
     impedances.push_back(impelc[k].toDouble());
    }
    nr_elc_sponge_triangles+=tri_mesh_vmesh->num_nodes();
  }

 }

 /// create matrix outputs
 elc_sponge_surf_vfld->resize_values();
 elc_sponge_surf_vfld->set_values(field_values);

 elc_elem = boost::make_shared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(),1));
 elc_elem_def = boost::make_shared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(),4));
 elc_elem_typ = boost::make_shared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(),1));
 elc_con_imp = boost::make_shared<DenseMatrix>(DenseMatrix::Zero(elc_sponge_surf_vmesh->num_elems(),1));

 if (impedances.size() != elc_sponge_surf_vmesh->num_elems())
 {
    THROW_ALGORITHM_PROCESSING_ERROR("Internal error: Number of impedances and electrode contact surface elements do not match up. ");
 }

 if (elc_sponge_surf_vmesh->num_elems()!=field_values.size())
 {
   THROW_ALGORITHM_PROCESSING_ERROR("Internal error: internal field definition is ");
 }

 mesh_vmesh->synchronize(Mesh::NODE_LOCATE_E);

 std::vector<double> electrode_sponge_areas;
 double area=0.0; double prev_elc=0;
 for(VMesh::Elem::index_type l=0; l<elc_sponge_surf_vmesh->num_elems(); l++)
 {
  (*elc_elem_typ)(l,0)=2; //define triangles to incject currents in TDCS simulations
  VMesh::Node::array_type onodes(3);
  elc_sponge_surf_vmesh->get_nodes(onodes, l);

  elc_sponge_surf_vmesh->get_center(p,onodes[0]);
  mesh_vmesh->find_closest_node(distance,q,node_ind,p);
  (*elc_elem_def)(l,0)=node_ind;
  Point pos;
  mesh_vmesh->get_point(pos, node_ind);
  double x1=pos.x(), y1=pos.y(), z1=pos.z();

  elc_sponge_surf_vmesh->get_center(p,onodes[1]);
  mesh_vmesh->find_closest_node(distance,q,node_ind,p);
  (*elc_elem_def)(l,1)=node_ind;
  mesh_vmesh->get_point(pos, node_ind);
  double x2=pos.x(), y2=pos.y(), z2=pos.z();

  elc_sponge_surf_vmesh->get_center(p,onodes[2]);
  mesh_vmesh->find_closest_node(distance,q,node_ind,p);
  (*elc_elem_def)(l,2)=node_ind;
  mesh_vmesh->get_point(pos, node_ind);
  double x3=pos.x(), y3=pos.y(), z3=pos.z();

  (*elc_elem_def)(l,3)=0;
  (*elc_elem)(l,0)=field_values[l];
  (*elc_con_imp)(l,0)=impedances[l];

  ///compute surface area of electrode/scalp interface
  double area_tmp =y1*z2+z1*y3+y2*z3-z2*y3-z1*y2-y1*z3;
  double area_tmp1=z1*x2+x1*z3+z2*x3-x2*z3-x1*z2-z1*x3;
  double area_tmp2=x1*y2+y1*x3+x2*y3-y2*x3-y1*x2-x1*y3;
  double triangle_area=0.5 * sqrt(area_tmp*area_tmp+area_tmp1*area_tmp1+area_tmp2*area_tmp2);

  double tmp_fld_val=0;
  elc_sponge_surf_vfld->get_value(tmp_fld_val,l);

  if(prev_elc!=tmp_fld_val)
  {
   area*=1e-6; /// convert electrode/sponge surface area from mm^2 to m^2
   electrode_sponge_areas.push_back(area);
   area=0.0;
   prev_elc=tmp_fld_val;
  } else
  {
   area+=triangle_area;
  }
 }
 area*=1e-6;
 electrode_sponge_areas.push_back(area);

 return boost::make_tuple(lhs_knows, elc_elem, elc_elem_typ, elc_elem_def, elc_con_imp,elc_sponge_surf, electrode_sponge_areas);
}

DenseMatrixHandle SetupTDCSAlgorithm::create_rhs(FieldHandle mesh, const std::vector<Variable>& elcs, int num_of_elc) const
{
  /// get the right-hand-side from GUI
  std::vector<Variable, std::allocator<Variable>> elcs_wanted;
  for (int i=0; i<num_of_elc; i++)
    elcs_wanted.push_back(elcs[i]);

  VField* vfield = mesh->vfield();

  // making sure current magnitudes of the electrodes summed are greater than 10e-6
  double min_current = 0;
  for (int i=0; i<elcs_wanted.size(); i++)
  {
    double temp = elcs_wanted[i].toDouble();
    min_current += temp;
  }

  if (std::fabs(min_current) > electode_current_summation_bound)
        THROW_ALGORITHM_INPUT_ERROR("Summed electrode current intensities are greater than 1e-6 mA. The sum should be close to 0 mA !!! ");

  int node_elements  = vfield->vmesh()->num_nodes();
  int total_elements = node_elements + elcs_wanted.size();

  DenseMatrixHandle output (boost::make_shared<DenseMatrix>(total_elements,1));
  int cnt = 0;
  double module_half_done=total_elements*2; /// half of the runtime is reserved for rhs

  /// su
  for (int i=0; i < total_elements; i++)
  {
    if (i < node_elements)
      (*output)(i,0) = 0.0; // for the nodes
    else
      (*output)(i,0) = elcs_wanted[i-node_elements].toDouble()/1000.0; /// for electrodes ~ converting to Amps

    cnt++;
    if (cnt == total_elements/4)
    {
      cnt = 0;
      update_progress_max(i, module_half_done); /// progress bar is devided in 2 parts; first part = create rhs and second for lhs
    }
  }

  return output;
}

boost::tuple<DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, DenseMatrixHandle, FieldHandle, DenseMatrixHandle, std::vector<double>> SetupTDCSAlgorithm::run(FieldHandle mesh, const std::vector<Variable>& elcs, const std::vector<Variable>& impelc, int num_of_elc, FieldHandle scalp_tri_surf, FieldHandle elc_tri_surf, DenseMatrixHandle elc_sponge_location) const
{
  if (num_of_elc > max_number_of_electrodes) { THROW_ALGORITHM_INPUT_ERROR("Number of electrodes (>512) given exceeds what is possible ");}  /// number of  possible electrodes is currently bound to 512 electrodes in default setting
  if (num_of_elc < 0) { THROW_ALGORITHM_INPUT_ERROR("Negative number of electrodes given ");}

  if (!mesh) THROW_ALGORITHM_INPUT_ERROR("Input field (mesh) was not allocated ");
  if (!scalp_tri_surf) THROW_ALGORITHM_INPUT_ERROR("Input field (scalp triangle surface) was not allocated ");
  if (!elc_tri_surf) THROW_ALGORITHM_INPUT_ERROR("Input field (electrode triangle surface) was not allocated ");
  if (!elc_sponge_location) THROW_ALGORITHM_INPUT_ERROR("Input field (electrode triangle surface) was not allocated ");

  DenseMatrixHandle rhs=create_rhs(mesh, elcs, num_of_elc); /// get the right-hand-side

  DenseMatrixHandle lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp;
  FieldHandle elec_sponge_surf;
  std::vector<double> electrode_sponge_areas;
  boost::tie(lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, elec_sponge_surf, electrode_sponge_areas) = create_lhs(mesh, impelc,scalp_tri_surf, elc_tri_surf, elc_sponge_location); /// get various left-hand-side outputs

  DenseMatrixHandle selectmatrixind(new DenseMatrix(mesh->vmesh()->num_nodes(), 1)); ///create indeces for SelectSubMatrix
  for(long i=0;i<mesh->vmesh()->num_nodes();i++)
  {
   (*selectmatrixind)(i,0)=i;
  }

  return boost::make_tuple(lhs_knowns, elc_element, elc_element_typ, elc_element_def, elc_contact_imp, rhs, elec_sponge_surf, selectmatrixind, electrode_sponge_areas);
}


//
// Expansion Hunter
// Copyright 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Xiao Chen <xchen2@illumina.com>
//         Egor Dolzhenko <edolzhenko@illumina.com>
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
//

#pragma once

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

#include "common/Common.hh"

namespace ehunter
{

class CopyNumberGenotyper
{
public:
    CopyNumberGenotyper(
        int maxCopyNumber, double depthScaleFactor, double standardDeviationOfCN2,
        const std::vector<double>& meanDepthValues, const std::vector<double>& priorCopyNumberFreq);

    boost::optional<int> genotype(double normalizedDepth) const;

    // return genotype likelihood and p-value of the given genotype
    std::pair<double, double> genotypeLikelihoodAndPvalue(int currentGenotype, double adjustedDepth) const;
    /**
     * return the best genotype and its posterior probability
     * @param likelihoodOfAllCN vector containing likelihoods of each copy number state
     */
    std::pair<int, double> getBestGenotypeAndPosterior(const std::vector<double>& likelihoodOfAllCN) const;

private:
    int maxCopyNumber_;
    double depthScaleFactor_;
    // standard deviation for copy number 2
    double standardDeviationOfCN2_;
    // vector containing mean values for all possible copy number states
    std::vector<double> meanDepthValues_;
    // vector containing prior frequencies for all possible copy number states
    std::vector<double> priorCopyNumberFreq_;
    // hard-coded parameters
    // standard deviation for copy number 0
    double standardDeviationOfCN0_ = 0.032;
    // posterior probability threshold for calling copy number genotype
    double posteriorProbabilityThreshold_ = 0.95;
    // p-value threshold for calling copy number genotype
    double pvalueThreshold_ = 1e-3;
};
}


/*
 * Copyright 2014-2017, Andrea Del Prete, Rohan Budhiraja LAAS-CNRS
 *
 * This file is part of sot-torque-control.
 * sot-dyninv is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-torque-control 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 Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-torque-control.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <Eigen/Dense>

#include <dynamic-graph/factory.h>
#include <sot/core/debug.hh>
#include <sot/torque_control/numerical-difference.hh>
#include <sot/torque_control/commands-helper.hh>
#include <sot/torque_control/motor-model.hh>

namespace dynamicgraph
{
  namespace sot
  {
    namespace torque_control
    {

#define ALL_INPUT_SIGNALS m_xSIN

#define ALL_OUTPUT_SIGNALS  m_x_filteredSOUT << m_dxSOUT  << m_ddxSOUT

      namespace dynamicgraph = ::dynamicgraph;
      using namespace dynamicgraph;
      using namespace dynamicgraph::command;
      using namespace Eigen;

      /// Define EntityClassName here rather than in the header file
      /// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
      typedef NumericalDifference EntityClassName;

      /* --- DG FACTORY ------------------------------------------------------- */
      DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NumericalDifference,"NumericalDifference");

      /* --- CONSTRUCTION ----------------------------------------------------- */
      /* --- CONSTRUCTION ----------------------------------------------------- */
      /* --- CONSTRUCTION ----------------------------------------------------- */
      NumericalDifference::
      NumericalDifference( const std::string & name )
        : Entity(name),
          CONSTRUCT_SIGNAL_IN(x,                dynamicgraph::Vector)
        ,CONSTRUCT_SIGNAL_OUT(x_filtered,       dynamicgraph::Vector, m_x_dx_ddxSINNER)
        ,CONSTRUCT_SIGNAL_OUT(dx,               dynamicgraph::Vector, m_x_dx_ddxSINNER)
        ,CONSTRUCT_SIGNAL_OUT(ddx,              dynamicgraph::Vector, m_x_dx_ddxSINNER)
        ,CONSTRUCT_SIGNAL_INNER(x_dx_ddx,       dynamicgraph::Vector, m_xSIN)
      {
        Entity::signalRegistration( ALL_INPUT_SIGNALS << ALL_OUTPUT_SIGNALS);

        /* Commands. */
        addCommand("getTimestep",
                   makeDirectGetter(*this,&m_dt,
                                    docDirectGetter("Control timestep [s ]","double")));
        addCommand("getDelay",
                   makeDirectGetter(*this,&m_delay,
                                    docDirectGetter("Delay in the estimation of signal x","double")));
        addCommand("getSize",
                   makeDirectGetter(*this,&x_size,
                                    docDirectGetter("Size of the x signal","int")));
        addCommand("init", makeCommandVoid4(*this, &NumericalDifference::init,
                              docCommandVoid4("Initialize the estimator.",
                                              "Control timestep [s].",
                                              "Size of the input signal x",
                                              "Estimation delay for signal x",
                                              "Polynomial order")));
      }

      /* --- COMMANDS ---------------------------------------------------------- */
      /* --- COMMANDS ---------------------------------------------------------- */
      /* --- COMMANDS ---------------------------------------------------------- */
      void NumericalDifference::init(const double &timestep, const int& xSize,
                                     const double& delay, const int& polyOrder)
      {
        assert(timestep>0.0 && "Timestep should be > 0");
        assert(delay>=1.5*timestep && "Estimation delay should be >= 1.5*timestep");
        m_dt = timestep;
        m_delay = delay;
        x_size = xSize;
        int winSizeEnc     = (int)(2*delay/m_dt);
        assert(winSizeEnc>=3 && "Estimation-window's length should be >= 3");

        if (polyOrder==1)
          m_filter       = new LinEstimator(winSizeEnc, x_size, m_dt);
        else if(polyOrder==2)
          m_filter       = new QuadEstimator(winSizeEnc, x_size, m_dt);
        else
          SEND_MSG("Only polynomial orders 1 and 2 allowed. Reinitialize the filter", MSG_TYPE_INFO);
        m_ddx_filter_std.resize(x_size);
        m_dx_filter_std.resize(x_size);
        m_x_filter_std.resize(x_size);
        m_x_std.resize(x_size);
      }

      /* --- SIGNALS ---------------------------------------------------------- */
      /* --- SIGNALS ---------------------------------------------------------- */
      /* --- SIGNALS ---------------------------------------------------------- */

      /** Signal Filtering and Differentiation. */
      DEFINE_SIGNAL_INNER_FUNCTION(x_dx_ddx, dynamicgraph::Vector)
      {
        sotDEBUG(15)<<"Compute x_dx_ddx inner signal "<<iter<<std::endl;

        // read encoders and copy in std vector
        const dynamicgraph::Vector& base_x = m_xSIN(iter);
        COPY_VECTOR_TO_ARRAY(base_x, m_x_std);

        // Signal Filters
        m_filter->estimate(m_x_filter_std, m_x_std);
        m_filter->getEstimateDerivative(m_dx_filter_std, 1);
        m_filter->getEstimateDerivative(m_ddx_filter_std, 2);

        // copy data in signal vector
        if(s.size()!=3*x_size)
          s.resize(3*x_size);
        for(int i=0; i<x_size; i++)
          s(i) = m_x_filter_std[i];
        for(int i=0; i<x_size; i++)
          s(i+x_size) = m_dx_filter_std[i];
        for(int i=0; i<x_size; i++)
          s(i+2*x_size) = m_ddx_filter_std[i];

        return s;
      }

      /// ************************************************************************* ///
      /// The following signals depend only on other inner signals, so they
      /// just need to copy the interested part of the inner signal they depend on.
      /// ************************************************************************* ///

      DEFINE_SIGNAL_OUT_FUNCTION(x_filtered, dynamicgraph::Vector)
      {
        sotDEBUG(15)<<"Compute x_filtered output signal "<<iter<<std::endl;

        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
        if(s.size()!=x_size)
          s.resize(x_size);
	s = x_dx_ddx.head(x_size);
        return s;
      }

      DEFINE_SIGNAL_OUT_FUNCTION(dx, dynamicgraph::Vector)
      {
        sotDEBUG(15)<<"Compute dx output signal "<<iter<<std::endl;

        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
        if(s.size()!=x_size)
	  s.resize(x_size);
	s = x_dx_ddx.segment(x_size,x_size);
        return s;
      }

      DEFINE_SIGNAL_OUT_FUNCTION(ddx, dynamicgraph::Vector)
      {
        sotDEBUG(15)<<"Compute ddx output signal "<<iter<<std::endl;

        const dynamicgraph::Vector &x_dx_ddx = m_x_dx_ddxSINNER(iter);
        if(s.size()!=x_size)
          s.resize(x_size);
	s = x_dx_ddx.segment(2*x_size, x_size);
        return s;
      }

      void NumericalDifference::display( std::ostream& os ) const
      {
        os << "NumericalDifference "<<getName()<<":\n";
        try
        {
          getProfiler().report_all(3, os);
        }
        catch (ExceptionSignal e) {}
      }

    } // namespace torque_control
  } // namespace sot
} // namespace dynamicgraph


#ifndef __SIMPLIFY_HPP_DEFINED__
#define __SIMPLIFY_HPP_DEFINED__

#include <ast.hpp>
#include <smt.hpp>
#include <stack>
#include <smt.hpp>
#include <rewriter.hpp>
#include <boost/logic/tribool.hpp>

namespace ila
{
    class ITESimplifier
    {
        std::shared_ptr<z3::context> ctx;
        std::shared_ptr<z3::solver> S;
        std::shared_ptr<Z3ExprAdapter> adapter;

        rwmap_t rwmap;

        void _add(const nptr_t& a, bool negate=false);
        boost::logic::tribool _isConstant(const nptr_t& c);

        void getNewArgs(const Node* op, nptr_vec_t& args);
        nptr_t getRepl(const Node* n) const;
        nptr_t rewriteITE(const Node* n);
        void dfs(const Node* n);
    public:
        // constructor.
        ITESimplifier(const nptr_t& assump);
        // constructor with existing contexts, solvers and expr adapters.
        ITESimplifier(
                std::shared_ptr<z3::context> ctx_,
                std::shared_ptr<z3::solver> S_,
                std::shared_ptr<Z3ExprAdapter> adapter_);

        // destructor.
        virtual ~ITESimplifier();
        // add a vector of assumption.
        void addAssumptions(const nptr_vec_t& assumps);
        // add exactly one assumption.
        void addAssumption(const nptr_t& a);
        // create clone
        void createClone(const Node* n);

        void reset() { rwmap.clear(); }
        nptr_t simplify(Node* n);

    };
}
#endif


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef BOOST_MATH_CCMATH
#define BOOST_MATH_CCMATH

#include <boost/math/ccmath/sqrt.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/fabs.hpp>
#include <boost/math/ccmath/isfinite.hpp>
#include <boost/math/ccmath/isnormal.hpp>
#include <boost/math/ccmath/fpclassify.hpp>

#endif // BOOST_MATH_CCMATH


#pragma once

#if defined(_OPENMP)
#include <omp.h>
#endif

#include <chrono>
#include <cmath>
#include <cstdint>
#include <functional>
#include <fstream>
#include <iostream>
#include <stdexcept>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/maybe.hpp>
#include <common_robotics_utilities/openmp_helpers.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#include <voxelized_geometry_tools/signed_distance_field.hpp>

namespace voxelized_geometry_tools
{
namespace signed_distance_field_generation
{
using common_robotics_utilities::voxel_grid::GridIndex;
using common_robotics_utilities::voxel_grid::GridSizes;

struct BucketCell
{
  double distance_square = 0.0;
  int32_t update_direction = 0;
  uint32_t location[3] = {0u, 0u, 0u};
  uint32_t closest_point[3] = {0u, 0u, 0u};
};

typedef common_robotics_utilities::voxel_grid
    ::VoxelGrid<BucketCell, std::vector<BucketCell>> DistanceField;

inline int32_t GetDirectionNumber(
    const int32_t dx, const int32_t dy, const int32_t dz)
{
  return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
}

inline std::vector<std::vector<std::vector<std::vector<int32_t>>>>
MakeNeighborhoods()
{
  // First vector<>: 2 - the first bucket queue, the points we know are zero
  // distance, start with a complete set of neighbors to check. Every other
  // bucket queue checks fewer neighbors.
  // Second vector<>: 27 (# of source directions in fully-connected 3d grid).
  // Third vector<>:
  std::vector<std::vector<std::vector<std::vector<int32_t>>>> neighborhoods;
  // I don't know why there are 2 initial neighborhoods.
  neighborhoods.resize(2);
  for (size_t n = 0; n < neighborhoods.size(); n++)
  {
    neighborhoods[n].resize(27);
    // Loop through the source directions.
    for (int32_t dx = -1; dx <= 1; dx++)
    {
      for (int32_t dy = -1; dy <= 1; dy++)
      {
        for (int32_t dz = -1; dz <= 1; dz++)
        {
          const int32_t direction_number = GetDirectionNumber(dx, dy, dz);
          // Loop through the target directions.
          for (int32_t tdx = -1; tdx <= 1; tdx++)
          {
            for (int32_t tdy = -1; tdy <= 1; tdy++)
            {
              for (int32_t tdz = -1; tdz <= 1; tdz++)
              {
                // Ignore the case of ourself.
                if (tdx == 0 && tdy == 0 && tdz == 0)
                {
                  continue;
                }
                // Why is one set of neighborhoods larger than the other?
                if (n >= 1)
                {
                  if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
                  {
                    continue;
                  }
                  if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
                  {
                    continue;
                  }
                }
                std::vector<int32_t> new_point;
                new_point.resize(3);
                new_point[0] = tdx;
                new_point[1] = tdy;
                new_point[2] = tdz;
                neighborhoods[n][direction_number].push_back(new_point);
              }
            }
          }
        }
      }
    }
  }
  return neighborhoods;
}

inline double ComputeDistanceSquared(
    const int32_t x1, const int32_t y1, const int32_t z1,
    const int32_t x2, const int32_t y2, const int32_t z2)
{
  const int32_t dx = x1 - x2;
  const int32_t dy = y1 - y2;
  const int32_t dz = z1 - z2;
  return double((dx * dx) + (dy * dy) + (dz * dz));
}

class MultipleThreadIndexQueueWrapper
{
public:

  explicit MultipleThreadIndexQueueWrapper(const size_t max_queues)
  {
    per_thread_queues_.resize(
        common_robotics_utilities::openmp_helpers::GetNumOmpThreads(),
        ThreadIndexQueues(max_queues));
  }

  const GridIndex& Query(const int32_t distance_squared, const size_t idx) const
  {
    size_t working_index = idx;
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      const auto& current_thread_queue =
          per_thread_queues_.at(thread).at(distance_squared);
      const size_t current_thread_queue_size = current_thread_queue.size();
      if (working_index < current_thread_queue_size)
      {
        return current_thread_queue.at(working_index);
      }
      else
      {
        working_index -= current_thread_queue_size;
      }
    }
    throw std::runtime_error("Failed to find item");
  }

  size_t NumQueues() const
  {
    return per_thread_queues_.at(0).size();
  }

  size_t Size(const int32_t distance_squared) const
  {
    size_t total_size = 0;
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      total_size += per_thread_queues_.at(thread).at(distance_squared).size();
    }
    return total_size;
  }

  void Enqueue(const int32_t distance_squared, const GridIndex& index)
  {
    const int32_t thread_num
        = common_robotics_utilities::openmp_helpers::GetContextOmpThreadNum();
    per_thread_queues_.at(thread_num).at(distance_squared).push_back(index);
  }

  void ClearCompletedQueues(const int32_t distance_squared)
  {
    for (size_t thread = 0; thread < per_thread_queues_.size(); thread++)
    {
      per_thread_queues_.at(thread).at(distance_squared).clear();
    }
  }

private:
  typedef std::vector<std::vector<GridIndex>> ThreadIndexQueues;
  std::vector<ThreadIndexQueues> per_thread_queues_;

};

inline DistanceField BuildDistanceFieldSerial(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes, const std::vector<GridIndex>& points)
{
  if (!grid_sizes.UniformCellSize())
  {
    throw std::invalid_argument(
        "Cannot build distance field from grid with non-uniform cells");
  }
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  // Make the DistanceField container
  BucketCell default_cell;
  default_cell.distance_square = std::numeric_limits<double>::infinity();
  DistanceField distance_field(grid_origin_transform, grid_sizes, default_cell);
  // Compute maximum distance square
  const int64_t max_distance_square =
      (distance_field.GetNumXCells() * distance_field.GetNumXCells())
      + (distance_field.GetNumYCells() * distance_field.GetNumYCells())
      + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
  // Make bucket queue
  std::vector<std::vector<BucketCell>> bucket_queue(max_distance_square + 1);
  bucket_queue[0].reserve(points.size());
  // Set initial update direction
  int32_t initial_update_direction = GetDirectionNumber(0, 0, 0);
  // Mark all provided points with distance zero and add to the bucket queue
  for (size_t index = 0; index < points.size(); index++)
  {
    const GridIndex& current_index = points[index];
    auto query = distance_field.GetMutable(current_index);
    if (query)
    {
      query.Value().location[0] = static_cast<uint32_t>(current_index.X());
      query.Value().location[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().location[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().closest_point[0] = static_cast<uint32_t>(current_index.X());
      query.Value().closest_point[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().closest_point[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().distance_square = 0.0;
      query.Value().update_direction = initial_update_direction;
      bucket_queue[0].push_back(query.Value());
    }
    // If the point is outside the bounds of the SDF, skip
    else
    {
      throw std::runtime_error("Point for BuildDistanceField out of bounds");
    }
  }
  // HERE BE DRAGONS
  // Process the bucket queue
  const std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods =
      MakeNeighborhoods();
  for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
  {
    for (const auto& cur_cell : bucket_queue[bq_idx])
    {
      // Get the current location
      const double x = cur_cell.location[0];
      const double y = cur_cell.location[1];
      const double z = cur_cell.location[2];
      // Pick the update direction
      // Only the first bucket queue gets the larger set of neighborhoods?
      // Don't really userstand why.
      const size_t direction_switch = (bq_idx > 0) ? 1 : 0;
      // Make sure the update direction is valid
      if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
      {
        continue;
      }
      // Get the current neighborhood list
      const std::vector<std::vector<int>>& neighborhood =
          neighborhoods[direction_switch][cur_cell.update_direction];
      // Update the distance from the neighboring cells
      for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
      {
        // Get the direction to check
        const int32_t dx = neighborhood[nh_idx][0];
        const int32_t dy = neighborhood[nh_idx][1];
        const int32_t dz = neighborhood[nh_idx][2];
        const int32_t nx = static_cast<int32_t>(x + dx);
        const int32_t ny = static_cast<int32_t>(y + dy);
        const int32_t nz = static_cast<int32_t>(z + dz);
        auto neighbor_query =
            distance_field.GetMutable(static_cast<int64_t>(nx),
                                      static_cast<int64_t>(ny),
                                      static_cast<int64_t>(nz));
        if (!neighbor_query)
        {
          // "Neighbor" is outside the bounds of the SDF
          continue;
        }
        // Update the neighbor's distance based on the current
        const int32_t new_distance_square =
            static_cast<int32_t>(ComputeDistanceSquared(
                                   nx, ny, nz,
                                   cur_cell.closest_point[0],
                                   cur_cell.closest_point[1],
                                   cur_cell.closest_point[2]));
        if (new_distance_square > max_distance_square)
        {
          // Skip these cases
          continue;
        }
        if (new_distance_square < neighbor_query.Value().distance_square)
        {
          // If the distance is better, time to update the neighbor
          neighbor_query.Value().distance_square = new_distance_square;
          neighbor_query.Value().closest_point[0] = cur_cell.closest_point[0];
          neighbor_query.Value().closest_point[1] = cur_cell.closest_point[1];
          neighbor_query.Value().closest_point[2] = cur_cell.closest_point[2];
          neighbor_query.Value().location[0] = nx;
          neighbor_query.Value().location[1] = ny;
          neighbor_query.Value().location[2] = nz;
          neighbor_query.Value().update_direction =
              GetDirectionNumber(dx, dy, dz);
          // Add the neighbor into the bucket queue
          bucket_queue[new_distance_square].push_back(neighbor_query.Value());
        }
      }
    }
    // Clear the current queue now that we're done with it
    bucket_queue[bq_idx].clear();
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed DistanceField in " << elapsed.count() << " seconds"
            << std::endl;
  return distance_field;
}

inline DistanceField BuildDistanceFieldParallel(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes,
    const std::vector<GridIndex>& points)
{
  if (!grid_sizes.UniformCellSize())
  {
    throw std::invalid_argument(
        "Cannot build distance field from grid with non-uniform cells");
  }
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  // Make the DistanceField container
  BucketCell default_cell;
  default_cell.distance_square = std::numeric_limits<double>::infinity();
  DistanceField distance_field(grid_origin_transform, grid_sizes, default_cell);
  // Compute maximum distance square
  const int64_t max_distance_square =
      (distance_field.GetNumXCells() * distance_field.GetNumXCells())
      + (distance_field.GetNumYCells() * distance_field.GetNumYCells())
      + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
  // Make bucket queue
  std::vector<std::vector<BucketCell>> bucket_queue(max_distance_square + 1);
  bucket_queue[0].reserve(points.size());
  MultipleThreadIndexQueueWrapper bucket_queues(max_distance_square + 1);
  // Set initial update direction
  int32_t initial_update_direction = GetDirectionNumber(0, 0, 0);
  // Mark all provided points with distance zero and add to the bucket queues
  // points MUST NOT CONTAIN DUPLICATE ENTRIES!
#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t index = 0; index < points.size(); index++)
  {
    const GridIndex& current_index = points[index];
    auto query = distance_field.GetMutable(current_index);
    if (query)
    {
      query.Value().location[0] = static_cast<uint32_t>(current_index.X());
      query.Value().location[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().location[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().closest_point[0] = static_cast<uint32_t>(current_index.X());
      query.Value().closest_point[1] = static_cast<uint32_t>(current_index.Y());
      query.Value().closest_point[2] = static_cast<uint32_t>(current_index.Z());
      query.Value().distance_square = 0.0;
      query.Value().update_direction = initial_update_direction;
      bucket_queues.Enqueue(0, current_index);
    }
    // If the point is outside the bounds of the SDF, skip
    else
    {
      throw std::runtime_error("Point for BuildDistanceField out of bounds");
    }
  }
  // HERE BE DRAGONS
  // Process the bucket queue
  const std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods =
      MakeNeighborhoods();
  for (int32_t current_distance_square = 0;
       current_distance_square
           < static_cast<int32_t>(bucket_queues.NumQueues());
       current_distance_square++)
  {
#if defined(_OPENMP)
#pragma omp parallel for
#endif
    for (size_t idx = 0; idx < bucket_queues.Size(current_distance_square);
         idx++)
    {
      const GridIndex& current_index =
          bucket_queues.Query(current_distance_square, idx);
      // Get the current location
      const BucketCell& cur_cell =
          distance_field.GetImmutable(current_index).Value();
      const double x = cur_cell.location[0];
      const double y = cur_cell.location[1];
      const double z = cur_cell.location[2];
      // Pick the update direction
      // Only the first bucket queue gets the larger set of neighborhoods?
      // Don't really userstand why.
      const size_t direction_switch = (current_distance_square > 0) ? 1 : 0;
      // Make sure the update direction is valid
      if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
      {
        continue;
      }
      // Get the current neighborhood list
      const std::vector<std::vector<int32_t>>& neighborhood =
          neighborhoods[direction_switch][cur_cell.update_direction];
      // Update the distance from the neighboring cells
      for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
      {
        // Get the direction to check
        const int32_t dx = neighborhood[nh_idx][0];
        const int32_t dy = neighborhood[nh_idx][1];
        const int32_t dz = neighborhood[nh_idx][2];
        const int32_t nx = static_cast<int32_t>(x + dx);
        const int32_t ny = static_cast<int32_t>(y + dy);
        const int32_t nz = static_cast<int32_t>(z + dz);
        const GridIndex neighbor_index(static_cast<int64_t>(nx),
                                       static_cast<int64_t>(ny),
                                       static_cast<int64_t>(nz));
        auto neighbor_query = distance_field.GetMutable(neighbor_index);
        if (!neighbor_query)
        {
          // "Neighbor" is outside the bounds of the SDF
          continue;
        }
        // Update the neighbor's distance based on the current
        const int32_t new_distance_square =
            static_cast<int32_t>(ComputeDistanceSquared(
                                   nx, ny, nz,
                                   cur_cell.closest_point[0],
                                   cur_cell.closest_point[1],
                                   cur_cell.closest_point[2]));
        if (new_distance_square > max_distance_square)
        {
          // Skip these cases
          continue;
        }
        if (new_distance_square < neighbor_query.Value().distance_square)
        {
          // If the distance is better, time to update the neighbor
          neighbor_query.Value().distance_square = new_distance_square;
          neighbor_query.Value().closest_point[0] = cur_cell.closest_point[0];
          neighbor_query.Value().closest_point[1] = cur_cell.closest_point[1];
          neighbor_query.Value().closest_point[2] = cur_cell.closest_point[2];
          neighbor_query.Value().location[0] = nx;
          neighbor_query.Value().location[1] = ny;
          neighbor_query.Value().location[2] = nz;
          neighbor_query.Value().update_direction =
              GetDirectionNumber(dx, dy, dz);
          // Add the neighbor into the bucket queue
          bucket_queues.Enqueue(new_distance_square, neighbor_index);
        }
      }
    }
    // Clear the current queues now that we're done with it
    bucket_queues.ClearCompletedQueues(current_distance_square);
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed DistanceField in " << elapsed.count() << " seconds"
            << std::endl;
  return distance_field;
}

inline DistanceField BuildDistanceField(
    const Eigen::Isometry3d& grid_origin_transform,
    const GridSizes& grid_sizes,
    const std::vector<GridIndex>& points,
    const bool use_parallel)
{
  if (use_parallel)
  {
    return BuildDistanceFieldParallel(
        grid_origin_transform, grid_sizes, points);
  }
  else
  {
    return BuildDistanceFieldSerial(grid_origin_transform, grid_sizes, points);
  }
}

template<typename SDFBackingStore>
class SignedDistanceFieldResult
{
private:
  SignedDistanceField<SDFBackingStore> distance_field_;
  double maximum_ = 0.0;
  double minimum_ = 0.0;

public:
  SignedDistanceFieldResult(
      const SignedDistanceField<SDFBackingStore>& distance_field,
      const double maximum, const double minimum)
      : distance_field_(distance_field),
        maximum_(maximum), minimum_(minimum)
  {
    if (minimum_ > maximum_)
    {
      throw std::invalid_argument("minimum_ > maximum_");
    }
  }

  const SignedDistanceField<SDFBackingStore>& DistanceField() const
  {
    return distance_field_;
  }

  const SignedDistanceField<SDFBackingStore>& MutableDistanceField()
  {
    return distance_field_;
  }

  double Maximum() const { return maximum_; }

  double Minimum() const { return minimum_; }
};

template<typename SDFBackingStore>
SignedDistanceFieldResult<SDFBackingStore> MakeSignedDistanceFieldResult(
    const SignedDistanceField<SDFBackingStore>& signed_distance_field,
    const double maximum, const double minimum)
{
  return SignedDistanceFieldResult<SDFBackingStore>(
      signed_distance_field, maximum, minimum);
}

template<typename T, typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const Eigen::Isometry3d& grid_origin_tranform, const GridSizes& grid_sizes,
    const std::function<bool(const GridIndex&)>& is_filled_fn,
    const float oob_value, const std::string& frame, const bool use_parallel)
{
  const std::chrono::time_point<std::chrono::steady_clock> start_time
      = std::chrono::steady_clock::now();
  std::vector<GridIndex> filled;
  std::vector<GridIndex> free;
  for (int64_t x_index = 0; x_index < grid_sizes.NumXCells(); x_index++)
  {
    for (int64_t y_index = 0; y_index < grid_sizes.NumYCells(); y_index++)
    {
      for (int64_t z_index = 0; z_index < grid_sizes.NumZCells(); z_index++)
      {
        const GridIndex current_index(x_index, y_index, z_index);
        if (is_filled_fn(current_index))
        {
          // Mark as filled
          filled.push_back(current_index);
        }
        else
        {
          // Mark as free space
          free.push_back(current_index);
        }
      }
    }
  }
  // Make two distance fields, one for distance to filled voxels, one for
  // distance to free voxels.
  const DistanceField filled_distance_field =
      BuildDistanceField(
        grid_origin_tranform, grid_sizes, filled, use_parallel);
  const DistanceField free_distance_field =
      BuildDistanceField(grid_origin_tranform, grid_sizes, free, use_parallel);
  // Generate the SDF
  SignedDistanceField<SDFBackingStore> new_sdf(
      grid_origin_tranform, frame, grid_sizes, oob_value);
  double max_distance = -std::numeric_limits<double>::infinity();
  double min_distance = std::numeric_limits<double>::infinity();
  for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
  {
    for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
    {
      for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
      {
        const double distance1 =
            std::sqrt(
                filled_distance_field.GetImmutable(x_index, y_index, z_index)
                    .Value().distance_square)
            * new_sdf.GetResolution();
        const double distance2 =
            std::sqrt(
                free_distance_field.GetImmutable(x_index, y_index, z_index)
                    .Value().distance_square)
            * new_sdf.GetResolution();
        const double distance = distance1 - distance2;
        if (distance > max_distance)
        {
          max_distance = distance;
        }
        if (distance < min_distance)
        {
          min_distance = distance;
        }
        new_sdf.SetValue(
            x_index, y_index, z_index, static_cast<float>(distance));
      }
    }
  }
  const std::chrono::time_point<std::chrono::steady_clock> end_time
      = std::chrono::steady_clock::now();
  const std::chrono::duration<double> elapsed = end_time - start_time;
  std::cout << "Computed SDF for grid size in " << elapsed.count() << " seconds"
            << std::endl;
  return MakeSignedDistanceFieldResult<SDFBackingStore>(
      new_sdf, max_distance, min_distance);
}

template<typename T, typename BackingStore=std::vector<T>,
         typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const common_robotics_utilities::voxel_grid
        ::VoxelGridBase<T, BackingStore>& grid,
    const std::function<bool(const GridIndex&)>& is_filled_fn,
    const float oob_value, const std::string& frame,
    const bool use_parallel, const bool add_virtual_border)
{
  if (!grid.HasUniformCellSize())
  {
    throw std::invalid_argument("Grid must have uniform resolution");
  }
  if (add_virtual_border == false)
  {
    // This is the conventional single-pass result
    return ExtractSignedDistanceField<T, SDFBackingStore>(
        grid.GetOriginTransform(), grid.GetGridSizes(), is_filled_fn, oob_value,
        frame, use_parallel);
  }
  else
  {
    const int64_t x_axis_size_offset =
        (grid.GetNumXCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t x_axis_query_offset =
        (grid.GetNumXCells() > 1) ? INT64_C(1) : INT64_C(0);
    const int64_t y_axis_size_offset =
        (grid.GetNumYCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t y_axis_query_offset =
        (grid.GetNumYCells() > 1) ? INT64_C(1) : INT64_C(0);
    const int64_t z_axis_size_offset =
        (grid.GetNumZCells() > 1) ? INT64_C(2) : INT64_C(0);
    const int64_t z_axis_query_offset =
        (grid.GetNumZCells() > 1) ? INT64_C(1) : INT64_C(0);
    // We need to lie about the size of the grid to add a virtual border
    const int64_t num_x_cells = grid.GetNumXCells() + x_axis_size_offset;
    const int64_t num_y_cells = grid.GetNumYCells() + y_axis_size_offset;
    const int64_t num_z_cells = grid.GetNumZCells() + z_axis_size_offset;
    // Make some deceitful helper functions that hide our lies about size
    // For the free space SDF, we lie and say the virtual border is filled
    const std::function<bool(const GridIndex&)> free_is_filled_fn
        = [&] (const GridIndex& virtual_border_grid_index)
    {
      // Is there a virtual border on our axis?
      if (x_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.X() == 0)
            || (virtual_border_grid_index.X() == (num_x_cells - 1)))
        {
          return true;
        }
      }
      // Is there a virtual border on our axis?
      if (y_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Y() == 0)
            || (virtual_border_grid_index.Y() == (num_y_cells - 1)))
        {
          return true;
        }
      }
      // Is there a virtual border on our axis?
      if (z_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Z() == 0)
            || (virtual_border_grid_index.Z() == (num_z_cells - 1)))
        {
          return true;
        }
      }
      const GridIndex real_grid_index(
            virtual_border_grid_index.X() - x_axis_query_offset,
            virtual_border_grid_index.Y() - y_axis_query_offset,
            virtual_border_grid_index.Z() - z_axis_query_offset);
      return is_filled_fn(real_grid_index);
    };
    // For the filled space SDF, we lie and say the virtual border is empty
    const std::function<bool(const GridIndex&)> filled_is_filled_fn
        = [&] (const GridIndex& virtual_border_grid_index)
    {
      // Is there a virtual border on our axis?
      if (x_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.X() == 0)
            || (virtual_border_grid_index.X() == (num_x_cells - 1)))
        {
          return false;
        }
      }
      // Is there a virtual border on our axis?
      if (y_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Y() == 0)
            || (virtual_border_grid_index.Y() == (num_y_cells - 1)))
        {
          return false;
        }
      }
      // Is there a virtual border on our axis?
      if (z_axis_size_offset > 0)
      {
        // Are we a virtual border cell?
        if ((virtual_border_grid_index.Z() == 0)
            || (virtual_border_grid_index.Z() == (num_z_cells - 1)))
        {
          return false;
        }
      }
      const GridIndex real_grid_index(
            virtual_border_grid_index.X() - x_axis_query_offset,
            virtual_border_grid_index.Y() - y_axis_query_offset,
            virtual_border_grid_index.Z() - z_axis_query_offset);
      return is_filled_fn(real_grid_index);
    };
    // Make both SDFs
    const common_robotics_utilities::voxel_grid::GridSizes enlarged_sizes(
        grid.GetCellSizes().x(), num_x_cells, num_y_cells, num_z_cells);
    auto free_sdf_result
        = ExtractSignedDistanceField<T>(
            grid.GetOriginTransform(), enlarged_sizes, free_is_filled_fn,
            oob_value, frame, use_parallel);
    auto filled_sdf_result
        = ExtractSignedDistanceField<T>(
            grid.GetOriginTransform(), enlarged_sizes, filled_is_filled_fn,
            oob_value, frame, use_parallel);
    // Combine to make a single SDF
    SignedDistanceField<SDFBackingStore> combined_sdf(
          grid.GetOriginTransform(), frame, grid.GetGridSizes(), oob_value);
    for (int64_t x_idx = 0; x_idx < combined_sdf.GetNumXCells(); x_idx++)
    {
      for (int64_t y_idx = 0; y_idx < combined_sdf.GetNumYCells(); y_idx++)
      {
        for (int64_t z_idx = 0; z_idx < combined_sdf.GetNumZCells(); z_idx++)
        {
          const int64_t query_x_idx = x_idx + x_axis_query_offset;
          const int64_t query_y_idx = y_idx + y_axis_query_offset;
          const int64_t query_z_idx = z_idx + z_axis_query_offset;
          const float free_sdf_value
              = free_sdf_result.DistanceField().GetImmutable(
                  query_x_idx, query_y_idx, query_z_idx).Value();
          const float filled_sdf_value
              = filled_sdf_result.DistanceField().GetImmutable(
                  query_x_idx, query_y_idx, query_z_idx).Value();
          if (free_sdf_value >= 0.0)
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, free_sdf_value);
          }
          else if (filled_sdf_value <= -0.0)
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, filled_sdf_value);
          }
          else
          {
            combined_sdf.SetValue(x_idx, y_idx, z_idx, 0.0f);
          }
        }
      }
    }
    // Get the combined max/min values
    return MakeSignedDistanceFieldResult<SDFBackingStore>(
        combined_sdf, free_sdf_result.Maximum(), filled_sdf_result.Minimum());
  }
}

template<typename T, typename BackingStore=std::vector<T>,
         typename SDFBackingStore=std::vector<float>>
inline SignedDistanceFieldResult<SDFBackingStore> ExtractSignedDistanceField(
    const common_robotics_utilities::voxel_grid
        ::VoxelGridBase<T, BackingStore>& grid,
    const std::function<bool(const T&)>& is_filled_fn,
    const float oob_value, const std::string& frame,
    const bool use_parallel)
{
  if (!grid.HasUniformCellSize())
  {
    throw std::invalid_argument("Grid must have uniform resolution");
  }
  const std::function<bool(const GridIndex&)> real_is_filled_fn =
      [&] (const GridIndex& index)
  {
    const T& stored = grid.GetImmutable(index).Value();
    // If it matches an object to use OR there are no objects supplied
    if (is_filled_fn(stored))
    {
      // Mark as filled
      return true;
    }
    else
    {
      // Mark as free space
      return false;
    }
  };
  return ExtractSignedDistanceField<T, BackingStore, SDFBackingStore>(
        grid, real_is_filled_fn, oob_value, frame, use_parallel, false);
}
}  // namespace signed_distance_field_generation
}  // namespace voxelized_geometry_tools


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/rem_pio2.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/pio_2.hpp>

STF_CASE_TPL (" rem_pio2",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::rem_pio2;
  using iT = bd::as_integer_t<T>;
  using r_t = decltype(rem_pio2(T()));

  // specific values tests
  using r_t0 = typename boost::fusion::result_of::value_at_c<r_t,0>::type;
  using r_t1 = typename boost::fusion::result_of::value_at_c<r_t,1>::type;
  using r_t2 = typename boost::fusion::result_of::value_at_c<r_t,2>::type;

  {

    r_t res = rem_pio2(bs::Zero<T>());
    STF_ULP_EQUAL( boost::fusion::get<0>(res), bs::Zero<r_t0>(), 1);
    STF_ULP_EQUAL( boost::fusion::get<1>(res), bs::Zero<r_t1>(), 1);
    STF_ULP_EQUAL( boost::fusion::get<2>(res), bs::Zero<r_t2>(), 1);
    STF_ULP_EQUAL( boost::fusion::get<0>(res), bs::Zero<T>(), 1);
    STF_ULP_EQUAL( boost::fusion::get<1>(res), bs::Zero<T>(), 1);
    STF_ULP_EQUAL( boost::fusion::get<2>(res), bs::Zero<iT>(), 1);
    T xr;
    iT n = rem_pio2(bs::Zero<T>(), xr);
    STF_ULP_EQUAL( xr, bs::Zero<T>(), 1);
    STF_ULP_EQUAL( n, bs::Zero<iT>(), 1);

  }
} // end of test for floating_

STF_CASE_TPL (" rem_pio2 targeted",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::rem_pio2;
  using iT = bd::as_integer_t<T>;

  iT n;
  T x = bs::Pio_2<T>(), xr;
  n = rem_pio2(x, xr, bd::as_<bs::tag::big_tag>());
  STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL( n, bs::One<iT>(), 0);

  n = rem_pio2(x, xr, bd::as_<bs::tag::medium_tag>());
  STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL( n, bs::One<iT>(), 0);

  n = rem_pio2(x, xr, bd::as_<bs::tag::small_tag>());
  STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL( n, bs::One<iT>(), 0);

  n = rem_pio2(x, xr, bd::as_<bs::tag::very_small_tag>());
  STF_ULP_EQUAL( xr, bs::Zero<T>(), 0.5);
  STF_ULP_EQUAL( n, bs::One<iT>(), 0);
}