#include <cstdio>
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <loopclosure-common/types.h>
#include <maplab-common/test/testing-entrypoint.h>

#include "vocabulary-tree/simple-kmeans.h"
#include "vocabulary-tree/tree-builder.h"

static const int kDescriptorBytes = 64;

TEST(VocabularyTree, VocabularyTreeSerialization) {
  std::mt19937 generator(42);
  static const uint32_t K = 5;
  static const uint32_t LEVELS = 2;

  using loop_closure::DescriptorType;
  std::vector<DescriptorType> descriptor_refs;
  loop_closure::DescriptorContainer descriptors;
  descriptors.resize(kDescriptorBytes, 40 * pow(K, LEVELS));
  for (int i = 0; i < descriptors.cols(); ++i) {
    DescriptorType descriptor_ref(
        &descriptors(0, i), descriptors.rows(), false);
    descriptor_ref.SetRandom(generator());
    descriptor_refs.push_back(descriptor_ref);
  }

  typedef loop_closure::TreeBuilder<
      DescriptorType, loop_closure::distance::Hamming<DescriptorType> >
      TreeBuilder;
  DescriptorType descriptor_zero(kDescriptorBytes);
  descriptor_zero.SetZero();
  TreeBuilder builder(descriptor_zero);
  builder.kmeans().SetRestarts(5);
  builder.Build(descriptor_refs, K, LEVELS);

  const typename TreeBuilder::Tree& tree_save = builder.tree();
  std::string filename = "./binary_vt.tree";
  tree_save.Save(filename);

  ASSERT_GT(tree_save.levels(), static_cast<unsigned int>(0));
  ASSERT_GT(tree_save.splits(), static_cast<unsigned int>(0));
  ASSERT_GT(tree_save.words(), static_cast<unsigned int>(0));

  TreeBuilder::Tree load_tree;

  ASSERT_FALSE(tree_save.IsBinaryEqual(load_tree));

  load_tree.Load(filename);

  ASSERT_TRUE(tree_save.IsBinaryEqual(load_tree));
}

MAPLAB_UNITTEST_ENTRYPOINT


#include <iostream>
#include <memory>
#include <string>
#include <chrono>

#include <boost/program_options.hpp>
#include <algorithms/Grover.hpp>
#include <algorithms/QFT.hpp>
#include <algorithms/Entanglement.hpp>

#include "Simulator.hpp"
#include "QFRSimulator.hpp"
#include "GroverSimulator.hpp"
#include "ShorFastSimulator.hpp"
#include "ShorSimulator.hpp"

int main(int argc, char** argv) {
    namespace po = boost::program_options;
    unsigned long long seed;

    po::options_description description("JKQ DDSIM by https://iic.jku.at/eda/ -- Allowed options");
    description.add_options()
            ("help,h", "produce help message")
            ("seed", po::value<unsigned long long>(&seed)->default_value(0), "seed for random number generator (default zero is possibly directly used as seed!)")
            ("shots", po::value<unsigned int>()->default_value(0), "number of measurements (if the algorithm does not contain non-unitary gates, weak simulation is used)")
            ("display_vector", "display the state vector")
            ("ps", "print simulation stats (applied gates, sim. time, and maximal size of the DD)")
            ("verbose", "Causes some simulators to print additional information to STDERR")
            ("benchmark", "print simulation stats in a single CSV style line (overrides --ps and suppresses most other output, please don't rely on the format across versions)")

            ("simulate_file", po::value<std::string>(), "simulate a quantum circuit given by file (detection by the file extension)")
            ("simulate_qft", po::value<unsigned int>(), "simulate Quantum Fourier Transform for given number of qubits")
            ("simulate_ghz", po::value<unsigned int>(), "simulate state preparation of GHZ state for given number of qubits")
            ("step_fidelity", po::value<double>()->default_value(1.0), "target fidelity for each approximation run (>=1 = disable approximation)")
            ("steps", po::value<unsigned int>()->default_value(1), "number of approximation steps")
            ("initial_reorder", po::value<int>()->default_value(0), "Try to find a good initial variable order (0=None, 1=Most affected qubits to the top, 2=Most affected targets to the top)")
            ("dynamic_reorder", po::value<int>()->default_value(0), "Apply reordering strategy during simulation (0=None, 1=Sifting, 2=Move2Top)")
            ("post_reorder", po::value<int>()->default_value(0), "Apply a reordering strategy after simulation (0=None, 1=Sifting)")

            ("simulate_grover", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle")
            ("simulate_grover_emulated", po::value<unsigned int>(), "simulate Grover's search for given number of qubits with random oracle and emulation")
            ("simulate_grover_oracle_emulated", po::value<std::string>(), "simulate Grover's search for given number of qubits with given oracle and emulation")

            ("simulate_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number")
            ("simulate_shor_coprime", po::value<unsigned int>()->default_value(0), "coprime number to use with Shor's algorithm (zero randomly generates a coprime)")
            ("simulate_shor_no_emulation", "Force Shor simulator to do modular exponentiation instead of using emulation (you'll usually want emulation)")

            ("simulate_fast_shor", po::value<unsigned int>(), "simulate Shor's algorithm factoring this number with intermediate measurements")
            ("simulate_fast_shor_coprime", po::value<unsigned int>()->default_value(0), "coprime number to use with Shor's algorithm (zero randomly generates a coprime)")
            ("noise_effects", po::value<std::string>(),
             "Noise effects (A (=amplitude damping),D (=depolarization),P (=phase flip)) in the form of a character string describing the noise effects (default=\" \")")
            ("noise_prob", po::value<double>(), "Probability for applying noise (default=0.001)")
            ("confidence", po::value<double>(),
             "Confidence in the error bound of the stochastic simulation (default= 0.05)")
            ("error_bound", po::value<double>(), "Error bound of the stochastic simulation (default=0.1)")
            ("stoch_runs", po::value<long>()->default_value(0), "Number of stochastic runs. When the value is 0 the value is calculated using the confidence, error_bound and number of tracked properties. (default = 0)")
            ("properties", po::value<std::string>(), R"(Comma separated list of tracked properties. Note that -1 is the fidelity and "-" can be used to specify a range.  (default="0-1000"))")
            ;
    po::variables_map vm;
    try {
        po::store(po::parse_command_line(argc, argv, description), vm);

        if (vm.count("help")) {
            std::cout << description;
            return 0;
        }
        po::notify(vm);
    } catch (const po::error &e) {
        std::cerr << "[ERROR] " << e.what() << "! Try option '--help' for available commandline options.\n";
        std::exit(1);
    }

    std::unique_ptr<qc::QuantumComputation> quantumComputation;
    std::unique_ptr<Simulator> ddsim{nullptr};

    if (vm.count("simulate_file")) {
        const std::string fname = vm["simulate_file"].as<std::string>();
    	quantumComputation = std::make_unique<qc::QuantumComputation>(fname);
        ddsim = std::make_unique<QFRSimulator>(quantumComputation,
                                               vm["steps"].as<unsigned int>(), vm["step_fidelity"].as<double>(),
                                               vm["initial_reorder"].as<int>(), vm["dynamic_reorder"].as<int>(), vm["post_reorder"].as<int>(),
                                               seed);
    } else {
        std::cerr << "Did not find anything to simulate. See help below.\n"
                  << description << "\n";
        return 1;
    }

    if (quantumComputation && quantumComputation->getNqubits() > dd::MAXN) {
        std::cerr << "Quantum computation contains to many qubits (limit is set to " << dd::MAXN << "). See documentation for details.\n";
        std::exit(1);
    }
        if (vm.count("noise_effects")) {
            ddsim->setNoiseEffects((char *) &vm["noise_effects"].as<std::string>()[0]);
    } else {
            ddsim->setNoiseEffects((char *) &std::string("0")[0]);
    }
    if (vm.count("noise_prob")) {
        ddsim->setAmplitudeDampingProbability(vm["noise_prob"].as<double>());
    } else {
        ddsim->setAmplitudeDampingProbability(0.001);
    }

    if (vm.count("confidence")) {
        ddsim->stoch_confidence = vm["confidence"].as<double>();
    }

    if (vm.count("properties")) {
        ddsim->setRecordedProperties((char *) &vm["properties"].as<std::string>()[0]);
    } else {
        ddsim->setRecordedProperties("0-1000");
    }

    if (vm.count("error_bound")) {
        ddsim->stoch_error_margin = vm["error_bound"].as<double>();
    } else {
        ddsim->stoch_error_margin = 0.01;
    }
    ddsim->stochastic_runs = vm["stoch_runs"].as<long>();

    auto t1 = std::chrono::high_resolution_clock::now();
    ddsim->StochSimulate();
    auto t2 = std::chrono::high_resolution_clock::now();

    std::chrono::duration<float> duration_simulation = t2-t1;

    if (vm.count("benchmark")) {
        auto more_info = ddsim->AdditionalStatistics();
        std::cout << ddsim->getName() << ", "
                  << ddsim->getNumberOfQubits() << ", "
                  //<< vm["approximate"].as<float>() << ", "
                  << std::fixed << duration_simulation.count() << std::defaultfloat << ", "
                  //<< more_info["approximation_runs"] << ","
                  //<< more_info["final_fidelity"] << ", "
                  << more_info["coprime_a"] << ", "
                  << more_info["sim_result"] << ", "
                  << more_info["polr_result"] << ", "
                  << ddsim->getSeed() << ", "
                  << ddsim->getNumberOfOps() << ", "
                  << ddsim->getMaxNodeCount()
                  << "\n";
        return 0;
    }

    std::cout << "{\n";

    if (vm.count("ps")) {
        std::cout << "  \"statistics\": {\n"
                  << "    \"simulation_time\": " << std::fixed << duration_simulation.count() << std::defaultfloat << ",\n"
                  << "    \"benchmark\": \"" << ddsim->getName() << "\",\n"
                  << "    \"shots\": " << vm["shots"].as<unsigned int>() << ",\n"
//                  << "    \"distinct_results\": " << m.size() << ",\n"
                  << "    \"n_qubits\": " << ddsim->getNumberOfQubits() << ",\n"
                  << "    \"applied_gates\": " << ddsim->getNumberOfOps() << ",\n"
                  << "    \"max_nodes\": " << ddsim->getMaxNodeCount() << ",\n"
                  ;
        for(const auto& item : ddsim->AdditionalStatistics()) {
            std::cout << "    \"" << item.first << "\": \"" << item.second << "\",\n";
        }
        std::cout << "    \"seed\": " << ddsim->getSeed() << "\n"
                  << "  },\n";
    }
    std::cout << "  \"dummy\": 0\n}\n"; // trailing element to make json printout easier
}


#include <iostream>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>

#include <besiq/method/caseonly_method.hpp>
#include <besiq/method/peer_method.hpp>
#include <besiq/method/method.hpp>

#include "common_options.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-caseonly [OPTIONS] pairs genotype_plink_prefix";
const std::string DESCRIPTION = "A stage-wise case-only filter for genetic interactions.";

int
main(int argc, char *argv[])
{
    char const* const method_choices[] = { "css", "r2", "contrast", "peer" };
    OptionParser parser = create_common_options( USAGE, DESCRIPTION, false );
    parser.add_option( "-m", "--method" ).choices( &method_choices[ 0 ], &method_choices[ 4 ] ).metavar( "method" ).help( "The type of test statistic to compute 'r2' or 'css'." ).set_default( "css" );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }
    shared_ptr<common_options> parsed_data = parse_common_options( options, parser.args( ) );

    method_type *m = NULL;
    if( options[ "method" ] != "peer" )
    {
        m = new caseonly_method( parsed_data->data, options[ "method" ] );
    }
    else if( options[ "method" ] == "peer" )
    {
        m = new peer_method( parsed_data->data );
    }

    run_method( *m, parsed_data->genotypes, *parsed_data->pairs, *parsed_data->result_file );

    delete m;

    return 0;
}


/***********************************************************************************************************************
*  OpenStudio(R), Copyright (c) 2008-2018, Alliance for Sustainable Energy, LLC. All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
*  following conditions are met:
*
*  (1) Redistributions of source code must retain the above copyright notice, this list of conditions and the following
*  disclaimer.
*
*  (2) Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
*  disclaimer in the documentation and/or other materials provided with the distribution.
*
*  (3) Neither the name of the copyright holder nor the names of any contributors may be used to endorse or promote products
*  derived from this software without specific prior written permission from the respective party.
*
*  (4) Other than as required in clauses (1) and (2), distributions in any form of modifications or other derivative works
*  may not use the "OpenStudio" trademark, "OS", "os", or any other confusingly similar designation without specific prior
*  written permission from Alliance for Sustainable Energy, LLC.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER(S) AND ANY CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
*  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER(S), ANY CONTRIBUTORS, THE UNITED STATES GOVERNMENT, OR THE UNITED
*  STATES DEPARTMENT OF ENERGY, NOR ANY OF THEIR EMPLOYEES, BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
*  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
*  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
*  STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
*  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
***********************************************************************************************************************/

#include <gtest/gtest.h>
#include "../Intersection.hpp"
#include "GeometryFixture.hpp"

#undef BOOST_UBLAS_TYPE_CHECK
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint> BoostPolygon;
typedef boost::geometry::model::ring<BoostPoint> BoostRing;
typedef boost::geometry::model::multi_polygon<BoostPolygon> BoostMultiPolygon;

using namespace std;
using namespace boost;
using namespace openstudio;

std::string printPolygon(const BoostPolygon& polygon)
{
  std::stringstream ss;

  ss << boost::geometry::area(polygon);

  BoostRing outer = polygon.outer();
  if (outer.empty()){
    return ss.str();
  }

  // add point for each vertex except final vertex
  ss << " [";
  for(unsigned i = 0; i < outer.size(); ++i){
    ss << outer[i].x() << " " << outer[i].y();
    if (i < outer.size()-1){
      ss << ", ";
    }
  }

  for (const BoostRing& inner : polygon.inners()){
    if (!inner.empty()){
      ss << "], [";
      // inner loop already in reverse order
      for(unsigned i = 0; i < inner.size(); ++i){
        ss << inner[i].x() << " " << inner[i].y();
        if (i < inner.size()-1){
          ss << ", ";
        }
      }
    }
  }
  ss << "]";

  return ss.str();
}

std::vector<Point3d> makeRectangleUp(double xmin, double ymin, double width, double height)
{
  std::vector<Point3d> result;
  result.push_back(Point3d(xmin, ymin, 0));
  result.push_back(Point3d(xmin+width, ymin, 0));
  result.push_back(Point3d(xmin+width, ymin+height, 0));
  result.push_back(Point3d(xmin, ymin+height, 0));
  return result;
}

std::vector<Point3d> makeRectangleDown(double xmin, double ymin, double width, double height)
{
  std::vector<Point3d> result;
  result.push_back(Point3d(xmin+width, ymin+height, 0));
  result.push_back(Point3d(xmin+width, ymin, 0));
  result.push_back(Point3d(xmin, ymin, 0));
  result.push_back(Point3d(xmin, ymin+height, 0));
  return result;
}

TEST_F(GeometryFixture, BoostGeometry_Polygon1)
{
  BoostPolygon blue, yellow;

  // blue
  boost::geometry::read_wkt(
      "POLYGON((-11.379200508 -12.0396003048 , -11.379200508 12.0395996952 , 3.555999492 12.0395996952 , 3.555999492 -12.0396003048 , -11.379200508 -12.0396003048))", blue);
  //Surface Intersection Model 74
  //Surface 37,                             !- Name
  //-11.379200508, -12.0396003048, 3.3528,  !- X,Y,Z Vertex 1 {m}
  //-11.379200508, 12.0395996952, 3.3528,   !- X,Y,Z Vertex 2 {m}
  //3.555999492, 12.0395996952, 3.3528,     !- X,Y,Z Vertex 3 {m}
  //3.555999492, -12.0396003048, 3.3528;    !- X,Y,Z Vertex 4 {m}

  // yellow
  boost::geometry::read_wkt(
      "POLYGON((3.2512004064 -7.4676003048 , 3.2512004064 7.4675996952 , 3.555999492 7.7723987808 , 3.555999492 -7.7723993904 , 3.2512004064 -7.4676003048))", yellow);
  //Surface Intersection Model 74
  //Surface 44,                             !- Name
  //3.555999492, -7.7723993904, 3.3528,     !- X,Y,Z Vertex 1 {m}
  //3.555999492, 7.7723987808, 3.3528,      !- X,Y,Z Vertex 2 {m}
  //3.2512004064, 7.4675996952, 3.3528,     !- X,Y,Z Vertex 3 {m}
  //3.2512004064, -7.4676003048, 3.3528;    !- X,Y,Z Vertex 4 {m}

  std::vector<BoostPolygon> output;
  boost::geometry::difference(yellow, blue, output);

  int i = 0;
  std::cout << "yellow - blue:" << std::endl;
  for (BoostPolygon const& p : output)
  {
    std::cout << i++ << ": " << printPolygon(p) << std::endl;
  }

  output.clear();
  boost::geometry::difference(blue, yellow, output);

  i = 0;
  std::cout << "blue - yellow:" << std::endl;
  for (BoostPolygon const& p : output)
  {
    std::cout << i++ << ": " << printPolygon(p) << std::endl;
  }

}

TEST_F(GeometryFixture, PointInPolygon_Down)
{
  double tol = 0.01;
  double tol2 = tol/2.0;

  Point3dVector points = makeRectangleDown(0, 0, 1, 1);

  // center
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,0.5,0), points, tol));

  // corners
  EXPECT_TRUE(pointInPolygon(Point3d(0,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,1,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0,1,0), points, tol));

  // edges
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,0,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(1,0.5,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0.5,1,0), points, tol));
  EXPECT_TRUE(pointInPolygon(Point3d(0,0.5,0), points, tol));

  EXPECT_TRUE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_TRUE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(pointInPolygon(Point3d(2,0,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1,2,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1,0,0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1,-1,0), points, tol));

  // not on z = 0
  EXPECT_FALSE(pointInPolygon(Point3d(0.5,0.5,0.5), points, tol));
}

TEST_F(GeometryFixture, PointInPolygon_Up)
{
  double tol = 0.01;
  double tol2 = tol / 2.0;

  Point3dVector points = makeRectangleUp(0, 0, 1, 1);

  // center
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(pointInPolygon(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(0, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(pointInPolygon(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(pointInPolygon(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(pointInPolygon(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Within_Down)
{
  double tol = 0.01;
  double tol2 = tol/2.0;

  Point3dVector points = makeRectangleDown(0, 0, 1, 1);

  // center
  EXPECT_TRUE(within(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(within(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(within(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 0.5, 0), points, tol));

  EXPECT_TRUE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_TRUE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_TRUE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(within(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Within_Up)
{
  double tol = 0.01;
  double tol2 = tol / 2.0;

  Point3dVector points = makeRectangleUp(0, 0, 1, 1);

  // center
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0), points, tol));

  // corners
  EXPECT_FALSE(within(Point3d(0, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 1, 0), points, tol));

  // edges
  EXPECT_FALSE(within(Point3d(0.5, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 - tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 - tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(tol2, 0.5, 0), points, tol));

  EXPECT_FALSE(within(Point3d(0.5, -tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1 + tol2, 0.5, 0), points, tol));
  EXPECT_FALSE(within(Point3d(0.5, 1 + tol2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-tol2, 0.5, 0), points, tol));

  // outside
  EXPECT_FALSE(within(Point3d(2, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(1, 2, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, 0, 0), points, tol));
  EXPECT_FALSE(within(Point3d(-1, -1, 0), points, tol));

  // not on z = 0
  EXPECT_FALSE(within(Point3d(0.5, 0.5, 0.5), points, tol));
}

TEST_F(GeometryFixture, Intersect_False)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // non intersecting, sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(3, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // non intersecting, sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(3, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is down, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is right, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 0));
  points1.push_back(Point3d(0, 0, 1));
  points1.push_back(Point3d(0, 1, 1));
  points1.push_back(Point3d(0, 1, 0));

  points2.clear();
  points2.push_back(Point3d(0, 0, 0));
  points2.push_back(Point3d(0, 0, 1));
  points2.push_back(Point3d(0, 1, 1));
  points2.push_back(Point3d(0, 1, 0));

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_SamePoints)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(points2, test->polygon2())) << test->polygon2();
  EXPECT_EQ(0, test->newPolygons1().size());
  EXPECT_EQ(0, test->newPolygons2().size());

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points2, test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(points1, test->polygon2())) << test->polygon2();
  EXPECT_EQ(0, test->newPolygons1().size());
  EXPECT_EQ(0, test->newPolygons2().size());

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Adjacent)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Overlap_Down)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  ASSERT_EQ(1, test->newPolygons1().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  ASSERT_EQ(1, test->newPolygons2().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  ASSERT_EQ(1, test->newPolygons1().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  ASSERT_EQ(1, test->newPolygons2().size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Intersect_Overlap_Up)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleUp(0, 0, 2, 1);
  points2 = makeRectangleUp(1, 0, 2, 1);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Intersect_Within_Down)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 3, 3);
  points2 = makeRectangleDown(1, 1, 2, 2);

  test = intersect(points1, points2, tol);
  ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  test = intersect(points2, points1, tol);
  ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Intersect_Within_Up)
{
  double tol = 0.01;

  boost::optional<IntersectionResult> test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 3, 3);
  points2 = makeRectangleUp(1, 1, 2, 2);

  test = intersect(points1, points2, tol);
  EXPECT_FALSE(test);

  test = intersect(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_False)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // non intersecting, sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(3, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // non intersecting, sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(3, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is down, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is up, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 3));
  points1.push_back(Point3d(1, 0, 3));
  points1.push_back(Point3d(1, 1, 3));
  points1.push_back(Point3d(0, 1, 3));

  points2.clear();
  points2.push_back(Point3d(0, 0, 3));
  points2.push_back(Point3d(1, 0, 3));
  points2.push_back(Point3d(1, 1, 3));
  points2.push_back(Point3d(0, 1, 3));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // sense is right, same points but not on z=0 plane
  points1.clear();
  points1.push_back(Point3d(0, 0, 0));
  points1.push_back(Point3d(0, 0, 1));
  points1.push_back(Point3d(0, 1, 1));
  points1.push_back(Point3d(0, 1, 0));

  points2.clear();
  points2.push_back(Point3d(0, 0, 0));
  points2.push_back(Point3d(0, 0, 1));
  points2.push_back(Point3d(0, 1, 1));
  points2.push_back(Point3d(0, 1, 0));

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_SamePoints)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(points1, *test)) << *test;

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Adjacent)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), *test)) << *test;

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Overlap_Down)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), *test)) << *test;

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), *test)) << *test;
}

TEST_F(GeometryFixture, Join_Overlap_Up)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 2, 1);
  points2 = makeRectangleUp(1, 0, 2, 1);

  test = join(points1, points2, tol);
  EXPECT_FALSE(test);

  test = join(points2, points1, tol);
  EXPECT_FALSE(test);
}

TEST_F(GeometryFixture, Join_Overlap2)
{
  double tol = 0.01;

  boost::optional<std::vector<Point3d> > test;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 4, 1);
  points2 = makeRectangleDown(1, -1, 4, 1);

  test = join(points1, points2, tol);
  ASSERT_TRUE(test);

  test = join(points2, points1, tol);
  ASSERT_TRUE(test);
}

TEST_F(GeometryFixture, JoinAll)
{
  double tol = 0.01;

  std::vector<Point3dVector> test;
  std::vector<Point3dVector> polygons;

  // empty
  test = joinAll(polygons, tol);
  EXPECT_EQ(0, test.size());

  // two overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(1u, test.size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 2, 1), test[0])) << test[0];

  // three overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  polygons.push_back(makeRectangleDown(2, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(1u, test.size());
  EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 3, 1), test[0])) << test[0];

  // three not overlapping
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(2, 0, 1, 1));
  polygons.push_back(makeRectangleDown(4, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(3u, test.size());

  // two overlapping, one not
  polygons.clear();
  polygons.push_back(makeRectangleDown(0, 0, 1, 1));
  polygons.push_back(makeRectangleDown(1, 0, 1, 1));
  polygons.push_back(makeRectangleDown(3, 0, 1, 1));
  test = joinAll(polygons, tol);
  ASSERT_EQ(2u, test.size());

  bool found1 = false;
  bool found2 = false;
  for (unsigned i = 0; i < 2; ++i){
    if (circularEqual(makeRectangleDown(0, 0, 2, 1), test[i])){
      found1 = true;
    }else if (circularEqual(makeRectangleDown(3, 0, 1, 1), test[i])){
      found2 = true;
    }
  }
  EXPECT_TRUE(found1);
  EXPECT_TRUE(found2);
}

TEST_F(GeometryFixture, RemoveSpikes_Down)
{
  double tol = 0.01;

  std::vector<Point3d> points;
  std::vector<Point3d> expected;
  std::vector<Point3d> result;

  // spike at beginning
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(10, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike at beginning 2
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike in middle
  {
    // sense is down
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(-5, 0, 0)); // the spike
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();
    expected.push_back(Point3d(10, 5, 0));
    expected.push_back(Point3d(10, 0, 0));
    expected.push_back(Point3d(0, 0, 0));
    expected.push_back(Point3d(0, 5, 0));

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }
}

TEST_F(GeometryFixture, RemoveSpikes_Up)
{
  double tol = 0.01;

  std::vector<Point3d> points;
  std::vector<Point3d> expected;
  std::vector<Point3d> result;

  // spike at end
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike at end 2
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(0, 5, 0));
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(10, 10, 0)); // the spike
    points.push_back(Point3d(10, 5, 0));

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }

  // spike in middle
  {
    // sense is up
    points.clear();
    points.push_back(Point3d(0, 0, 0));
    points.push_back(Point3d(10, 0, 0));
    points.push_back(Point3d(-5, 0, 0)); // the spike
    points.push_back(Point3d(10, 5, 0));
    points.push_back(Point3d(0, 5, 0));

    expected.clear();

    result = removeSpikes(points, tol);
    EXPECT_TRUE(circularEqual(expected, result)) << result;
  }
}

TEST_F(GeometryFixture, Subtract_SamePoints)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(points1, test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(points2, test->polygon2())) << test->polygon2();
  //EXPECT_EQ(0, test->newPolygons1().size());
  //EXPECT_EQ(0, test->newPolygons2().size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(points2, test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(points1, test->polygon2())) << test->polygon2();
  //EXPECT_EQ(0, test->newPolygons1().size());
  //EXPECT_EQ(0, test->newPolygons2().size());

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());

  // opposite sense, points1 is up, points2 is down
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleDown(0, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());
}

TEST_F(GeometryFixture, Subtract_Empty_Down)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);

  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  // can't subtract single point
  points2.push_back(Point3d(1, 1, 1));

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}

TEST_F(GeometryFixture, Subtract_Empty_Up)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleUp(0, 0, 1, 1);

  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  points2.push_back(Point3d(1, 1, 1));

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  // can't subtract single point
  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}

TEST_F(GeometryFixture, Subtract_Adjacent)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 1, 1);
  points2 = makeRectangleDown(1, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));

  // sense is up
  points1 = makeRectangleUp(0, 0, 1, 1);
  points2 = makeRectangleUp(1, 0, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_EQ(0, test.size());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_EQ(0, test.size());
}

TEST_F(GeometryFixture, Subtract_Overlap)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 2, 1);
  points2 = makeRectangleDown(1, 0, 2, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.size() == 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(1.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Subtract_Within_Down)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is down
  points1 = makeRectangleDown(0, 0, 3, 3);
  points2 = makeRectangleDown(1, 1, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.size() > 1);
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(8.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
  for (auto polygon : test){
    std::cout << polygon << std::endl;
  }
  EXPECT_DOUBLE_EQ(0.0, totalArea(test));
  //ASSERT_TRUE(test);
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon1())) << test->polygon1();
  //EXPECT_TRUE(circularEqual(makeRectangleDown(1, 0, 1, 1), test->polygon2())) << test->polygon2();
  //ASSERT_EQ(1, test->newPolygons1().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(2, 0, 1, 1), test->newPolygons1()[0])) << test->newPolygons1()[0];
  //ASSERT_EQ(1, test->newPolygons2().size());
  //EXPECT_TRUE(circularEqual(makeRectangleDown(0, 0, 1, 1), test->newPolygons2()[0])) << test->newPolygons2()[0];
}

TEST_F(GeometryFixture, Subtract_Within_Up)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points1;
  Point3dVector points2;

  // sense is up
  points1 = makeRectangleUp(0, 0, 3, 3);
  points2 = makeRectangleUp(1, 1, 1, 1);

  holes.clear();
  holes.push_back(points2);
  test = subtract(points1, holes, tol);
  EXPECT_TRUE(test.empty());

  holes.clear();
  holes.push_back(points1);
  test = subtract(points2, holes, tol);
  EXPECT_TRUE(test.empty());
}

TEST_F(GeometryFixture, selfIntersects)
{
  double tol = 0.01;

  std::vector<std::vector<Point3d> > test;
  std::vector<std::vector<Point3d> > holes;
  Point3dVector points;

  points.clear();
  points.push_back(Point3d(0, 1, 0));
  points.push_back(Point3d(2, 1, 0));
  points.push_back(Point3d(2, 0, 0));
  points.push_back(Point3d(0, 0, 0));
  EXPECT_FALSE(selfIntersects(points,tol));

  points.clear();
  points.push_back(Point3d(0, 1, 0));
  points.push_back(Point3d(2, 1, 0));
  points.push_back(Point3d(0, 0, 0));
  points.push_back(Point3d(2, 0, 0));
  EXPECT_TRUE(selfIntersects(points,tol));

  points.clear();
  points.push_back(Point3d(0, 1, 0));
  points.push_back(Point3d(1, 0.2, 0));
  points.push_back(Point3d(2, 1, 0));
  points.push_back(Point3d(2, 0, 0));
  points.push_back(Point3d(0, 0, 0));
  EXPECT_FALSE(selfIntersects(points,tol));

  points.clear();
  points.push_back(Point3d(0, 1, 0));
  points.push_back(Point3d(1, 0, 0));
  points.push_back(Point3d(2, 1, 0));
  points.push_back(Point3d(2, 0, 0));
  points.push_back(Point3d(0, 0, 0));
  EXPECT_FALSE(selfIntersects(points,tol));

  points.clear();
  points.push_back(Point3d(0, 1, 0));
  points.push_back(Point3d(1, -1, 0));
  points.push_back(Point3d(2, 1, 0));
  points.push_back(Point3d(2, 0, 0));
  points.push_back(Point3d(0, 0, 0));
  EXPECT_TRUE(selfIntersects(points,tol));
}

TEST_F(GeometryFixture, simplify)
{
  double tol = 0.01;

  Point3dVector points;

  points.clear();
  points.push_back(Point3d(158, 168, 0));
  points.push_back(Point3d(200, 168, 0));
  points.push_back(Point3d(200, 30, 0));
  points.push_back(Point3d(158, 30, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(200, 168, 0));
  expectedPoints.push_back(Point3d(200, 30, 0));
  expectedPoints.push_back(Point3d(158, 30, 0));
  expectedPoints.push_back(Point3d(158, 40, 0));
  expectedPoints.push_back(Point3d(158, 70, 0));
  expectedPoints.push_back(Point3d(158, 98, 0));
  expectedPoints.push_back(Point3d(158, 128, 0));
  expectedPoints.push_back(Point3d(158, 138, 0));
  expectedPoints.push_back(Point3d(158, 168, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }
}

TEST_F(GeometryFixture, simplify2)
{
  double tol = 0.01;

  Point3dVector points;

  points.clear();
  points.push_back(Point3d(158, 168, 0));
  points.push_back(Point3d(200, 168, 0));
  points.push_back(Point3d(200, 30, 0));
  points.push_back(Point3d(158, 30, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 40, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 70, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 98, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 138, 0));
  points = reverse(points);

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(200, 30, 0));
  expectedPoints.push_back(Point3d(200, 168, 0));
  expectedPoints.push_back(Point3d(158, 168, 0));
  expectedPoints.push_back(Point3d(158, 138, 0));
  expectedPoints.push_back(Point3d(158, 128, 0));
  expectedPoints.push_back(Point3d(158, 98, 0));
  expectedPoints.push_back(Point3d(158, 70, 0));
  expectedPoints.push_back(Point3d(158, 40, 0));
  expectedPoints.push_back(Point3d(158, 30, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }
}

TEST_F(GeometryFixture, simplify3)
{
  double tol = 0.01;

  Point3dVector points;

  points.clear();
  points.push_back(Point3d(158, 49, 0));
  points.push_back(Point3d(158, 108, 0));
  points.push_back(Point3d(158, 89, 0));
  points.push_back(Point3d(158, 21, 0));
  points.push_back(Point3d(157, 21, 0));
  points.push_back(Point3d(144, 21, 0));
  points.push_back(Point3d(143, 21, 0));
  points.push_back(Point3d(114, 21, 0));
  points.push_back(Point3d(113, 21, 0));
  points.push_back(Point3d(113, 28, 0));
  points.push_back(Point3d(113, 50, 0));
  points.push_back(Point3d(113, 70, 0));
  points.push_back(Point3d(113, 91, 0));
  points.push_back(Point3d(113, 98, 0));
  points.push_back(Point3d(113, 119, 0));
  points.push_back(Point3d(113, 120, 0));
  points.push_back(Point3d(114, 120, 0));
  points.push_back(Point3d(143, 120, 0));
  points.push_back(Point3d(157, 120, 0));
  points.push_back(Point3d(158, 120, 0));

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(158, 120, 0));
  expectedPoints.push_back(Point3d(158, 108, 0));
  expectedPoints.push_back(Point3d(158, 89, 0));
  expectedPoints.push_back(Point3d(158, 49, 0));
  expectedPoints.push_back(Point3d(158, 21, 0));
  expectedPoints.push_back(Point3d(157, 21, 0));
  expectedPoints.push_back(Point3d(144, 21, 0));
  expectedPoints.push_back(Point3d(143, 21, 0));
  expectedPoints.push_back(Point3d(114, 21, 0));
  expectedPoints.push_back(Point3d(113, 21, 0));
  expectedPoints.push_back(Point3d(113, 28, 0));
  expectedPoints.push_back(Point3d(113, 50, 0));
  expectedPoints.push_back(Point3d(113, 70, 0));
  expectedPoints.push_back(Point3d(113, 91, 0));
  expectedPoints.push_back(Point3d(113, 98, 0));
  expectedPoints.push_back(Point3d(113, 119, 0));
  expectedPoints.push_back(Point3d(113, 120, 0));
  expectedPoints.push_back(Point3d(114, 120, 0));
  expectedPoints.push_back(Point3d(143, 120, 0));
  expectedPoints.push_back(Point3d(157, 120, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }
}

TEST_F(GeometryFixture, simplify4)
{
  double tol = 0.01;

  Point3dVector points;
  points.clear();
  points.push_back(Point3d(158, 120, 0));
  points.push_back(Point3d(158, 49, 0));
  points.push_back(Point3d(158, 108, 0));
  points.push_back(Point3d(158, 21, 0));
  points.push_back(Point3d(113, 21, 0));
  points.push_back(Point3d(113, 120, 0));

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(158, 120, 0));
  expectedPoints.push_back(Point3d(158, 108, 0));
  expectedPoints.push_back(Point3d(158, 49, 0));
  expectedPoints.push_back(Point3d(158, 21, 0));
  expectedPoints.push_back(Point3d(113, 21, 0));
  expectedPoints.push_back(Point3d(113, 120, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  std::cout << points2 << std::endl;
  std::cout << expectedPoints << std::endl;
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }
}

TEST_F(GeometryFixture, simplify5)
{
  double tol = 0.01;

  Point3dVector points;
  points.clear();
  points.push_back(Point3d(158, 98,  0));
  points.push_back(Point3d(158, 138, 0));
  points.push_back(Point3d(158, 40,  0));
  points.push_back(Point3d(158, 70,  0));
  points.push_back(Point3d(158, 128, 0));
  points.push_back(Point3d(158, 168, 0));
  points.push_back(Point3d(200, 168, 0));
  points.push_back(Point3d(200, 30,  0));
  points.push_back(Point3d(158, 30,  0));

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(200, 168, 0));
  expectedPoints.push_back(Point3d(200, 30, 0));
  expectedPoints.push_back(Point3d(158, 30, 0));
  expectedPoints.push_back(Point3d(158, 40, 0));
  expectedPoints.push_back(Point3d(158, 70, 0));
  expectedPoints.push_back(Point3d(158, 98, 0));
  expectedPoints.push_back(Point3d(158, 128, 0));
  expectedPoints.push_back(Point3d(158, 138, 0));
  expectedPoints.push_back(Point3d(158, 168, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }

  // remove colinear
  Point3dVector points4 = simplify(points, true, tol);
  EXPECT_EQ(4u, points4.size());

  std::cout << points4 << std::endl << std::endl;

  auto area4 = getArea(points4);
  ASSERT_TRUE(area4);

  EXPECT_NEAR(*area, *area4, tol*tol);
}

TEST_F(GeometryFixture, simplify6)
{
  double tol = 0.01;

  Point3dVector points;
  points.clear();
  points.push_back(Point3d(3.6576, 3.048, 0));
  points.push_back(Point3d(5.4864, 3.048, 0));
  points.push_back(Point3d(5.4864, 5.1816, 0));
  points.push_back(Point3d(5.4864, 11.5824, 0));
  points.push_back(Point3d(5.4864, 5.1816, 0));
  points.push_back(Point3d(5.4864, 11.5824, 0));
  points.push_back(Point3d(5.4864, 17.0688, 0));
  points.push_back(Point3d(3.6576, 17.0688, 0));
  points.push_back(Point3d(3.6576, 14.9352, 0));
  points.push_back(Point3d(3.6576, 11.5824, 0));
  points.push_back(Point3d(3.6576, 7.0104, 0));
  points.push_back(Point3d(3.6576, 11.5824, 0));
  points.push_back(Point3d(3.6576, 7.0104, 0));

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(5.4864, 3.048, 0));
  expectedPoints.push_back(Point3d(5.4864, 5.1816, 0));
  expectedPoints.push_back(Point3d(5.4864, 11.5824, 0));
  expectedPoints.push_back(Point3d(5.4864, 17.0688, 0));
  expectedPoints.push_back(Point3d(3.6576, 17.0688, 0));
  expectedPoints.push_back(Point3d(3.6576, 14.9352, 0));
  expectedPoints.push_back(Point3d(3.6576, 11.5824, 0));
  expectedPoints.push_back(Point3d(3.6576, 7.0104, 0));
  expectedPoints.push_back(Point3d(3.6576, 3.048, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }

  // remove colinear
  Point3dVector points4 = simplify(points, true, tol);
  EXPECT_EQ(4u, points4.size());

  std::cout << points4 << std::endl << std::endl;

  auto area4 = getArea(points4);
  ASSERT_TRUE(area4);

  EXPECT_NEAR(*area, *area4, tol*tol);
}

TEST_F(GeometryFixture, simplify7)
{
  double tol = 0.01;

  Point3dVector points;
  points.clear();
  points.push_back(Point3d(3.6576, 3.048, 0));
  points.push_back(Point3d(3.6576, 11.5824, 0));
  points.push_back(Point3d(3.6576, 7.0104, 0));
  points.push_back(Point3d(3.6576, 14.9352, 0));
  points.push_back(Point3d(3.6576, 17.0688, 0));
  points.push_back(Point3d(5.4864, 17.0688, 0));
  points.push_back(Point3d(5.4864, 5.1816, 0));
  points.push_back(Point3d(5.4864, 11.5824, 0));
  points.push_back(Point3d(5.4864, 3.048, 0));

  std::cout << points << std::endl << std::endl;

  auto area = getArea(points);
  ASSERT_TRUE(area);

  Point3dVector points2 = simplify(points, false, tol);

  std::cout << points2 << std::endl << std::endl;

  auto area2 = getArea(points2);
  ASSERT_TRUE(area2);

  EXPECT_NEAR(*area, *area2, tol*tol);

  Point3dVector expectedPoints;
  expectedPoints.push_back(Point3d(5.4864, 17.0688, 0));
  expectedPoints.push_back(Point3d(5.4864, 11.5824, 0));
  expectedPoints.push_back(Point3d(5.4864, 5.1816, 0));
  expectedPoints.push_back(Point3d(5.4864, 3.048, 0));
  expectedPoints.push_back(Point3d(3.6576, 3.048, 0));
  expectedPoints.push_back(Point3d(3.6576, 7.0104, 0));
  expectedPoints.push_back(Point3d(3.6576, 11.5824, 0));
  expectedPoints.push_back(Point3d(3.6576, 14.9352, 0));
  expectedPoints.push_back(Point3d(3.6576, 17.0688, 0));

  auto area3 = getArea(expectedPoints);
  ASSERT_TRUE(area3);

  EXPECT_NEAR(*area, *area3, tol*tol);

  ASSERT_EQ(points2.size(), expectedPoints.size());
  for (auto i = 0; i < expectedPoints.size(); ++i){
    EXPECT_EQ(expectedPoints[i].x(), points2[i].x()) << i;
    EXPECT_EQ(expectedPoints[i].y(), points2[i].y()) << i;
    EXPECT_EQ(expectedPoints[i].z(), points2[i].z()) << i;
  }

  // remove colinear
  Point3dVector points4 = simplify(points, true, tol);
  EXPECT_EQ(4u, points4.size());

  std::cout << points4 << std::endl << std::endl;

  auto area4 = getArea(points4);
  ASSERT_TRUE(area4);

  EXPECT_NEAR(*area, *area4, tol*tol);
}


#include <boost/numeric/odeint/iterator/n_step_iterator.hpp>


//==================================================================================================
/*!
  @file

  @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IF_ELSE_ZERO_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_IF_ELSE_ZERO_HPP_INCLUDED

#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/scalar/is_nez.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{

  BOOST_DISPATCH_OVERLOAD ( if_else_zero_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< logical_<A0> >
                          , bd::scalar_< bd::unspecified_<A1> >
                          )
  {
    BOOST_FORCEINLINE A1 operator() ( A0 a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return is_nez(a0) ? a1 : Zero<A1>();
    }
  };

  BOOST_DISPATCH_OVERLOAD ( if_else_zero_
                          , (typename A0, typename A1)
                          , bd::cpu_
                          , bd::scalar_< bd::unspecified_<A0> >
                          , bd::scalar_< bd::unspecified_<A1> >
                          )
  {
    BOOST_FORCEINLINE A1 operator() ( A0 const& a0, A1 const& a1) const BOOST_NOEXCEPT
    {
      return is_nez(a0) ? a1 : Zero<A1>();
    }
  };

   BOOST_DISPATCH_OVERLOAD ( if_else_zero_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::bool_<A0> >
                          , bd::scalar_< bd::bool_<A0> >
                          )
  {
    BOOST_FORCEINLINE bool operator() ( A0 a0, A0  a1) const BOOST_NOEXCEPT
    {
      return a0 ? a1 : false;
    }
  };
} } }

#endif


// Boost.Geometry Index
// Unit Test

// Copyright (c) 2014 Adam Wulkiewicz, Lodz, Poland.

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

#include <rtree/test_rtree.hpp>

#include <boost/geometry/geometries/register/point.hpp>

struct point
{
    point(double xx = 0, double yy = 0) : x(xx), y(yy) {}
    double x, y;
};

BOOST_GEOMETRY_REGISTER_POINT_2D(point, double, bg::cs::cartesian, x, y)

template <typename Box, typename Params>
void test_pair()
{
    typedef std::pair<Box, std::size_t> Value;

    typename boost::remove_const<Box>::type box;
    bg::assign_zero(box);

    Value val(box, 0);

    // sanity check
    std::vector<Value> vec;
    vec.push_back(val);
    vec.push_back(std::make_pair(box, 0));
    vec.push_back(std::make_pair(box, (unsigned short)0));

    bgi::rtree<Value, Params> rt;
    rt.insert(val);
    rt.insert(std::make_pair(box, 0));
    rt.insert(std::make_pair(box, (unsigned short)0));
    BOOST_CHECK( rt.size() == 3 );

    BOOST_CHECK( rt.count(val) == 3 );
    BOOST_CHECK( rt.count(std::make_pair(box, 0)) == 3 );
    BOOST_CHECK( rt.count(std::make_pair(box, (unsigned short)0)) == 3 );
    BOOST_CHECK( rt.count(box) == 3 );

    BOOST_CHECK( rt.remove(val) == 1 );
    BOOST_CHECK( rt.remove(std::make_pair(box, 0)) == 1 );
    BOOST_CHECK( rt.remove(std::make_pair(box, (unsigned short)0)) == 1 );
    BOOST_CHECK( rt.size() == 0 );
}

template <typename Params>
void test_point()
{
    bgi::rtree<point, Params> rt;

    rt.insert(0.0);
    BOOST_CHECK( rt.size() == 1 );
    BOOST_CHECK( rt.remove(0.0) == 1 );
}

int test_main(int, char* [])
{
    typedef bg::model::point<double, 2, bg::cs::cartesian> Pt;
    typedef bg::model::box<Pt> Box;

    test_pair< Box, bgi::linear<16> >();
    test_pair< Box, bgi::quadratic<4> >();
    test_pair< Box, bgi::rstar<4> >();
    //test_rtree< Box const, bgi::linear<16> >();
    //test_rtree< Box const, bgi::quadratic<4> >();
    //test_rtree< Box const, bgi::rstar<4> >();

    test_point< bgi::linear<16> >();
    test_point< bgi::quadratic<4> >();
    test_point< bgi::rstar<4> >();

    return 0;
}


#ifndef GUARD_ARCH_GRAPH_H
#define GUARD_ARCH_GRAPH_H

#include <cassert>
#include <memory>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/graph/adjacency_list.hpp>

#include "arch_graph_system.hpp"
#include "nauty_graph.hpp"
#include "perm_group.hpp"
#include "perm_set.hpp"

namespace mpsym
{

namespace internal { class NautyGraph; }

class ArchGraph : public ArchGraphSystem
{
  using processor_type_size_type = std::vector<std::string>::size_type;
  using channel_type_size_type = std::vector<std::string>::size_type;

  template<typename L>
  using typed_channel_dict_type = std::unordered_map<
    unsigned,
    std::vector<std::pair<unsigned, L>>
  >;

  using untyped_channel_dict_type = std::unordered_map<
    unsigned,
    std::vector<unsigned>
  >;

  using vertex_selector = boost::vecS;
  using edge_selector = boost::vecS;

  struct VertexProperty { processor_type_size_type type; };
  struct EdgeProperty { channel_type_size_type type; };

  using adjacency_type = boost::adjacency_list<
    edge_selector, vertex_selector, boost::directedS,
    VertexProperty, EdgeProperty>;

  using vertices_size_type = adjacency_type::vertices_size_type;
  using edges_size_type = adjacency_type::edges_size_type;

public:
  using ProcessorType = processor_type_size_type;
  using ChannelType = channel_type_size_type;

  template<typename L = void>
  using ChannelDict = typename std::conditional<
    std::is_void<L>::value,
    untyped_channel_dict_type,
    typed_channel_dict_type<L>
  >::type;

  ArchGraph(bool directed = false)
  : _directed(directed)
  {}

  virtual ~ArchGraph() = default;

  std::string to_gap() const override;
  std::string to_json() const override;

  ProcessorType new_processor_type(std::string const &pl);
  ChannelType new_channel_type(std::string const &cl);

  unsigned add_processor(ProcessorType pt);
  unsigned add_processor(std::string const &pl);

  template<typename T>
  unsigned add_processors(unsigned n, T pt)
  {
    assert(n > 0u);

    for (unsigned i = 0u; i < n - 1u; ++i)
      add_processor(pt);

    return add_processor(pt);
  }

  void add_channel(unsigned pe1, unsigned pe2, ChannelType ct);
  void add_channel(unsigned pe1, unsigned pe2, std::string const &cl);

  template<typename T>
  void add_channels(ChannelDict<T> const &channels)
  {
    for (auto const &tmp : channels) {
      unsigned pe1 = tmp.first;
      auto const &to = tmp.second;

      for (auto const &c : to) {
        unsigned pe2 = c.first;
        T ct = c.second;

        add_channel(pe1, pe2, ct);
      }
    }
  }

  template<typename T>
  void add_channels(ChannelDict<> const &channels, T ct)
  {
    for (auto const &tmp : channels) {
      unsigned pe1 = tmp.first;
      auto const &to = tmp.second;

      for (auto &pe2 : to)
        add_channel(pe1, pe2, ct);
    }
  }

  template<typename T>
  void fully_connect(T ct)
  {
    for (unsigned pe1 = 0u; pe1 < num_processors(); ++pe1) {
      for (unsigned pe2 = (directed() ? 0u : pe1 + 1u); pe2 < num_processors(); ++pe2) {
        if (pe1 != pe2)
          add_channel(pe1, pe2, ct);
      }
    }
  }

  void fully_connect(ProcessorType pt, ChannelType ct);
  void fully_connect(std::string const &pl, std::string const &cl);

  template<typename T>
  void self_connect(T ct)
  {
    for (unsigned pe = 0u; pe < num_processors(); ++pe)
      add_channel(pe, pe, ct);
  }

  void self_connect(ProcessorType pt, ChannelType ct);
  void self_connect(std::string const &pl, std::string const &cl);

  bool directed() const;
  bool effectively_directed() const;

  unsigned num_processors() const override;
  unsigned num_channels() const override;

private:
  internal::PermGroup automorphisms_(
    AutomorphismOptions const *options,
    internal::timeout::flag aborted) override
  {
    if (num_processors() == 0u)
      return internal::PermGroup();

    return automorphisms_nauty(options, aborted);
  }

  void init_repr_(AutomorphismOptions const *options,
                  internal::timeout::flag aborted) override
  { automorphisms(options, aborted); }

  // Convenience functions

  ChannelType assert_channel_type(std::string const &cl);
  ProcessorType assert_processor_type(std::string const &cl);

  bool channel_exists(unsigned from, unsigned to, ChannelType ct) const;
  bool channel_exists_directed(unsigned from, unsigned to, ChannelType ct) const;
  bool channel_exists_undirected(unsigned from, unsigned to, ChannelType ct) const;

  using pe_it = adjacency_type::vertex_iterator;
  using pe = pe_it::value_type;

  boost::iterator_range<pe_it> processors() const
  { return boost::make_iterator_range(boost::vertices(_adj)); }

  processor_type_size_type num_processor_types() const
  { return _processor_types.size(); }

  ProcessorType processor_type(pe pe) const
  { return _adj[pe].type; }

  std::string processor_type_str(pe pe) const
  { return _processor_types[processor_type(pe)]; }

  using ch_it = adjacency_type::edge_iterator;
  using ch_out_it = adjacency_type::out_edge_iterator;
  using ch = ch_it::value_type;

  boost::iterator_range<ch_it> channels() const
  { return boost::make_iterator_range(boost::edges(_adj)); }

  pe source(ch ch) const
  { return boost::source(ch, _adj); }

  pe target(ch ch) const
  { return boost::target(ch, _adj); }

  boost::iterator_range<ch_out_it> out_channels(pe pe) const
  { return boost::make_iterator_range(boost::out_edges(pe, _adj)); }

  channel_type_size_type num_channel_types() const
  { return _channel_types.size(); }

  ChannelType channel_type(ch ch) const
  { return _adj[ch].type; }

  std::string channel_type_str(ch ch) const
  { return _channel_types[channel_type(ch)]; }

  // Nauty

  internal::NautyGraph graph_nauty() const;

  std::string to_gap_nauty() const;

  internal::PermSet automorphism_generators_nauty();

  internal::PermGroup automorphisms_nauty(
    AutomorphismOptions const *options,
    internal::timeout::flag aborted);

  adjacency_type _adj;
  bool _directed;

  std::vector<std::string> _processor_types;
  std::vector<std::string> _channel_types;

  std::vector<vertices_size_type> _processor_type_instances;
  std::vector<edges_size_type> _channel_type_instances;
};

}

#endif // GUARD_ARCH_GRAPH_H


/*
 * Copyright (c) 2011, Mattia Penati <mattia.penati@gmail.com>
 * 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 Politecnico di Milano nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef AMA_TENSOR_DETAIL_TENSOR_OUTER_HPP
#define AMA_TENSOR_DETAIL_TENSOR_OUTER_HPP 1

#include <ama/tensor/detail/tensor_base.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/fold.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/vector/vector0_c.hpp>

namespace ama
{
  namespace tensor_
  {

    /* forward declaration*/
    template <typename LEFT, typename RIGHT> class tensor_outer;

    /* traits definition */
    template <typename LEFT, typename RIGHT>
    struct tensor_traits< tensor_outer<LEFT, RIGHT> >
    {
      typedef typename LEFT::value_type value_type;

      typedef typename LEFT::dimension_type dimension_type;

      typedef typename ::boost::mpl::plus<
                             typename LEFT::controvariant_type
                           , typename RIGHT::controvariant_type
                           >::type controvariant_type;
      typedef typename ::boost::mpl::plus<
                             typename LEFT::covariant_type
                           , typename RIGHT::covariant_type
                           >::type covariant_type;

      typedef ::boost::mpl::false_ is_assignable;
      typedef ::boost::mpl::true_ is_temporary;
    };

    /* class declaration */
    template <typename LEFT, typename RIGHT>
    class tensor_outer:
        public tensor_base< tensor_outer<LEFT, RIGHT> >
    {
    protected:
      typedef tensor_base< tensor_outer<LEFT, RIGHT> > base_type;
      typedef tensor_outer<LEFT, RIGHT> derived_type;

    protected:
      typedef LEFT left_operand_type;
      typedef RIGHT right_operand_type;

    public:
      typedef typename base_type::value_type value_type;

    public:
      /* costructor */
      tensor_outer(left_operand_type const & left,
                   right_operand_type const & right)
          : m_left(left),
            m_right(right) { }

    public:
      /* retrieve the value */
      template <typename ILIST>
      value_type at() const
      {
        namespace mpl = ::boost::mpl;

        /* iterator */
        typedef typename mpl::begin<ILIST>::type begin1;
        typedef typename mpl::advance<begin1, typename LEFT::controvariant_type>::type begin2;
        typedef typename mpl::advance<begin2, typename RIGHT::controvariant_type>::type begin3;
        typedef typename mpl::advance<begin3, typename LEFT::covariant_type>::type begin4;
        typedef typename mpl::advance<begin4, typename RIGHT::covariant_type>::type begin5;
        typedef typename mpl::end<ILIST>::type end;

        /* check for no errors in the code */
        BOOST_MPL_ASSERT((mpl::equal_to<mpl::distance<end,begin5>,mpl::size_t<0> >));

        /* split multi-index */
        typedef typename mpl::fold<
              mpl::iterator_range<begin1, begin2>
            , mpl::vector0_c<size_t>
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type controvariant_left;

        typedef typename mpl::fold<
              mpl::iterator_range<begin2, begin3>
            , mpl::vector0_c<size_t>
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type controvariant_right;

        typedef typename mpl::fold<
              mpl::iterator_range<begin3, begin4>
            , mpl::vector0_c<size_t>
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type covariant_left;

        typedef typename mpl::fold<
              mpl::iterator_range<begin4, begin5>
            , mpl::vector0_c<size_t>
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type covariant_right;

        /* append the splittend multi-index */
        typedef typename mpl::fold<
              covariant_left
            , controvariant_left
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type left_ilist;

        typedef typename mpl::fold<
              covariant_right
            , controvariant_right
            , mpl::push_back<mpl::_1, mpl::_2>
            >::type right_ilist;

        return (m_left.template at<left_ilist>()) *
            (m_right.template at<right_ilist>());
      }

    protected:
      /* if the operand is temporary we save a copy, otherwise a reference */
      typedef typename LEFT::is_temporary left_operand_is_temporary;
      typedef typename ::boost::mpl::if_<
            left_operand_is_temporary
          , left_operand_type const
          , left_operand_type const &
          >::type const_left_operand_type;

      /* if the operand is temporary we save a copy, otherwise a reference */
      typedef typename RIGHT::is_temporary right_operand_is_temporary;
      typedef typename ::boost::mpl::if_<
            right_operand_is_temporary
          , right_operand_type const
          , right_operand_type const &
          >::type const_right_operand_type;

      /* members */
      const_left_operand_type m_left;
      const_right_operand_type m_right;
    };

  }
}

#endif /* AMA_TENSOR_DETAIL_TENSOR_OUTER_HPP */


#ifndef MANIPULATION_FUNCTION_OBJECT_H
#define MANIPULATION_FUNCTION_OBJECT_H

#include <Eigen/Dense>
#include <iostream>
#include <string>

#include <avatar_locomanipulation/cubic_interpolation_module/cubic_interpolation_six_dim_vec.hpp>
#include <avatar_locomanipulation/helpers/param_handler.hpp>

#define MANIPULATE_TYPE_RIGHT_HAND 0
#define MANIPULATE_TYPE_LEFT_HAND 1
#define MANIPULATE_TYPE_BOTH_HANDS 2

// Data container for a manipulation function object
class ManipulationFunction{
public:
  ManipulationFunction();
  ManipulationFunction(std::string filename);
  ~ManipulationFunction();

  void setWorldTransform(const Eigen::Vector3d & translation, const Eigen::Quaterniond & orientation);
  void getPose(double & s, Eigen::Vector3d & pos_out, Eigen::Quaterniond & quat_out);

  void setWaypointsFromYaml(std::string filename);

  void setRightWaypointsFromYaml(std::string filename);
  void setLeftWaypointsFromYaml(std::string filename);

  void getRightHandPose(double & s, Eigen::Vector3d & pos_out, Eigen::Quaterniond & quat_out);
  void getLeftHandPose(double & s, Eigen::Vector3d & pos_out, Eigen::Quaterniond & quat_out);

  void setManipulationType(int type);
  int getManipulationType();

private:
  void common_initialization();

  int manipulation_type;

  std::shared_ptr<CubicInterpolationSixDimVec> f_rh_s;
  std::shared_ptr<CubicInterpolationSixDimVec> f_lh_s;

  std::string rh_waypoint_list_yaml_filename;
  std::string lh_waypoint_list_yaml_filename;
  int N_waypoints;

  // Local frame position and orientation of the interpolated waypoint
  Eigen::Vector3d rhand_local_pos;
  Eigen::Quaterniond rhand_local_ori;

  Eigen::Vector3d lhand_local_pos;
  Eigen::Quaterniond lhand_local_ori;

  bool right_hand_waypoints_set = false;
  bool left_hand_waypoints_set = false;

  // Homogenous transform w.r.t world frame to convert
  Eigen::Vector3d transform_translation;
  Eigen::Quaterniond transform_orientation;

};

#endif


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_AXPY_HPP
#define BOOST_NUMERIC_BINDINGS_BLAS_LEVEL1_AXPY_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/has_linear_array.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>

//
// The BLAS-backend is selected by defining a pre-processor variable,
//  which can be one of
// * for CBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
// * for CUBLAS, define BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
// * netlib-compatible BLAS is the default
//
#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
#include <boost/numeric/bindings/blas/detail/cblas.h>
#include <boost/numeric/bindings/blas/detail/cblas_option.hpp>
#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
#include <boost/numeric/bindings/blas/detail/cublas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#else
#include <boost/numeric/bindings/blas/detail/blas.h>
#include <boost/numeric/bindings/blas/detail/blas_option.hpp>
#endif

namespace boost {
namespace numeric {
namespace bindings {
namespace blas {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end BLAS-routine.
//
namespace detail {

#if defined BOOST_NUMERIC_BINDINGS_BLAS_CBLAS
//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * float value-type.
//
inline void axpy( const int n, const float a, const float* x, const int incx,
        float* y, const int incy ) {
    cblas_saxpy( n, a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * double value-type.
//
inline void axpy( const int n, const double a, const double* x,
        const int incx, double* y, const int incy ) {
    cblas_daxpy( n, a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<float> value-type.
//
inline void axpy( const int n, const std::complex<float> a,
        const std::complex<float>* x, const int incx, std::complex<float>* y,
        const int incy ) {
    cblas_caxpy( n, &a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CBLAS backend, and
// * complex<double> value-type.
//
inline void axpy( const int n, const std::complex<double> a,
        const std::complex<double>* x, const int incx,
        std::complex<double>* y, const int incy ) {
    cblas_zaxpy( n, &a, x, incx, y, incy );
}

#elif defined BOOST_NUMERIC_BINDINGS_BLAS_CUBLAS
//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * float value-type.
//
inline void axpy( const int n, const float a, const float* x, const int incx,
        float* y, const int incy ) {
    cublasSaxpy( n, a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * double value-type.
//
inline void axpy( const int n, const double a, const double* x,
        const int incx, double* y, const int incy ) {
    cublasDaxpy( n, a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<float> value-type.
//
inline void axpy( const int n, const std::complex<float> a,
        const std::complex<float>* x, const int incx, std::complex<float>* y,
        const int incy ) {
    cublasCaxpy( n, a, x, incx, y, incy );
}

//
// Overloaded function for dispatching to
// * CUBLAS backend, and
// * complex<double> value-type.
//
inline void axpy( const int n, const std::complex<double> a,
        const std::complex<double>* x, const int incx,
        std::complex<double>* y, const int incy ) {
    cublasZaxpy( n, a, x, incx, y, incy );
}

#else
//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * float value-type.
//
inline void axpy( const fortran_int_t n, const float a, const float* x,
        const fortran_int_t incx, float* y, const fortran_int_t incy ) {
    BLAS_SAXPY( &n, &a, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * double value-type.
//
inline void axpy( const fortran_int_t n, const double a, const double* x,
        const fortran_int_t incx, double* y, const fortran_int_t incy ) {
    BLAS_DAXPY( &n, &a, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<float> value-type.
//
inline void axpy( const fortran_int_t n, const std::complex<float> a,
        const std::complex<float>* x, const fortran_int_t incx,
        std::complex<float>* y, const fortran_int_t incy ) {
    BLAS_CAXPY( &n, &a, x, &incx, y, &incy );
}

//
// Overloaded function for dispatching to
// * netlib-compatible BLAS backend (the default), and
// * complex<double> value-type.
//
inline void axpy( const fortran_int_t n, const std::complex<double> a,
        const std::complex<double>* x, const fortran_int_t incx,
        std::complex<double>* y, const fortran_int_t incy ) {
    BLAS_ZAXPY( &n, &a, x, &incx, y, &incy );
}

#endif

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to axpy.
//
template< typename Value >
struct axpy_impl {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;
    typedef void result_type;

    //
    // Static member function that
    // * Deduces the required arguments for dispatching to BLAS, and
    // * Asserts that most arguments make sense.
    //
    template< typename VectorX, typename VectorY >
    static result_type invoke( const value_type a, const VectorX& x,
            VectorY& y ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (is_same< typename remove_const<
                typename bindings::value_type< VectorX >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorY >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorX >::value) );
        BOOST_STATIC_ASSERT( (bindings::has_linear_array< VectorY >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorY >::value) );
        detail::axpy( bindings::size(x), a, bindings::begin_value(x),
                bindings::stride(x), bindings::begin_value(y),
                bindings::stride(y) );
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. Calls
// to these functions are passed to the axpy_impl classes. In the
// documentation, the const-overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for axpy. Its overload differs for
//
template< typename VectorX, typename VectorY >
inline typename axpy_impl< typename bindings::value_type<
        VectorX >::type >::result_type
axpy( const typename bindings::value_type< VectorX >::type a,
        const VectorX& x, VectorY& y ) {
    axpy_impl< typename bindings::value_type<
            VectorX >::type >::invoke( a, x, y );
}

} // namespace blas
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif