//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2021 Ilias Khairullin <ilias@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE crypto3_marshalling_r1cs_gg_ppzksnark_verification_key_test

#include <boost/test/unit_test.hpp>
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <iostream>
#include <iomanip>

#include <nil/marshalling/status_type.hpp>
#include <nil/marshalling/field_type.hpp>
#include <nil/marshalling/endianness.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/number.hpp>

#include <nil/crypto3/algebra/random_element.hpp>
#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/detail/marshalling.hpp>
#include <nil/crypto3/algebra/pairing/bls12.hpp>
#include <nil/crypto3/algebra/pairing/mnt4.hpp>
#include <nil/crypto3/algebra/pairing/mnt6.hpp>

#include <nil/crypto3/zk/snark/sparse_vector.hpp>
#include <nil/crypto3/zk/snark/accumulation_vector.hpp>

#include <nil/crypto3/marshalling/zk/types/r1cs_gg_ppzksnark/verification_key.hpp>

template<typename TIter>
void print_byteblob(TIter iter_begin, TIter iter_end) {
    for (TIter it = iter_begin; it != iter_end; it++) {
        std::cout << std::hex << int(*it) << std::endl;
    }
}

template<typename FpCurveGroupElement>
void print_fp_curve_group_element(FpCurveGroupElement e) {
    std::cout << e.X.data << " " << e.Y.data << " " << e.Z.data << std::endl;
}

template<typename Fp2CurveGroupElement>
void print_fp2_curve_group_element(Fp2CurveGroupElement e) {
    std::cout << "(" << e.X.data[0].data << " " << e.X.data[1].data << ") (" << e.Y.data[0].data << " "
              << e.Y.data[1].data << ") (" << e.Z.data[0].data << " " << e.Z.data[1].data << ")" << std::endl;
}

template<typename Endianness, typename VerificationKeyMarshaling, typename VerificationKey,
         typename CurveType = typename VerificationKey::curve_type>
void test_verification_key(const VerificationKey &val) {

    using namespace nil::crypto3::marshalling;

    std::size_t units_bits = 8;
    using unit_type = unsigned char;
    using verification_key_marshaling_type = VerificationKeyMarshaling;

    verification_key_marshaling_type filled_val =
        types::fill_r1cs_gg_ppzksnark_verification_key<VerificationKey, Endianness>(val);

    VerificationKey constructed_val =
        types::make_r1cs_gg_ppzksnark_verification_key<VerificationKey, Endianness>(filled_val);
    BOOST_CHECK(val == constructed_val);

    std::size_t unitblob_size = filled_val.length();

    std::vector<unit_type> cv;
    cv.resize(unitblob_size, 0x00);

    auto write_iter = cv.begin();

    nil::marshalling::status_type status = filled_val.write(write_iter, cv.size());

    verification_key_marshaling_type test_val_read;

    auto read_iter = cv.begin();
    status = test_val_read.read(read_iter, cv.size());

    VerificationKey constructed_val_read =
        types::make_r1cs_gg_ppzksnark_verification_key<VerificationKey, Endianness>(test_val_read);

    BOOST_CHECK(val == constructed_val_read);
}

// TODO: move to pubkey marshling
template<typename Endianness, typename KeyMarshaling, typename Key,
         typename CurveType = typename Key::scheme_type::curve_type>
void test_pubkey(const Key &val) {

    using namespace nil::crypto3::marshalling;

    std::size_t units_bits = 8;
    using unit_type = unsigned char;
    using key_marshaling_type = KeyMarshaling;

    key_marshaling_type filled_val = types::fill_public_key<Key, Endianness>(val);

    Key constructed_val = types::make_public_key<Key, Endianness>(filled_val);
    BOOST_CHECK(val == constructed_val);

    std::size_t unitblob_size = filled_val.length();

    std::vector<unit_type> cv;
    cv.resize(unitblob_size, 0x00);

    auto write_iter = cv.begin();

    nil::marshalling::status_type status = filled_val.write(write_iter, cv.size());

    key_marshaling_type test_val_read;

    auto read_iter = cv.begin();
    status = test_val_read.read(read_iter, cv.size());

    Key constructed_val_read = types::make_public_key<Key, Endianness>(test_val_read);

    BOOST_CHECK(val == constructed_val_read);
}

template<typename VerificationKey, typename VerificationKeyMarshaling, typename Endianness, std::size_t TSize,
         typename CurveType = typename VerificationKey::curve_type>
typename std::enable_if<
    std::is_same<nil::crypto3::zk::snark::r1cs_gg_ppzksnark_verification_key<CurveType>, VerificationKey>::value>::type
    test_verification_key() {
    using g1_type = typename CurveType::template g1_type<>;
    using g2_type = typename CurveType::template g2_type<>;
    using gt_type = typename CurveType::gt_type;

    std::cout << std::hex;
    std::cerr << std::hex;
    for (unsigned i = 0; i < 128; ++i) {
        if (!(i % 16) && i) {
            std::cout << std::dec << i << " tested" << std::endl;
        }
        typename g1_type::value_type first = nil::crypto3::algebra::random_element<g1_type>();
        std::vector<typename g1_type::value_type> rest;
        for (std::size_t i = 0; i < TSize; i++) {
            rest.push_back(nil::crypto3::algebra::random_element<g1_type>());
        }
        test_verification_key<Endianness, VerificationKeyMarshaling>(VerificationKey(
            nil::crypto3::algebra::random_element<gt_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            std::move(nil::crypto3::zk::snark::accumulation_vector<g1_type>(std::move(first), std::move(rest)))));
    }
}

template<typename VerificationKey, typename VerificationKeyMarshaling, typename Endianness, std::size_t TSize,
         typename CurveType = typename VerificationKey::curve_type>
typename std::enable_if<std::is_same<nil::crypto3::zk::snark::r1cs_gg_ppzksnark_extended_verification_key<CurveType>,
                                     VerificationKey>::value>::type
    test_verification_key() {
    using g1_type = typename CurveType::template g1_type<>;
    using g2_type = typename CurveType::template g2_type<>;
    using gt_type = typename CurveType::gt_type;

    std::cout << std::hex;
    std::cerr << std::hex;
    for (unsigned i = 0; i < 128; ++i) {
        if (!(i % 16) && i) {
            std::cout << std::dec << i << " tested" << std::endl;
        }
        typename g1_type::value_type first = nil::crypto3::algebra::random_element<g1_type>();
        std::vector<typename g1_type::value_type> rest;
        for (std::size_t i = 0; i < TSize; i++) {
            rest.push_back(nil::crypto3::algebra::random_element<g1_type>());
        }
        test_verification_key<Endianness, VerificationKeyMarshaling>(VerificationKey(
            nil::crypto3::algebra::random_element<gt_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            nil::crypto3::algebra::random_element<g2_type>(),
            nil::crypto3::algebra::random_element<g1_type>(),
            std::move(nil::crypto3::zk::snark::accumulation_vector<g1_type>(std::move(first), std::move(rest))),
            nil::crypto3::algebra::random_element<g1_type>()));
    }
}

// TODO: move to pubkey marshling
template<typename PublicKey, typename PublicKeyMarshaling, typename Endianness, std::size_t TSize,
         typename CurveType = typename PublicKey::scheme_type::curve_type>
typename std::enable_if<
    std::is_same<nil::crypto3::pubkey::public_key<nil::crypto3::pubkey::elgamal_verifiable<
                     typename PublicKey::scheme_type::curve_type, PublicKey::scheme_type::block_bits>>,
                 PublicKey>::value>::type
    test_pubkey() {
    using g1_type = typename PublicKey::g1_type;
    using g2_type = typename PublicKey::g2_type;

    std::cout << std::hex;
    std::cerr << std::hex;
    for (unsigned i = 0; i < 128; ++i) {
        if (!(i % 16) && i) {
            std::cout << std::dec << i << " tested" << std::endl;
        }
        std::vector<typename g1_type::value_type> delta_s_g1;
        std::vector<typename g1_type::value_type> t_g1;
        std::vector<typename g2_type::value_type> t_g2;
        for (std::size_t i = 0; i < TSize; i++) {
            delta_s_g1.push_back(nil::crypto3::algebra::random_element<g1_type>());
            t_g1.push_back(nil::crypto3::algebra::random_element<g1_type>());
            t_g2.push_back(nil::crypto3::algebra::random_element<g2_type>());
        }
        t_g2.push_back(nil::crypto3::algebra::random_element<g2_type>());
        test_pubkey<Endianness, PublicKeyMarshaling>(PublicKey(
            nil::crypto3::algebra::random_element<g1_type>(), std::move(delta_s_g1), std::move(t_g1), std::move(t_g2),
            nil::crypto3::algebra::random_element<g1_type>(), nil::crypto3::algebra::random_element<g1_type>()));
    }
}

BOOST_AUTO_TEST_SUITE(verification_key_test_suite)

BOOST_AUTO_TEST_CASE(r1cs_gg_ppzksnark_verification_key_bls12_381_be) {
    using endianness = nil::marshalling::option::big_endian;
    using key_type =
        nil::crypto3::zk::snark::r1cs_gg_ppzksnark_verification_key<nil::crypto3::algebra::curves::bls12<381>>;
    using key_marshaling_type =
        nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_verification_key<nil::marshalling::field_type<endianness>,
                                                                             key_type>;
    std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key big-endian test started" << std::endl;
    test_verification_key<key_type, key_marshaling_type, endianness, 5>();
    std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key big-endian test finished" << std::endl;
}

BOOST_AUTO_TEST_CASE(r1cs_gg_ppzksnark_extended_verification_key_bls12_381_be) {
    using endianness = nil::marshalling::option::big_endian;
    using key_type =
        nil::crypto3::zk::snark::r1cs_gg_ppzksnark_extended_verification_key<nil::crypto3::algebra::curves::bls12<381>>;
    using key_marshaling_type = nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_extended_verification_key<
        nil::marshalling::field_type<endianness>, key_type>;
    std::cout << "BLS12-381 r1cs_gg_ppzksnark extended verification key big-endian test started" << std::endl;
    test_verification_key<key_type, key_marshaling_type, endianness, 5>();
    std::cout << "BLS12-381 r1cs_gg_ppzksnark extended verification key big-endian test finished" << std::endl;
}

// TODO: move to pubkey marshling
BOOST_AUTO_TEST_CASE(elgamal_verifiable_public_key_bls12_381_be) {
    using endianness = nil::marshalling::option::big_endian;
    using key_type = nil::crypto3::pubkey::public_key<
        nil::crypto3::pubkey::elgamal_verifiable<nil::crypto3::algebra::curves::bls12<381>>>;
    using key_marshaling_type =
        nil::crypto3::marshalling::types::elgamal_verifiable_public_key<nil::marshalling::field_type<endianness>,
                                                                        key_type>;
    std::cout << "BLS12-381 r1cs_gg_ppzksnark extended verification key big-endian test started" << std::endl;
    test_pubkey<key_type, key_marshaling_type, endianness, 5>();
    std::cout << "BLS12-381 r1cs_gg_ppzksnark extended verification key big-endian test finished" << std::endl;
}

BOOST_AUTO_TEST_CASE(r1cs_gg_ppzksnark_extended_verification_key_bls12_381_be_error_length) {
    using endianness = nil::marshalling::option::big_endian;
    using curve_type = nil::crypto3::algebra::curves::bls12<381>;
    using key_type = nil::crypto3::zk::snark::r1cs_gg_ppzksnark_extended_verification_key<curve_type>;
    using key_marshaling_type = nil::crypto3::marshalling::types::r1cs_gg_ppzksnark_extended_verification_key<
        nil::marshalling::field_type<endianness>, key_type>;

    using g1_type = typename curve_type::template g1_type<>;
    using g2_type = typename curve_type::template g2_type<>;
    using gt_type = typename curve_type::gt_type;

    using gt_marshalling_type =
        nil::crypto3::marshalling::types::field_element<nil::marshalling::field_type<endianness>, gt_type>;
    using g2_marshalling_type =
        nil::crypto3::marshalling::types::curve_element<nil::marshalling::field_type<endianness>, g2_type>;
    using g1_marshalling_type =
        nil::crypto3::marshalling::types::curve_element<nil::marshalling::field_type<endianness>, g1_type>;
    using accumulation_vector_marshalling_type =
        nil::crypto3::marshalling::types::accumulation_vector<nil::marshalling::field_type<endianness>,
                                                              nil::crypto3::zk::snark::accumulation_vector<g1_type>>;
    gt_marshalling_type gt_marshaling;
    std::cout << "Not ok: " << gt_marshaling.length() << std::endl;
    gt_marshalling_type filled_gt = nil::crypto3::marshalling::types::fill_field_element<gt_type, endianness>(
        nil::crypto3::algebra::random_element<gt_type>());
    std::cout << "Ok only after initialization: " << filled_gt.length() << std::endl;

    g2_marshalling_type g2_marshaling;
    std::cout << "Ok: " << g2_marshaling.length() << std::endl;

    g1_marshalling_type g1_marshaling;
    std::cout << "Ok: " << g1_marshaling.length() << std::endl;

    accumulation_vector_marshalling_type accumulation_vector_marshalling;
    std::cout << "Seems ok, full information about size should be available after initialization: "
              << accumulation_vector_marshalling.length() << std::endl;
    typename g1_type::value_type first = nil::crypto3::algebra::random_element<g1_type>();
    std::vector<typename g1_type::value_type> rest;
    for (std::size_t i = 0; i < 5; i++) {
        rest.push_back(nil::crypto3::algebra::random_element<g1_type>());
    }
    nil::crypto3::zk::snark::accumulation_vector<g1_type> acc_vec(std::move(first), std::move(rest));
    accumulation_vector_marshalling_type filled_acc_vec = nil::crypto3::marshalling::types::fill_accumulation_vector<
        nil::crypto3::zk::snark::accumulation_vector<g1_type>, endianness>(acc_vec);
    std::cout << "Ok: " << filled_acc_vec.length() << std::endl;

    // key_type key(nil::crypto3::algebra::random_element<gt_type>(),
    //              nil::crypto3::algebra::random_element<g2_type>(),
    //              nil::crypto3::algebra::random_element<g2_type>(),
    //              nil::crypto3::algebra::random_element<g1_type>(),
    //              std::move(zk::snark::accumulation_vector<g1_type>(std::move(first), std::move(rest))),
    //              nil::crypto3::algebra::random_element<g1_type>());
}

// BOOST_AUTO_TEST_CASE(sparse_vector_bls12_381_le) {
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key little-endian test started" << std::endl;
//     test_verification_key<nil::crypto3::algebra::curves::bls12<381>, nil::marshalling::option::little_endian, 5>();
//     std::cout << "BLS12-381 r1cs_gg_ppzksnark verification key little-endian test finished" << std::endl;
// }

BOOST_AUTO_TEST_SUITE_END()


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>

namespace sim_mob
{
namespace simple_password
{
void encryptionFunc(std::string &nString)
{
    const int KEY = 23;                       // Key used for XOR operation
    int strLen = (nString.length());          // Grab string length for iterating thru string
    char *cString = (char*)(nString.c_str()); // Convert string to C string so we can actually iterate through it(std::string will treat it as one full object :()

    for (int i = 0; i < strLen; i++)          // time to iterate thru string and XOR each individual char.
    {
     *(cString+i) = (*(cString+i) ^ KEY);     // ^ is the binary operator for XOR
    }
}

void decryptionFunc(std::string &nString)    // Time to undo what we did from above :D
{
    const int KEY = 23;
    int strLen = (nString.length());
    char *cString = (char*)(nString.c_str());

    for (int i = 0; i < strLen; i++)
    {
     *(cString+i) = (*(cString+i) ^ KEY);
    }
}
void saveBinary(std::string value)
{
  std::ofstream file("shared/password/password");
  boost::archive::binary_oarchive oa(file);
  oa << value;
}

void loadBinary(std::string & value)
{
    try
    {
        std::ifstream file("shared/password/password");
        boost::archive::binary_iarchive ia(file);
        ia >> value;
    }
    catch(...)
    {

    }
}

void save(std::string source)
{

    encryptionFunc(source);
    saveBinary(source);
}

std::string load(std::string dest)
{

    loadBinary(dest);
    decryptionFunc(dest);
    return dest;
}

}//simple_password
}//sim_mob

//int main(int argc, char * argv[])
//{
//  std::ostringstream out;
//  out << argv[1];
//  std::string source = out.str();
//    std::string dest;
//    loadBinary(dest);
//    decryptionFunc(dest);
//    std::cout << dest << std::endl;
//
//
//}


#include "drake/systems/sensors/rgbd_sensor.h"

#include <algorithm>
#include <limits>
#include <string>
#include <utility>

#include <Eigen/Dense>

#include "drake/common/text_logging.h"
#include "drake/geometry/scene_graph.h"
#include "drake/math/rigid_transform.h"
#include "drake/math/rotation_matrix.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/zero_order_hold.h"
#include "drake/systems/rendering/pose_vector.h"
#include "drake/systems/sensors/camera_info.h"
#include "drake/systems/sensors/image.h"

namespace drake {
namespace systems {
namespace sensors {

using Eigen::Translation3d;
using geometry::FrameId;
using geometry::QueryObject;
using geometry::SceneGraph;
using geometry::render::ColorRenderCamera;
using geometry::render::DepthRange;
using geometry::render::DepthRenderCamera;
using math::RigidTransformd;
using std::make_pair;
using std::move;
using std::pair;

RgbdSensor::RgbdSensor(FrameId parent_id, const RigidTransformd& X_PB,
                       const DepthRenderCamera& depth_camera,
                       bool show_color_window)
    : RgbdSensor(parent_id, X_PB,
                 ColorRenderCamera(depth_camera.core(), show_color_window),
                 depth_camera) {}

RgbdSensor::RgbdSensor(FrameId parent_id, const RigidTransformd& X_PB,
                       ColorRenderCamera color_camera,
                       DepthRenderCamera depth_camera)
    : parent_frame_id_(parent_id),
      color_camera_(move(color_camera)),
      depth_camera_(move(depth_camera)),
      X_PB_(X_PB) {
  const CameraInfo& color_intrinsics = color_camera_.core().intrinsics();
  const CameraInfo& depth_intrinsics = depth_camera_.core().intrinsics();

  query_object_input_port_ = &this->DeclareAbstractInputPort(
      "geometry_query", Value<geometry::QueryObject<double>>{});

  ImageRgba8U color_image(color_intrinsics.width(), color_intrinsics.height());
  color_image_port_ = &this->DeclareAbstractOutputPort(
      "color_image", color_image, &RgbdSensor::CalcColorImage);

  ImageDepth32F depth32(depth_intrinsics.width(), depth_intrinsics.height());
  depth_image_32F_port_ = &this->DeclareAbstractOutputPort(
      "depth_image_32f", depth32, &RgbdSensor::CalcDepthImage32F);

  ImageDepth16U depth16(depth_intrinsics.width(), depth_intrinsics.height());
  depth_image_16U_port_ = &this->DeclareAbstractOutputPort(
      "depth_image_16u", depth16, &RgbdSensor::CalcDepthImage16U);

  ImageLabel16I label_image(color_intrinsics.width(),
                            color_intrinsics.height());
  label_image_port_ = &this->DeclareAbstractOutputPort(
      "label_image", label_image, &RgbdSensor::CalcLabelImage);

  X_WB_pose_port_ = &this->DeclareVectorOutputPort(
      "X_WB", rendering::PoseVector<double>(), &RgbdSensor::CalcX_WB);

  // The depth_16U represents depth in *millimeters*. With 16 bits there is
  // an absolute limit on the farthest distance it can register. This tests to
  // see if the user has specified a maximum depth value that exceeds that
  // value.
  const float kMaxValidDepth16UInM =
      (std::numeric_limits<uint16_t>::max() - 1) / 1000.;
  const double max_depth = depth_camera_.depth_range().max_depth();
  if (max_depth > kMaxValidDepth16UInM) {
    drake::log()->warn(
        "Specified max depth is {} m > max valid depth for 16 bits {} m. "
        "depth_image_16u might not be able to capture the full depth range.",
        max_depth, kMaxValidDepth16UInM);
  }
}

const InputPort<double>& RgbdSensor::query_object_input_port() const {
  return *query_object_input_port_;
}

const OutputPort<double>& RgbdSensor::color_image_output_port() const {
  return *color_image_port_;
}

const OutputPort<double>& RgbdSensor::depth_image_32F_output_port() const {
  return *depth_image_32F_port_;
}

const OutputPort<double>& RgbdSensor::depth_image_16U_output_port() const {
  return *depth_image_16U_port_;
}

const OutputPort<double>& RgbdSensor::label_image_output_port() const {
  return *label_image_port_;
}

const OutputPort<double>& RgbdSensor::X_WB_output_port() const {
  return *X_WB_pose_port_;
}

void RgbdSensor::CalcColorImage(const Context<double>& context,
                                ImageRgba8U* color_image) const {
  const QueryObject<double>& query_object = get_query_object(context);
  query_object.RenderColorImage(
      color_camera_, parent_frame_id_,
      X_PB_ * color_camera_.core().sensor_pose_in_camera_body(), color_image);
}

void RgbdSensor::CalcDepthImage32F(const Context<double>& context,
                                   ImageDepth32F* depth_image) const {
  const QueryObject<double>& query_object = get_query_object(context);
  query_object.RenderDepthImage(
      depth_camera_, parent_frame_id_,
      X_PB_ * depth_camera_.core().sensor_pose_in_camera_body(), depth_image);
}

void RgbdSensor::CalcDepthImage16U(const Context<double>& context,
                                   ImageDepth16U* depth_image) const {
  ImageDepth32F depth32(depth_image->width(), depth_image->height());
  CalcDepthImage32F(context, &depth32);
  ConvertDepth32FTo16U(depth32, depth_image);
}

void RgbdSensor::CalcLabelImage(const Context<double>& context,
                                ImageLabel16I* label_image) const {
  const QueryObject<double>& query_object = get_query_object(context);
  query_object.RenderLabelImage(
      color_camera_, parent_frame_id_,
      X_PB_ * color_camera_.core().sensor_pose_in_camera_body(), label_image);
}

void RgbdSensor::CalcX_WB(const Context<double>& context,
                          rendering::PoseVector<double>* pose_vector) const {
  // Calculates X_WB.
  RigidTransformd X_WB;
  if (parent_frame_id_ == SceneGraph<double>::world_frame_id()) {
    X_WB = X_PB_;
  } else {
    const QueryObject<double>& query_object = get_query_object(context);
    X_WB = query_object.GetPoseInWorld(parent_frame_id_) * X_PB_;
  }

  Translation3d trans{X_WB.translation()};
  pose_vector->set_translation(trans);

  pose_vector->set_rotation(X_WB.rotation().ToQuaternion());
}

void RgbdSensor::ConvertDepth32FTo16U(const ImageDepth32F& d32,
                                      ImageDepth16U* d16) {
  // Convert to mm and 16bits.
  const float kDepth16UOverflowDistance =
      std::numeric_limits<uint16_t>::max() / 1000.;
  for (int w = 0; w < d16->width(); w++) {
    for (int h = 0; h < d16->height(); h++) {
      const double dist = std::min(d32.at(w, h)[0], kDepth16UOverflowDistance);
      d16->at(w, h)[0] = static_cast<uint16_t>(dist * 1000);
    }
  }
}

// Note: Ideally, this would be inlined. However, inlining this caused GCC
// to do something weird with GeometryState such that the rgbd_sensor_test.cc
// was unable to instantiate an GeometryStateTester that GCC recognized as a
// friend to GeometryState.
const geometry::QueryObject<double>& RgbdSensor::get_query_object(
    const Context<double>& context) const {
  return query_object_input_port().Eval<geometry::QueryObject<double>>(context);
}

RgbdSensorDiscrete::RgbdSensorDiscrete(std::unique_ptr<RgbdSensor> camera,
                                       double period, bool render_label_image)
    : camera_(camera.get()), period_(period) {
  const auto& color_camera_info = camera->color_camera_info();
  const auto& depth_camera_info = camera->depth_camera_info();

  DiagramBuilder<double> builder;
  builder.AddSystem(move(camera));
  query_object_port_ =
      builder.ExportInput(camera_->query_object_input_port(), "geometry_query");

  // Color image.
  const Value<ImageRgba8U> image_color(color_camera_info.width(),
                                       color_camera_info.height());
  const auto* const zoh_color =
      builder.AddSystem<ZeroOrderHold>(period_, image_color);
  builder.Connect(camera_->color_image_output_port(),
                  zoh_color->get_input_port());
  output_port_color_image_ =
      builder.ExportOutput(zoh_color->get_output_port(), "color_image");

  // Depth images.
  const Value<ImageDepth32F> image_depth_32F(depth_camera_info.width(),
                                             depth_camera_info.height());
  const auto* const zoh_depth_32F =
      builder.AddSystem<ZeroOrderHold>(period_, image_depth_32F);
  builder.Connect(camera_->depth_image_32F_output_port(),
                  zoh_depth_32F->get_input_port());
  output_port_depth_image_32F_ =
      builder.ExportOutput(zoh_depth_32F->get_output_port(), "depth_image_32f");

  // Depth images.
  const Value<ImageDepth16U> image_depth_16U(depth_camera_info.width(),
                                             depth_camera_info.height());
  const auto* const zoh_depth_16U =
      builder.AddSystem<ZeroOrderHold>(period_, image_depth_16U);
  builder.Connect(camera_->depth_image_16U_output_port(),
                  zoh_depth_16U->get_input_port());
  output_port_depth_image_16U_ =
      builder.ExportOutput(zoh_depth_16U->get_output_port(), "depth_image_16u");

  // Label image.
  if (render_label_image) {
    const Value<ImageLabel16I> image_label(color_camera_info.width(),
                                           color_camera_info.height());
    const auto* const zoh_label =
        builder.AddSystem<ZeroOrderHold>(period_, image_label);
    builder.Connect(camera_->label_image_output_port(),
                    zoh_label->get_input_port());
    output_port_label_image_ =
        builder.ExportOutput(zoh_label->get_output_port(), "label_image");
  }

  // No need to place a ZOH on pose output.
  X_WB_output_port_ = builder.ExportOutput(camera_->X_WB_output_port(), "X_WB");

  builder.BuildInto(this);
}

}  // namespace sensors
}  // namespace systems
}  // namespace drake


#ifdef __i386__
  #pragma message("i386 Architecture detected, disabling EIGEN VECTORIZATION")
  #define EIGEN_DONT_VECTORIZE
  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
#else
  #pragma message("64bit Architecture detected, enabling EIGEN VECTORIZATION")
#endif

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/surface/convex_hull.h>
#include <pcl/common/common.h>
#include <boost/make_shared.hpp>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <collision_avoidance_pick_and_place/GetTargetPose.h>
#include <math.h>

// alias
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
typedef boost::shared_ptr<tf::TransformListener> TransformListenerPtr;

// constants
const std::string SENSOR_CLOUD_TOPIC = "sensor_cloud";
const std::string FILTERED_CLOUD_TOPIC = "filtered_cloud";
const std::string TARGET_RECOGNITION_SERVICE = "target_recognition";

// defaults
const double BOX_SCALE = 1.2f;
const double ANGLE_TOLERANCE = 0.1f* (M_PI/180.0f);

class TargetRecognition
{
public:
	TargetRecognition():
		box_filter_scale_(BOX_SCALE),
		angle_tolerance_(ANGLE_TOLERANCE)
	{

	}

	~TargetRecognition()
	{

	}

	bool init()
	{
		ros::NodeHandle nh;
		ros::NodeHandle ph("~");

		// read parameters
		if(ph.getParam("box_filter_scale",box_filter_scale_) &&
				ph.getParam("angle_tolerance",angle_tolerance_))
		{
			ROS_INFO_STREAM("target recognition read parameters successfully");
		}
		else
		{
			ROS_WARN_STREAM("target recognition did not find one or more parameters, using defaults");
		}

		// initializing service server
		target_detection_server = nh.advertiseService(TARGET_RECOGNITION_SERVICE,&TargetRecognition::target_recognition_callback,this);

		// initializing publisher
		filtered_cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(FILTERED_CLOUD_TOPIC,1);

		// initializing subscriber
		point_cloud_subscriber = nh.subscribe(SENSOR_CLOUD_TOPIC,1,&TargetRecognition::point_cloud_callback,this);

		// initializing transform listener
		transform_listener_ptr = TransformListenerPtr(new tf::TransformListener(nh,ros::Duration(1.0f)));

		// initializing cloud messages
		filtered_cloud_msg_ = sensor_msgs::PointCloud2();

		return true;

	}

	void run()
	{

		while(ros::ok())
		{
			ros::Duration(0.2f).sleep();
			ros::spinOnce();
		}
	}

protected:

	bool grab_sensor_snapshot(sensor_msgs::PointCloud2& msg)
	{
		// grab sensor data snapshot
		ros::NodeHandle nh;
		sensor_msgs::PointCloud2ConstPtr msg_ptr =
				ros::topic::waitForMessage<sensor_msgs::PointCloud2>(SENSOR_CLOUD_TOPIC,nh,
						ros::Duration(5.0f));

		// check for empty message
		if(msg_ptr != sensor_msgs::PointCloud2ConstPtr())
		{

			msg = *msg_ptr;
		}

		return msg_ptr != sensor_msgs::PointCloud2ConstPtr();
	}

	void point_cloud_callback(const sensor_msgs::PointCloud2ConstPtr msg)
	{
		sensor_cloud_msg_ = sensor_msgs::PointCloud2(*msg);
	}

	bool target_recognition_callback(collision_avoidance_pick_and_place::GetTargetPose::Request& req,
			collision_avoidance_pick_and_place::GetTargetPose::Response& res)
	{
		// transforms
		tf::StampedTransform world_to_sensor_tf;
		tf::Transform world_to_box_pick_tf;
		tf::Transform world_to_ar_tf;
		tf::Vector3 box_pick_position;

		// updating global variables
		box_length_ = req.shape.dimensions[0];
		box_width_ = req.shape.dimensions[1];
		box_height_ = req.shape.dimensions[2];
		world_frame_id_ = req.world_frame_id;
		ar_frame_id_ = req.ar_tag_frame_id;

		// get point cloud message
		sensor_msgs::PointCloud2 msg = sensor_cloud_msg_;
		if(msg.data.size() == 0)
		{
				ROS_ERROR_STREAM("Cloud message is invalid, returning detection failure");
				res.succeeded = false;
				return true;
		}

		// looking up transforms
		if(get_transform(world_frame_id_ , ar_frame_id_,world_to_ar_tf) &&
				get_transform(world_frame_id_,msg.header.frame_id,world_to_sensor_tf))
		{

			// convert from message to point cloud
			Cloud::Ptr sensor_cloud_ptr(new Cloud());
			pcl::fromROSMsg<pcl::PointXYZ>(msg,*sensor_cloud_ptr);

			// applying statistical removal
			pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
			sor.setInputCloud (sensor_cloud_ptr);
			sor.setMeanK (50);
			sor.setStddevMulThresh (1.0);
			sor.filter (*sensor_cloud_ptr);

			// creating cloud message for publisher
			Cloud::Ptr filtered_cloud_ptr(new Cloud());

			// converting to world coordinates
			Eigen::Affine3d eigen_3d;
			tf::transformTFToEigen(world_to_sensor_tf,eigen_3d);
			Eigen::Affine3f eigen_3f(eigen_3d);
			pcl::transformPointCloud(*sensor_cloud_ptr,*sensor_cloud_ptr,eigen_3f);

			// find height of box top surface
			double height;
			if(!detect_box_height(*sensor_cloud_ptr,world_to_ar_tf,height) )
			{
				ROS_ERROR_STREAM("Target height detection failed");
				res.succeeded = false;
				return true;
			}

			// updating box pick transform
			world_to_box_pick_tf = world_to_ar_tf;
			box_pick_position = world_to_box_pick_tf.getOrigin();
			box_pick_position.setZ(height);
			world_to_box_pick_tf.setOrigin(box_pick_position);

			// filtering box from sensor cloud
			filter_box(world_to_sensor_tf,
					world_to_box_pick_tf,*sensor_cloud_ptr,*filtered_cloud_ptr);

			// filter box at requested locations
			for(unsigned int i =0;i < req.remove_at_poses.size();i++)
			{
				tf::Transform world_to_box;
				tf::poseMsgToTF(req.remove_at_poses[i],world_to_box);

				// copying last filter cloud
				pcl::copyPointCloud(*filtered_cloud_ptr,*sensor_cloud_ptr);

				// filter box from last computed cloud
				filter_box(world_to_sensor_tf,world_to_box,*sensor_cloud_ptr,*filtered_cloud_ptr);
			}

			// transforming to world frame
			pcl::transformPointCloud(*filtered_cloud_ptr,*filtered_cloud_ptr,eigen_3f.inverse());

			// converting to message
			filtered_cloud_msg_ = sensor_msgs::PointCloud2();
			pcl::toROSMsg(*filtered_cloud_ptr,filtered_cloud_msg_);

			// populating response
			tf::poseTFToMsg(world_to_box_pick_tf,res.target_pose);
			res.succeeded = true ;

			// publishing cloud
			filtered_cloud_msg_.header.stamp = ros::Time::now()-ros::Duration(0.5f);
			filtered_cloud_publisher.publish(filtered_cloud_msg_);
		}
		else
		{
			res.succeeded = false;
		}

		return true;
	}

	bool get_transform(std::string target,std::string source,tf::Transform& trg_to_src_tf)
	{
		// create tf listener and broadcaster
		//static tf::TransformListener tf_listener;
		tf::StampedTransform trg_to_src_stamped;

		// find ar tag transform
		try
		{
			transform_listener_ptr->waitForTransform(target,source,ros::Time::now(),ros::Duration(4.0f));
			transform_listener_ptr->lookupTransform(target,source,ros::Time::now() - ros::Duration(0.2f),trg_to_src_stamped);

			// copying transform data
			trg_to_src_tf.setRotation( trg_to_src_stamped.getRotation());
			trg_to_src_tf.setOrigin(trg_to_src_stamped.getOrigin());

		}
		catch(tf::LookupException &e)
		{
			ROS_ERROR_STREAM("transform lookup for '"<<ar_frame_id_<<"' failed");
			return false;
		}
		catch(tf::ExtrapolationException &e)
		{
			ROS_ERROR_STREAM("transform lookup for '"<<ar_frame_id_<<"' failed");
			return false;
		}
		catch(tf::TransformException &e)
		{
			ROS_ERROR_STREAM("transform lookup for '"<<ar_frame_id_<<"' failed");
			return false;
		}

		return true;
	}

	bool detect_box_height(const Cloud& cloud,
			const tf::Transform &world_to_ar_tf,double& height )
	{
		// cloud objects
		Cloud::Ptr cloud_ptr= boost::make_shared<Cloud>(cloud);
		Cloud::Ptr filtered_cloud_ptr(new Cloud);

		// applying filter in x axis
		float min = world_to_ar_tf.getOrigin().x() - 0.2f*box_length_;
		float max = world_to_ar_tf.getOrigin().x() + 0.2f*box_length_;
		pcl::PassThrough<pcl::PointXYZ> filter;
		filter.setInputCloud(cloud_ptr);
		filter.setFilterFieldName("x");
		filter.setFilterLimits(min,max);
		filter.filter(*filtered_cloud_ptr);

		// applying filter in y axis
		min = world_to_ar_tf.getOrigin().y()- 0.2f*box_width_;
		max = world_to_ar_tf.getOrigin().y() + 0.2f*box_width_;;
		filter.setInputCloud(filtered_cloud_ptr);
		filter.setFilterFieldName("y");
		filter.setFilterLimits(min,max);
		filter.filter(*filtered_cloud_ptr);

		// computing centroid
		Eigen::Vector4f centroid;
		int count = pcl::compute3DCentroid(*filtered_cloud_ptr,centroid);
		height = centroid[2];

		ROS_INFO_STREAM("Detected height is: "<<centroid[2]);

		// return z value
		return count != 0;
	}

	void filter_box(const tf::Transform& world_to_sensor_tf,
		const tf::Transform& world_to_box_pick_tf,const Cloud &sensor_cloud,
		Cloud& filtered_cloud)
{
	// creating surface with center at box pick location
	Cloud::Ptr pick_surface_cloud_ptr(new Cloud());

	// adding surface points to cloud
	pick_surface_cloud_ptr->width = 5;
	pick_surface_cloud_ptr->height = 1;
	pick_surface_cloud_ptr->points.resize(5);

	pick_surface_cloud_ptr->points[0].x = 0.5f*box_filter_scale_*box_length_;
	pick_surface_cloud_ptr->points[0].y = 0.5f*box_filter_scale_*box_width_;
	pick_surface_cloud_ptr->points[0].z = 0;

	pick_surface_cloud_ptr->points[1].x = -0.5f*box_filter_scale_*box_length_;
	pick_surface_cloud_ptr->points[1].y = 0.5f*box_filter_scale_*box_width_;
	pick_surface_cloud_ptr->points[1].z = 0;

	pick_surface_cloud_ptr->points[2].x = -0.5f*box_filter_scale_*box_length_;
	pick_surface_cloud_ptr->points[2].y = -0.5f*box_filter_scale_*box_width_;
	pick_surface_cloud_ptr->points[2].z = 0;

	pick_surface_cloud_ptr->points[3].x = 0.5f*box_filter_scale_*box_length_;
	pick_surface_cloud_ptr->points[3].y = -0.5f*box_filter_scale_*box_width_;
	pick_surface_cloud_ptr->points[3].z = 0;

	pick_surface_cloud_ptr->points[4].x = 0.5f*box_filter_scale_*box_length_;
	pick_surface_cloud_ptr->points[4].y = 0.5f*box_filter_scale_*box_width_;
	pick_surface_cloud_ptr->points[4].z = 0;

	ROS_INFO_STREAM("Points in surface: "<<pick_surface_cloud_ptr->points.size());

	// finding angle between world z and box z vectors
	tf::Vector3 z_world_vect(0,0,1);
	tf::Vector3 z_box_vect = world_to_box_pick_tf.getBasis().getColumn(2);
	double angle  = z_world_vect.angle(z_box_vect);
	ROS_INFO_STREAM("Angle between z vectors : "<<angle);

	// transforming cloud to match orientation of box (rectify in the z direction)
	tf::Vector3 axis = z_world_vect.cross(z_box_vect);
	tf::Transform world_to_rectified_box_tf = world_to_box_pick_tf;
	Eigen::Affine3d eigen3d;
	if(std::abs(angle) > angle_tolerance_)
	{
		ROS_INFO_STREAM("Rectifying pick pose z direction");
		world_to_rectified_box_tf.setRotation(world_to_box_pick_tf.getRotation() * tf::Quaternion(axis,-angle)) ;
	}

	tf::transformTFToEigen(world_to_rectified_box_tf,eigen3d);
	pcl::transformPointCloud(*pick_surface_cloud_ptr,*pick_surface_cloud_ptr,Eigen::Affine3f(eigen3d));

	// extracting points in extruded volume
	Cloud::Ptr sensor_cloud_ptr = boost::make_shared<Cloud>(sensor_cloud);
	pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

	ROS_INFO_STREAM("Sensor cloud points: "<<sensor_cloud_ptr->points.size());

	tf::Vector3 viewpoint = world_to_sensor_tf.getOrigin();
	prism.setInputCloud(sensor_cloud_ptr);
	prism.setInputPlanarHull( pick_surface_cloud_ptr);
	prism.setHeightLimits(-10,10);
	//prism.setViewPoint(viewpoint.x(),viewpoint.y(),viewpoint.z());
	prism.segment(*inliers);

	//pcl::copyPointCloud(*sensor_cloud_ptr,indices.indices,filtered_cloud);
	// extracting remaining points
	extract.setInputCloud(sensor_cloud_ptr);
	extract.setIndices(inliers);
	extract.setNegative(true);
	extract.filter(filtered_cloud);

	ROS_INFO_STREAM("Filtered cloud points: "<<filtered_cloud.points.size());

}

protected:

	// members
	std::string ar_frame_id_;
	std::string world_frame_id_;
	float box_width_;
	float box_length_;
	float box_height_;
	sensor_msgs::PointCloud2 filtered_cloud_msg_;
	sensor_msgs::PointCloud2 sensor_cloud_msg_;

	// ros parameter
	double box_filter_scale_;
	double angle_tolerance_;

	// ros service server
	ros::ServiceServer target_detection_server;

	// ros subscriber
	ros::Subscriber point_cloud_subscriber;

	// ros publishers and subscribers
	ros::Publisher filtered_cloud_publisher;

	// transform listener
	TransformListenerPtr transform_listener_ptr;

};
// main program
int main(int argc,char** argv)
{
	ros::init(argc,argv,"target_recognition_node");

	TargetRecognition tg;
	if(tg.init())
	{
		tg.run();
	}

	return 0;
}


//
// Created by System Administrator on 2019-07-29.
//

#include <tins/tins.h>
#include <unordered_map>
#include <unordered_set>
#include <boost/functional/hash.hpp>

#include <maths/stopping_points_t.hpp>
#include <node_t.hpp>

using namespace Tins;

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

    uint8_t max_ttl = 8;

    // Edges
    using edges_t = std::unordered_set<std::pair<uint32_t , uint32_t >, boost::hash<std::pair<uint32_t , uint32_t >>>;

    // In memory structure for the MDA. Tree structure with probabilities of each node to get reached.
    using probability_tree_t = std::vector<std::unordered_map<uint32_t, node_t>>;
    probability_tree_t probability_tree(max_ttl);

    PacketSender sender{NetworkInterface::default_interface()};

    for (uint8_t ttl = 2; ttl < max_ttl; ++ttl){
        // BFS into the tree.
        bool is_ttl_done = false;
        while(!is_ttl_done){
            // Dispatch the probes into different prefixes according to the distribution
            std::vector<IP> probes;

            for (const auto & reply_ip_routing_table : probability_tree[ttl]){

                // Each prefix of the routing table is considered as a possible source of multiple paths.
                // As far as new paths are found within a prefix, we split the prefix in smaller prefixes.
                // The stopping condition occurs when a prefix reach some statistical guarantees?
                const auto & routing_table_distribution = reply_ip_routing_table.second.routing_table_distribution;
                for (const auto & prefix_tracenodes : routing_table_distribution){
                    // Extract number of successors
                    const auto & tracenodes = prefix_tracenodes.second;
                    std::unordered_set<uint32_t> successors;
                    std::transform(tracenodes.begin(), tracenodes.end(), std::inserter(successors, successors.begin()), [](const auto & tracenode){
                        return tracenode.reply_ip;
                    });

                    auto n_successors = successors.size();
                    if (tracenodes.size() < mda_maths::nks95[])

                }

            }

            // Update the graph routing table after receiving the answers.

        }

    }

}


#include <array>
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <cstdlib>
#include <iostream>
#include <random>
#include <set>

using namespace std;

namespace tetris {
const int BOARD_WIDTH = 10;
const int BOARD_HEIGHT = 20;
const int PIECE_SIZE = 4;
const int ROTATIONS = 4;
const int NUM_SHAPES = 8;

// Scores for 0, 1, 2, ..., n line clears with a single placement
const array<int, 5> SCORES = {{0, 40, 100, 300, 1200}};

const array<array<bool, PIECE_SIZE>, PIECE_SIZE> BLANK_SHAPE = {
    {{false, false, false, false},
     {false, false, false, false},
     {false, false, false, false},
     {false, false, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> STRAIGHT_SHAPE = {
    {{false, false, false, false},
     {true, true, true, true},
     {false, false, false, false},
     {false, false, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> L_SHAPE = {
    {{false, false, false, false},
     {false, true, true, false},
     {false, true, false, false},
     {false, true, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> FLIPPED_L_SHAPE = {
    {{false, false, false, false},
     {false, true, true, false},
     {false, false, true, false},
     {false, false, true, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> ZIG_ZAG_SHAPE = {
    {{false, false, true, false},
     {false, true, true, false},
     {false, true, false, false},
     {false, false, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> FLIPPED_ZIG_ZAG_SHAPE = {
    {{false, true, false, false},
     {false, true, true, false},
     {false, false, true, false},
     {false, false, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> SQUARE_SHAPE = {
    {{false, false, false, false},
     {false, true, true, false},
     {false, true, true, false},
     {false, false, false, false}}};
const array<array<bool, PIECE_SIZE>, PIECE_SIZE> T_SHAPE = {
    {{false, false, false, false},
     {true, true, true, false},
     {false, true, false, false},
     {false, false, false, false}}};

const array<array<array<bool, PIECE_SIZE>, PIECE_SIZE>, NUM_SHAPES> SHAPES = {
    BLANK_SHAPE,   STRAIGHT_SHAPE,        L_SHAPE,      FLIPPED_L_SHAPE,
    ZIG_ZAG_SHAPE, FLIPPED_ZIG_ZAG_SHAPE, SQUARE_SHAPE, T_SHAPE};
}

using namespace tetris;

typedef vector<int> VectorInt;
typedef vector<bool> VectorBool;

class Piece {
 private:
  array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = {{false}};
  int row;
  int col;
  int rot;
  int pieceIndex;

 public:
  explicit Piece(array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape)
      : Piece(shape, 0, 3) {}

  explicit Piece(array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape, int row,
                 int col) {
    this->shape = shape;
    this->row = row;
    this->col = col;
    this->rot = 0;
  }

  explicit Piece(int pieceIndex, int row, int col, int rot) {
    this->pieceIndex = pieceIndex;
    this->row = row;
    this->col = col;
    this->rot = rot;
  }

  explicit Piece() {}

  void rotate() {
    for (int r = 0; r < PIECE_SIZE / 2; r++) {
      int rowInverse = PIECE_SIZE - r - 1;
      for (int c = r; c < rowInverse; c++) {
        int colInverse = PIECE_SIZE - c - 1;

        int temp = shape[r][c];
        shape[r][c] = shape[c][rowInverse];

        shape[c][rowInverse] = shape[rowInverse][colInverse];

        shape[rowInverse][colInverse] =
            shape[colInverse][r];

        shape[colInverse][r] = temp;
      }
    }

    rot = (rot + 1) % ROTATIONS;
  }

  array<array<bool, PIECE_SIZE>, PIECE_SIZE> getShape() { return shape; }

  int getRow() { return row; }

  int getCol() { return col; }

  void setRow(int newRow) { row = newRow; }

  void setCol(int newCol) { col = newCol; }

  void setPos(int newRow, int newCol) {
    setRow(newRow);
    setCol(newCol);
  }

  void setRot(int newRot) {
    while (rot != newRot) rotate();
  }
};

class Board {
 private:
  vector<VectorInt> currentMoves;
  Piece* pieces[7] = {};
  bool board[BOARD_HEIGHT][BOARD_WIDTH] = {{false}};
  // bool board[BOARD_HEIGHT][BOARD_WIDTH] = {false};
  int score = 0;
  // Keeps track of visited branches (prevents infinite recursion)
  bool visited[BOARD_HEIGHT + PIECE_SIZE][BOARD_WIDTH + PIECE_SIZE][ROTATIONS] =
      {{false}};
  // Keeps track of which branches are valid: 0 means not yet determined, -1
  // means invalid, 1 means valid
  int memo[BOARD_HEIGHT + PIECE_SIZE][BOARD_WIDTH + PIECE_SIZE][ROTATIONS] = {
      {0}};

  set<array<int, 3>> getMoves(Piece *piece, int row, int col, int rot) {
    resetVisited();
    piece->setRot(rot);

    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();

    set<array<int, 3>> moves;

    for (int r = 0; r < PIECE_SIZE; r++) {
      for (int c = 0; c < PIECE_SIZE; c++) {
        if (shape[r][c]) {
          if (isMoveValid(piece, row - r, col - c, rot)) {
            moves.insert({{row - r, col - c, rot}});
          }

          resetVisited();
          piece->setRot(rot);
        }
      }
    }

    return moves;
  }

  void resetMemo() {
    for (int i = 0; i < BOARD_HEIGHT + PIECE_SIZE; i++) {
      for (int j = 0; j < BOARD_WIDTH + PIECE_SIZE; j++) {
        for (int k = 0; k < ROTATIONS; k++) {
          memo[i][j][k] = 0;
        }
      }
    }
  }

  void resetVisited() {
    for (int i = 0; i < BOARD_HEIGHT + PIECE_SIZE; i++) {
      for (int j = 0; j < BOARD_WIDTH + PIECE_SIZE; j++) {
        for (int k = 0; k < ROTATIONS; k++) {
          visited[i][j][k] = false;
        }
      }
    }
  }

  // Recursively checks if the piece would be able to reach the given position
  // from the starting position (0, 3)
  bool isMoveValid(Piece *piece, int row, int col, int rot) {
    // Already visited this branch, terminate
    if (visited[row + PIECE_SIZE][col + PIECE_SIZE][rot])
      return false;
    else if (memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] < 0)
      return false;
    else if (memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] > 0)
      return true;

    visited[row + PIECE_SIZE][col + PIECE_SIZE][rot] = true;

    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();

    for (int r = 0; r < PIECE_SIZE; r++) {
      for (int c = 0; c < PIECE_SIZE; c++) {
        if (shape[r][c] &&
            (isOutOfBounds(row + r, col + c) || board[row + r][col + c])) {
          memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] = -1;
          return false;
        }
      }
    }

    bool foundFirstBlock = false;
    for (int r = 0; r < PIECE_SIZE; r++) {
        for (int c = 0; c < PIECE_SIZE; c++) {
            if (shape[r][c]) {
                // This is where the piece always starts in Tetris
                if (!foundFirstBlock && row + r == 0 && col + c == 3) {
                    memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] = 1;
                    return true;
                }
                foundFirstBlock = true;
            }
        }
    }

    bool canAccess = isMoveValid(piece, row - 1, col, rot) ||
                     isMoveValid(piece, row, col - 1, rot) ||
                     isMoveValid(piece, row, col + 1, rot);

    if (canAccess) {
      memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] = 1;
      return true;
    }

    for (int n = 1; n < ROTATIONS; n++) {
      int newRot = (rot + n) % ROTATIONS;
      piece->setRot(newRot);
      if (isMoveValid(piece, row, col, newRot)) {
        memo[row + PIECE_SIZE][col + PIECE_SIZE][rot] = 1;
        return true;
      }
    }

    return false;
  }

  void deleteRow(int row) {
    for (int r = row; r > 0; r--) {
      for (int c = 0; c < BOARD_WIDTH; c++) {
        board[r][c] = board[r - 1][c];
      }
    }
  }

  bool isRowCompleted(int row) {
    for (int c = 0; c < BOARD_WIDTH; c++) {
      if (!board[row][c]) return false;
    }

    return true;
  }

  static bool isOutOfBounds(int row, int col) {
    return row < 0 || row >= BOARD_HEIGHT || col < 0 || col >= BOARD_WIDTH;
  }

 public:
  Board() {
    int i = 0;
    pieces[i++] = new Piece(STRAIGHT_SHAPE);
    pieces[i++] = new Piece(L_SHAPE);
    pieces[i++] = new Piece(FLIPPED_L_SHAPE);
    pieces[i++] = new Piece(ZIG_ZAG_SHAPE);
    pieces[i++] = new Piece(FLIPPED_ZIG_ZAG_SHAPE);
    pieces[i++] = new Piece(SQUARE_SHAPE);
    pieces[i++] = new Piece(T_SHAPE);
  }

  // Returns a set of possible moves in format: (row, col, rot)
  set<array<int, 3>> getMoves(Piece *piece) {
    resetMemo();

    set<array<int, 3>> moves;

    for (int r = 0; r < BOARD_HEIGHT; r++) {
      for (int c = 0; c < BOARD_WIDTH; c++) {
        if (!board[r][c] && (r + 1 >= BOARD_HEIGHT || board[r + 1][c])) {
          for (int rot = 0; rot < ROTATIONS; rot++) {
            set<array<int, 3>> partialMoves = getMoves(piece, r, c, rot);
            moves.insert(partialMoves.begin(), partialMoves.end());
          }
        }
      }
    }

    return moves;
  }

  // Returns a set of possible moves in format: (pieceIndex, row, col, rot)
  set<array<int, 4>> getMoves(array<Piece *, 7> pieces) {
    set<array<int, 4>> moves;

    int i = 0;
    for (Piece *piece : pieces) {
      set<array<int, 3>> partialMoves = getMoves(piece);
      for (array<int, 3> move : partialMoves) {
        moves.insert({i, move[0], move[1], move[2]});
      }
      i++;
    }

    return moves;
  }

  void place(Piece *piece) {
    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();
    int row = piece->getRow();
    int col = piece->getCol();

    int rowsCompleted = 0;
    for (int r = 0; r < PIECE_SIZE; r++) {
      for (int c = 0; c < PIECE_SIZE; c++) {
        if (!isOutOfBounds(row + r, col + c))
            board[row + r][col + c] |= shape[r][c];
      }

      if (isRowCompleted(row + r)) {
        deleteRow(row + r);
        rowsCompleted++;
      }
    }

    score += SCORES[rowsCompleted];
  }

  void render(Piece *piece) {
    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();
    int row = piece->getRow();
    int col = piece->getCol();

    for (int r = 0; r < BOARD_HEIGHT; r++) {
      for (int c = 0; c < BOARD_WIDTH; c++) {
        bool renderPiece = r - row >= 0 && r - row < PIECE_SIZE &&
                           c - col >= 0 && c - col < PIECE_SIZE &&
                           shape[r - row][c - col];

        if (renderPiece) {
          cout << "O";
        } else if (board[r][c]) {
          cout << "X";
        } else {
          cout << "-";
        }
      }
      cout << "\n";
    }
    cout << endl;
  }

  // wrap
  vector<VectorInt> getMoves() {
    currentMoves.clear();
    random_device rd;
    mt19937 gen(rd());
    uniform_int_distribution<> distribution(0, 6);
    int randomIndex = distribution(gen);
    auto *piece = pieces[randomIndex];

    // vector<VectorInt> moves;
    set<array<int, 3>> partialMoves = getMoves(piece);

    for (array<int, 3> move : partialMoves) {
      currentMoves.push_back({randomIndex, move[0], move[1], move[2]});
    }

    return currentMoves;
  }

  int getNumberOfMoves() { return currentMoves.size(); }

  // wrap
  void printMoves(const vector<VectorInt> &vvi) {
    for (int i = 0; i < vvi.size(); i++) {
      for (int j = 0; j < vvi[i].size(); j++) {
        cout << vvi[i][j] << " ";
      }
      cout << endl;
    }
  }

  // wrap
  void place(int pieceIndex, int row, int col, int rot) {
    auto *piece = pieces[pieceIndex];
    piece->setRow(row);
    piece->setCol(col);
    piece->setRot(rot);
    place(piece);
  }

  // wrap
  vector<VectorBool> rend(int pieceIndex, int row, int col, int rot) {
    auto *piece = pieces[pieceIndex];
    piece->setRot(rot);
    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();

    vector<VectorBool> newBoard;
    VectorBool vb;

    for (int r = 0; r < BOARD_HEIGHT; r++) {
      for (int c = 0; c < BOARD_WIDTH; c++) {
        bool renderPiece = r - row >= 0 && r - row < PIECE_SIZE &&
                           c - col >= 0 && c - col < PIECE_SIZE &&
                           shape[r - row][c - col];

        vb.push_back(renderPiece || board[r][c]);
      }
      newBoard.push_back(vb);
      vb.clear();
    }

    return newBoard;
  }

  int getValueOfVectorInts(vector<VectorInt> &vvi, int i, int j) {
    return vvi[i][j];
  }

  bool getValueOfVectorBools(vector<VectorBool> &vvb, int i, int j) {
    return vvb[i][j];
  }

  int getScore() { return score; }

  void reset() {
    resetMemo();
    resetVisited();
    score = 0;

    for (int row = 0; row < BOARD_HEIGHT; row++) {
      for (int col = 0; col < BOARD_WIDTH; col++) {
        board[row][col] = false;
      }
    }
  }

  // Returns true if piece can fit on board
  bool isValid(Piece *piece) {
    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape = piece->getShape();
    int row = piece->getRow();
    int col = piece->getCol();

    for (int r = 0; r < PIECE_SIZE; r++) {
      for (int c = 0; c < PIECE_SIZE; c++) {
        if (shape[r][c] &&
            (isOutOfBounds(row + r, col + c) || board[row + r][col + c])) {
          return false;
        }
      }
    }

    return true;
  }

  // Returns true if piece can fit on board
  bool isValid(int pieceIndex, int row, int col, int rot) {
    auto *piece = pieces[pieceIndex];
    piece->setRow(row);
    piece->setCol(col);
    piece->setRot(rot);
    return isValid(piece);
  }

  // wrap
  void printRend(const vector<VectorBool> &vvb) {
    for (int i = 0; i < vvb.size(); i++) {
      for (int j = 0; j < vvb[i].size(); j++) {
        // cout << "vvb[" << i << "][" << j << "] = " << vvb[i][j] << " ";
        cout << vvb[i][j] << " ";
      }
      cout << endl;
    }
  }
};

int main() {
  srand(23477846);

  auto *board = new Board;
  auto *blankPiece = new Piece(BLANK_SHAPE);
  auto *straightPiece = new Piece(STRAIGHT_SHAPE);
  auto *lPiece = new Piece(L_SHAPE);
  auto *flippedLPiece = new Piece(FLIPPED_L_SHAPE);
  auto *zigZagPiece = new Piece(ZIG_ZAG_SHAPE);
  auto *flippedZigZagPiece = new Piece(FLIPPED_ZIG_ZAG_SHAPE);
  auto *squarePiece = new Piece(SQUARE_SHAPE);
  auto *tPiece = new Piece(T_SHAPE);

  array<Piece *, 7> pieces = {
      straightPiece,      lPiece,      flippedLPiece, zigZagPiece,
      flippedZigZagPiece, squarePiece, tPiece};

  board->render(blankPiece);

  for (int n = 0; n < 10; n++) {
    set<array<int, 4>> moves = board->getMoves(pieces);
    if (moves.empty()) break;
    int selectedMove = rand() % moves.size();
    int i = 0;
    for (array<int, 4> move : moves) {
      if (i++ == selectedMove) {
        cout << move[0] << ", " << move[1] << ", " << move[2] << ", " << move[3]
             << "\n";
        pieces[move[0]]->setPos(move[1], move[2]);
        pieces[move[0]]->setRot(move[3]);
        board->place(pieces[move[0]]);
        array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape =
            pieces[move[0]]->getShape();
        for (int r = 0; r < PIECE_SIZE; r++) {
          for (int c = 0; c < PIECE_SIZE; c++) {
            cout << (shape[r][c] ? "X" : "-");
          }
          cout << "\n";
        }
        board->render(pieces[move[0]]);
        break;
      }
    }
  }

  cout << "\n"
       << "\n"
       << "\n"
       << "Next moves:\n";
  set<array<int, 4>> moves = board->getMoves(pieces);
  for (array<int, 4> move : moves) {
    cout << move[0] << ", " << move[1] << ", " << move[2] << ", " << move[3]
         << "\n";
    pieces[move[0]]->setPos(move[1], move[2]);
    pieces[move[0]]->setRot(move[3]);
    array<array<bool, PIECE_SIZE>, PIECE_SIZE> shape =
        straightPiece->getShape();
    for (int r = 0; r < PIECE_SIZE; r++) {
      for (int c = 0; c < PIECE_SIZE; c++) {
        cout << (shape[r][c] ? "X" : "-");
      }
      cout << "\n";
    }
    board->render(pieces[move[0]]);
  }

  return 0;
}

BOOST_PYTHON_MODULE(tetris_boost) {
  using namespace boost::python;

  void (Board::*place)(int pieceIndex, int row, int col, int rot) =
      &Board::place;

  vector<VectorInt> (Board::*getMoves)() = &Board::getMoves;

  bool (Board::*isValid)(int pieceIndex, int row, int col, int rot) =
      &Board::isValid;

  class_<vector<VectorBool>>("vector<VectorBool>")
      .def(vector_indexing_suite<vector<VectorBool>>());

  class_<vector<VectorInt>>("vector<VectorInt>")
      .def(vector_indexing_suite<vector<VectorInt>>());

  class_<Board>("Board")
      .def("place", place)
      .def("getMoves", getMoves)
      .def("rend", &Board::rend)
      .def("printRend", &Board::printRend)
      .def("printMoves", &Board::printMoves)
      .def("getValueOfVectorInts", &Board::getValueOfVectorInts)
      .def("getValueOfVectorBools", &Board::getValueOfVectorBools)
      .def("getScore", &Board::getScore)
      .def("reset", &Board::reset)
      .def("isValid", isValid)
      .def("getNumberOfMoves", &Board::getNumberOfMoves);
}


// Copyright 2009-2010 Green Code LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <cmath>
#include <iostream>

#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include <boost/shared_ptr.hpp>

#include "Algorithm.h"
#include "DuplicatePoints.h"
#include "IPoint.h"
#include "IRasterSurface.h"
#include "ISurfaceInterpolation.h"
#include "IUnclassifiedPoints.h"
#include "LineIndent.h"
#include "PointCanopyHeightFile.h"
#include "StackedPoints.h"
#include "UnclassifiedPoints.h"

namespace mcc
{
  Algorithm::Algorithm(ISurfaceInterpolation & surfaceInterpolation,
                       bool                    writeNongroundPts,
                       bool                    writeRasterSurfaces)
    : surfaceInterpolation_(surfaceInterpolation),
      writeNongroundPts_(writeNongroundPts),
      writeRasterSurfaces_(writeRasterSurfaces)
  {
  }

  //---------------------------------------------------------------------------

  const double Algorithm::tension = 1.5;

  //---------------------------------------------------------------------------

  void Algorithm::classifyPoints(UnclassifiedPoints & points,
                                 double                scaleDomain2Spacing,
                                 double                curvatureThreshold)
  {
    // curvature thresholds for scale domains
    double t[4];  // array[0] not used; allow use of indexes in range of 1 to 3
    t[1] = curvatureThreshold;
    t[2] = t[1] + 0.1;
    t[3] = t[2] + 0.1;

     // cell resolutions for scale domains
    double CR[4];
    CR[2] = scaleDomain2Spacing;
    CR[1] = 0.5 * CR[2];
    CR[3] = 1.5 * CR[2];

    // percentage of points classified during a pass that represents convergence
    // for each scale domain
    double convergencePercent[4];
    convergencePercent[1] = 0.01;    // 1%
    convergencePercent[2] = 0.001;   // 0.1%
    convergencePercent[3] = 0.0001;  // 0.01%

    // points not yet classified

    IUnclassifiedPoints & U = points;
    UnclassifiedPoints splinePoints(points);

    // Locate points that are vertically stacked (at same x,y coordinates), and
    // within each stack, classify all points but the lowest one as non-ground.
    std::cout << "Searching for points with the same x,y coordinates..." << std::endl;
    std::vector<IPoint *> unclassifiedDuplicates;
    StackedPoints::classifyPointsAtSameXY(splinePoints, unclassifiedDuplicates);
    int nClassified = splinePoints.removeClassified();
    std::cout << "  " << nClassified << " points classified as non-ground" << std::endl;

    DuplicatePoints duplicatePoints(unclassifiedDuplicates);
    std::string pluralEnding = (duplicatePoints.setCount() == 1) ? "" : "s";
    std::cout << "Identified " << duplicatePoints.setCount() << " set" << pluralEnding << " of unclassified duplicate points" << std::endl;
    int nDuplicatesPutAside = duplicatePoints.putAsideAllButOnePointPerSet();
    nDuplicatesPutAside = splinePoints.removeClassified();

    LineIndent indent("  ");

    const int maxPasses = 100;  // max # of passes per scale domain
    int nDigitsForPass = (int) std::log10((double) maxPasses) + 1;
    std::string passFormat = boost::str(boost::format("0%d") % nDigitsForPass);

    for (int SD = 1; SD <= 3; SD++) {
      std::cout << "Scale domain: " << SD << " (cell resolution: " << CR[SD] << ")" << std::endl;
      int nPoints;
      int nClassified;
      double percentClassified;
      int pass = 0;
      do {
        pass++;
        if (pass > maxPasses) {
          std::cout << "Maximum # of passes reached for scale domain " << SD << std::endl;
          break;
        }
        std::cout << "SD " << SD << " - Pass " << pass << std::endl
                  << indent << "Interpolating " << splinePoints.count() << " points:" << std::endl;
        boost::shared_ptr<IRasterSurface> rasterSurface = surfaceInterpolation_(splinePoints, CR[SD], tension);

        std::cout << indent << "Averaging raster surface..." << std::endl;
        rasterSurface->average(3);  // kernel = 3x3

        if (writeRasterSurfaces_) {
          boost::format rasterNameFmt("surface_sd%d_p%|" + passFormat + "|.asc");
          std::string rasterName = boost::str(rasterNameFmt % SD % pass);
          std::cout << indent << "Writing raster surface to file \"" << rasterName  << "\"..." << std::endl;
          rasterSurface->writeAsciiGrid(rasterName);
        }

        std::cout << indent << "Identifying non-ground points:" << std::endl;
        PointCanopyHeightFile nongroundPtsFile;
        if (writeNongroundPts_) {
          boost::format fileName("nonground_sd%d_p%|" + passFormat + "|.csv");
          nongroundPtsFile.open(boost::str(fileName % SD % pass));
        }
        nPoints = U.count();
        std::cout << "Number of Points: " << nPoints << std::endl;
        BOOST_FOREACH( IPoint & point, U) {
          Coordinate surfaceHeight = (*rasterSurface)(point.x(), point.y());
          if (point.z() > surfaceHeight + t[SD]) {
            point.classifyAs(NonGround);
            if (nongroundPtsFile) {
              Coordinate canopyHeight = point.z() - surfaceHeight;
              nongroundPtsFile.writeRow(point, canopyHeight);
            }
          }
        }

        nClassified = U.removeClassified();
        percentClassified = nClassified / double(nPoints);
        boost::format percentFormat("%|.2|%%");
        std::string percentClassifiedStr = boost::str(percentFormat % (percentClassified * 100));
        std::cout << indent << "  " << nClassified << " points (" << percentClassifiedStr << ") classified as non-ground" << std::endl;
        if (nongroundPtsFile) {
          nongroundPtsFile.close();
          std::cout << indent << "  Wrote points to file \"" << nongroundPtsFile.path() << "\"" << std::endl;
        }
      } while ((percentClassified >= convergencePercent[SD]) && (U.count() > 0)); // until (n_C < convergence_% * n) or no more points

      if (U.count() == 0)
        break;
    }

    // Classify all the remaining points as ground
    std::cout << "Classifying " << U.count() << " points as ground..." << std::endl;
    BOOST_FOREACH( IPoint & point, U) {
      point.classifyAs(Ground);
    }

    pluralEnding = (nDuplicatesPutAside == 1) ? "" : "s";
    std::cout << "Copying classifications to " << nDuplicatesPutAside << " duplicate point" << pluralEnding << "..." << std::endl;
    duplicatePoints.copyClassificationAmongPointsInSet();
  }

  // Additions by GEH:

  void Algorithm::labelPointsUsingPass(UnclassifiedPoints & points,
                                 double                scaleDomainSpacing)
  {

    // points not yet classified

    IUnclassifiedPoints & U = points;
    UnclassifiedPoints splinePoints(points);

    // Locate points that are vertically stacked (at same x,y coordinates), and
    // within each stack, classify all points but the lowest one as non-ground.
    std::cout << "Searching for points with the same x,y coordinates..." << std::endl;
    std::vector<IPoint *> unclassifiedDuplicates;
    StackedPoints::classifyPointsAtSameXY(splinePoints, unclassifiedDuplicates);
    int nClassified = splinePoints.removeClassified();
    std::cout << "  " << nClassified << " points classified as non-ground" << std::endl;

    DuplicatePoints duplicatePoints(unclassifiedDuplicates);
    std::string pluralEnding = (duplicatePoints.setCount() == 1) ? "" : "s";
    std::cout << "Identified " << duplicatePoints.setCount() << " set" << pluralEnding << " of unclassified duplicate points" << std::endl;
    int nDuplicatesPutAside = duplicatePoints.putAsideAllButOnePointPerSet();
    nDuplicatesPutAside = splinePoints.removeClassified();

    LineIndent indent("  ");

    std::cout << "Scale domain: " << scaleDomainSpacing << std::endl;

    std::cout << "Interpolating " << splinePoints.count() << " points:" << std::endl;
    boost::shared_ptr<IRasterSurface> rasterSurface = surfaceInterpolation_(splinePoints, scaleDomainSpacing, tension);

    std::cout << indent << "Averaging raster surface..." << std::endl;
    rasterSurface->average(3);  // kernel = 3x3

    if (writeRasterSurfaces_) {
      boost::format rasterNameFmt("surface_sd%d_p%.asc");
      std::string rasterName = boost::str(rasterNameFmt % scaleDomainSpacing);
      std::cout << indent << "Writing raster surface to file \"" << rasterName  << "\"..." << std::endl;
      rasterSurface->writeAsciiGrid(rasterName);
    }

    std::cout << indent << "Identifying non-ground points:" << std::endl;
    PointCanopyHeightFile nongroundPtsFile;

    BOOST_FOREACH( IPoint & point, U) {
      Coordinate surfaceHeight = (*rasterSurface)(point.x(), point.y());
      point.setH(point.z() - surfaceHeight);
      point.classifyAs(Ground);
    }

  }

}


// Copyright (C) 2015, Pawel Tomulik <ptomulik@meil.pw.edu.pl>
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#define BOOST_TEST_MODULE test_txpl_vm_eval_binary_minus

#include <txpl/test_config.hpp>
#include <boost/test/unit_test.hpp>

#ifndef TXPL_TEST_SKIP_VM_EVAL_BINARY_MINUS

#include <txpl/vm/eval_binary_op.hpp>
#include <txpl/vm/basic_types.hpp>
#include <txpl/vm/value.hpp>
#include <boost/variant/apply_visitor.hpp>
#include <boost/variant/get.hpp>

using namespace txpl::vm;
typedef basic_types<>::char_type   char_type;
typedef basic_types<>::int_type    int_type;
typedef basic_types<>::bool_type   bool_type;
typedef basic_types<>::real_type   real_type;
typedef basic_types<>::string_type string_type;
typedef basic_types<>::regex_type  regex_type;
typedef basic_types<>::blank_type  blank_type;
typedef array<value<> >            array_type;
typedef object<value<> >           object_type;

BOOST_AUTO_TEST_CASE(char__minus__char)
{
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = char_type{'\4'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\2'} - char_type{'\4'}));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = int_type{4};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\2'} - int_type{4}));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'\3'};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (char_type{'\3'} - bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'\2'};
    const value<> v2 = real_type{2.1};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (char_type{'\2'} - real_type{2.1}));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(char__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = char_type{'a'};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(int__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{2};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{2} - char_type{'\3'}));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{2};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{2} - int_type{3}));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{3};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (int_type{3} - bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{12};
    const value<> v2 = real_type{3.21};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (int_type{12} - real_type{3.21}));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(int__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = int_type{0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(bool__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} - char_type{'\3'}));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} - int_type{3}));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    int_type x = int_type{654};
    BOOST_CHECK_NO_THROW(x = boost::get<int_type>(r));
    BOOST_CHECK(x == (bool_type{true} - bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{true};
    const value<> v2 = real_type{2.12};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (bool_type{true} - real_type{2.12}));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(bool__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = bool_type{false};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(real__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = char_type{'\3'};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (real_type{1.2} - char_type{'\3'}));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = int_type{3};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (real_type{1.2} - int_type{3}));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = bool_type{true};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (real_type{1.2} - bool_type{true}));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{1.2};
    const value<> v2 = real_type{3.4};
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    real_type x = real_type{.654};
    BOOST_CHECK_NO_THROW(x = boost::get<real_type>(r));
    BOOST_CHECK(x == (real_type{1.2} - real_type{3.4}));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(real__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = real_type{0.0};
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(string__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = char_type{'a'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type("asd");
    const value<> v2 = string_type{"qwer"};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(string__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = string_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(regex__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(regex__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = regex_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(array__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<array_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(array__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = array_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

BOOST_AUTO_TEST_CASE(object__minus__char)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = char_type{'\0'};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__int)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = int_type{0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__bool)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = bool_type{false};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__real)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = real_type{0.0};
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__string)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = string_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__regex)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = regex_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__array)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = array_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}
BOOST_AUTO_TEST_CASE(object__minus__object)
{
  using namespace txpl::vm;
  value<> r;
  auto op = eval_binary_op<op_t::minus_>(r);
  {
    const value<> v1 = object_type();
    const value<> v2 = object_type();
    r = blank_type();
    BOOST_CHECK(!boost::apply_visitor(op, v1, v2));
    BOOST_CHECK_NO_THROW(boost::get<blank_type>(r));
  }
}

#else
BOOST_AUTO_TEST_CASE(dummy)
{
  BOOST_CHECK(true);
}
#endif


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

  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_FUNCTION_SCALAR_SINCOSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_SINCOSD_HPP_INCLUDED

#include <boost/simd/function/definition/sincosd.hpp>
#include <boost/simd/arch/common/generic/function/sincosd.hpp>
#include <boost/simd/arch/common/scalar/function/sincosd.hpp>

#endif