// Copyright 2020 Tier IV, Inc.
//
// 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 <scene_module/crosswalk/util.hpp>
#include <utilization/util.hpp>

#include <autoware_perception_msgs/msg/dynamic_object_array.hpp>

#include <boost/assert.hpp>
#include <boost/assign/list_of.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <algorithm>
#include <cmath>
#include <memory>
#include <string>
#include <vector>

#define EIGEN_MPL2_ONLY
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <lanelet2_extension/regulatory_elements/road_marking.hpp>
#include <lanelet2_extension/utility/query.hpp>
#include <lanelet2_extension/utility/utilities.hpp>

#include <lanelet2_core/primitives/BasicRegulatoryElements.h>

namespace behavior_velocity_planner
{
namespace bg = boost::geometry;
using Point = bg::model::d2::point_xy<double>;
using Polygon = bg::model::polygon<Point>;
using Line = bg::model::linestring<Point>;

bool getBackwardPointFromBasePoint(
  const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2,
  const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point)
{
  Eigen::Vector2d line_vec = line_point2 - line_point1;
  Eigen::Vector2d backward_vec = backward_length * line_vec.normalized();
  output_point = base_point + backward_vec;
  return true;
}

bool insertTargetVelocityPoint(
  const autoware_planning_msgs::msg::PathWithLaneId & input, const Polygon & polygon,
  const double & margin, const double & velocity, const PlannerData & planner_data,
  autoware_planning_msgs::msg::PathWithLaneId & output, DebugData & debug_data,
  boost::optional<int> & first_stop_path_point_index)
{
  output = input;
  for (size_t i = 0; i < output.points.size() - 1; ++i) {
    const auto p0 = output.points.at(i).point.pose.position;
    const auto p1 = output.points.at(i + 1).point.pose.position;
    const Line line{{p0.x, p0.y}, {p1.x, p1.y}};
    std::vector<Point> collision_points;
    bg::intersection(polygon, line, collision_points);

    if (collision_points.empty()) {
      continue;
    }
    // -- debug code --
    for (const auto & cp : collision_points) {
      Eigen::Vector3d point3d(cp.x(), cp.y(), planner_data.current_pose.pose.position.z);
      debug_data.collision_points.push_back(point3d);
    }
    std::vector<Eigen::Vector3d> line3d;
    line3d.emplace_back(p0.x, p0.y, p0.z);
    line3d.emplace_back(p1.x, p1.y, p1.z);
    debug_data.collision_lines.push_back(line3d);
    // ----------------

    // check nearest collision point
    Point nearest_collision_point{};
    double min_dist = 0.0;
    for (size_t j = 0; j < collision_points.size(); ++j) {
      double dist = bg::distance(Point(p0.x, p0.y), collision_points.at(j));
      if (j == 0 || dist < min_dist) {
        min_dist = dist;
        nearest_collision_point = collision_points.at(j);
        debug_data.nearest_collision_point =
          planning_utils::toRosPoint(collision_points.at(j), p0.z);
      }
    }

    // search target point index
    size_t insert_target_point_idx = 0;
    const double base_link2front = planner_data.vehicle_info_.max_longitudinal_offset_m;
    double length_sum = 0;

    const double target_length = margin + base_link2front;
    Eigen::Vector2d point1, point2;
    point1 << nearest_collision_point.x(), nearest_collision_point.y();
    point2 << p0.x, p0.y;
    length_sum += (point2 - point1).norm();
    for (size_t j = i; 0 < j; --j) {
      if (target_length < length_sum) {
        insert_target_point_idx = j + 1;
        break;
      }
      const auto pj1 = output.points.at(j).point.pose.position;
      const auto pj2 = output.points.at(j - 1).point.pose.position;
      point1 << pj1.x, pj1.y;
      point2 << pj2.x, pj2.y;
      length_sum += (point2 - point1).norm();
    }

    // create target point
    Eigen::Vector2d target_point;
    autoware_planning_msgs::msg::PathPointWithLaneId target_point_with_lane_id;
    getBackwardPointFromBasePoint(point2, point1, point2, length_sum - target_length, target_point);
    const int target_velocity_point_idx =
      std::max(static_cast<int>(insert_target_point_idx) - 1, 0);
    target_point_with_lane_id = output.points.at(target_velocity_point_idx);
    target_point_with_lane_id.point.pose.position.x = target_point.x();
    target_point_with_lane_id.point.pose.position.y = target_point.y();
    if (insert_target_point_idx > 0) {
      // calculate z-position of the target point (Internal division point of point1/point2)
      // if insert_target_point_idx is zero, use z-position of target_velocity_point_idx
      const double internal_div_ratio =
        (point1 - target_point).norm() /
        ((point1 - target_point).norm() + (point2 - target_point).norm());
      target_point_with_lane_id.point.pose.position.z =
        output.points.at(insert_target_point_idx).point.pose.position.z * (1 - internal_div_ratio) +
        output.points.at(insert_target_point_idx - 1).point.pose.position.z * internal_div_ratio;
    }

    if ((point1 - point2).norm() > 1.0E-3) {
      const double yaw = std::atan2(point1.y() - point2.y(), point1.x() - point2.x());
      target_point_with_lane_id.point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
    }
    target_point_with_lane_id.point.twist.linear.x = velocity;
    if (velocity == 0.0 && target_velocity_point_idx < first_stop_path_point_index) {
      first_stop_path_point_index = target_velocity_point_idx;
      // -- debug code --
      debug_data.first_stop_pose = target_point_with_lane_id.point.pose;
      // ----------------
    }
    // -- debug code --
    if (velocity == 0.0) {
      debug_data.stop_poses.push_back(target_point_with_lane_id.point.pose);
    } else {
      debug_data.slow_poses.push_back(target_point_with_lane_id.point.pose);
    }
    // ----------------

    // insert target point
    output.points.insert(
      output.points.begin() + insert_target_point_idx, target_point_with_lane_id);

    // insert 0 velocity after target point
    for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) {
      output.points.at(j).point.twist.linear.x =
        std::min(velocity, output.points.at(j).point.twist.linear.x);
    }
    return true;
  }
  return false;
}

lanelet::Optional<lanelet::ConstLineString3d> getStopLineFromMap(
  const int lane_id, const std::shared_ptr<const PlannerData> & planner_data,
  const std::string & attribute_name)
{
  lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id);
  const auto road_markings = lanelet.regulatoryElementsAs<lanelet::autoware::RoadMarking>();
  lanelet::ConstLineStrings3d stop_line;
  for (const auto & road_marking : road_markings) {
    // TODO(someone): Create regulatory element for crosswalk
    const std::string type =
      road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none");
    const int target_id = road_marking->roadMarking().attributeOr(attribute_name, 0);
    if (type == lanelet::AttributeValueString::StopLine && target_id == lane_id) {
      stop_line.push_back(road_marking->roadMarking());
      break;  // only one stop_line exists.
    }
  }
  if (stop_line.empty()) {
    return {};
  }

  return stop_line.front();
}

bool insertTargetVelocityPoint(
  const autoware_planning_msgs::msg::PathWithLaneId & input,
  const lanelet::ConstLineString3d & stop_line, const double & margin, const double & velocity,
  const PlannerData & planner_data, autoware_planning_msgs::msg::PathWithLaneId & output,
  DebugData & debug_data, boost::optional<int> & first_stop_path_point_index)
{
  using lanelet::utils::to2D;
  using lanelet::utils::toHybrid;
  output = input;
  for (size_t i = 0; i < output.points.size() - 1; ++i) {
    const auto p0 = output.points.at(i).point.pose.position;
    const auto p1 = output.points.at(i + 1).point.pose.position;
    const Line line{{p0.x, p0.y}, {p1.x, p1.y}};
    std::vector<Point> collision_points;
    bg::intersection(toHybrid(to2D(stop_line)), line, collision_points);

    if (collision_points.empty()) {
      continue;
    }
    // -- debug code --
    for (const auto & cp : collision_points) {
      Eigen::Vector3d point3d(cp.x(), cp.y(), planner_data.current_pose.pose.position.z);
      debug_data.collision_points.push_back(point3d);
    }
    std::vector<Eigen::Vector3d> line3d;
    line3d.emplace_back(p0.x, p0.y, p0.z);
    line3d.emplace_back(p1.x, p1.y, p1.z);
    debug_data.collision_lines.push_back(line3d);
    // ----------------

    // check nearest collision point
    Point nearest_collision_point{};
    double min_dist = 0.0;
    for (size_t j = 0; j < collision_points.size(); ++j) {
      double dist = bg::distance(Point(p0.x, p0.y), collision_points.at(j));
      if (j == 0 || dist < min_dist) {
        min_dist = dist;
        nearest_collision_point = collision_points.at(j);
        debug_data.nearest_collision_point =
          planning_utils::toRosPoint(collision_points.at(j), p0.z);
      }
    }

    // search target point index
    size_t insert_target_point_idx = 0;
    const double base_link2front = planner_data.vehicle_info_.max_longitudinal_offset_m;
    double length_sum = 0;

    const double target_length = margin + base_link2front;
    Eigen::Vector2d point1, point2;
    point1 << nearest_collision_point.x(), nearest_collision_point.y();
    point2 << p0.x, p0.y;
    length_sum += (point2 - point1).norm();
    for (size_t j = i; 0 < j; --j) {
      if (target_length < length_sum) {
        insert_target_point_idx = j + 1;
        break;
      }
      const auto pj1 = output.points.at(j).point.pose.position;
      const auto pj2 = output.points.at(j - 1).point.pose.position;
      point1 << pj1.x, pj1.y;
      point2 << pj2.x, pj2.y;
      length_sum += (point2 - point1).norm();
    }

    // create target point
    Eigen::Vector2d target_point;
    autoware_planning_msgs::msg::PathPointWithLaneId target_point_with_lane_id;
    getBackwardPointFromBasePoint(point2, point1, point2, length_sum - target_length, target_point);
    const int target_velocity_point_idx =
      std::max(static_cast<int>(insert_target_point_idx) - 1, 0);
    target_point_with_lane_id = output.points.at(target_velocity_point_idx);
    target_point_with_lane_id.point.pose.position.x = target_point.x();
    target_point_with_lane_id.point.pose.position.y = target_point.y();
    if (insert_target_point_idx > 0) {
      // calculate z-position of the target point (Internal division point of point1/point2)
      // if insert_target_point_idx is zero, use z-position of target_velocity_point_idx
      const double internal_div_ratio =
        (point1 - target_point).norm() /
        ((point1 - target_point).norm() + (point2 - target_point).norm());
      target_point_with_lane_id.point.pose.position.z =
        output.points.at(insert_target_point_idx).point.pose.position.z * (1 - internal_div_ratio) +
        output.points.at(insert_target_point_idx - 1).point.pose.position.z * internal_div_ratio;
    }

    if ((point1 - point2).norm() > 1.0E-3) {
      const double yaw = std::atan2(point1.y() - point2.y(), point1.x() - point2.x());
      target_point_with_lane_id.point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw);
    }
    target_point_with_lane_id.point.twist.linear.x = velocity;
    if (velocity == 0.0 && target_velocity_point_idx < first_stop_path_point_index) {
      first_stop_path_point_index = target_velocity_point_idx;
      // -- debug code --
      debug_data.first_stop_pose = target_point_with_lane_id.point.pose;
      // ----------------
    }
    // -- debug code --
    if (velocity == 0.0) {
      debug_data.stop_poses.push_back(target_point_with_lane_id.point.pose);
    } else {
      debug_data.slow_poses.push_back(target_point_with_lane_id.point.pose);
    }
    // ----------------

    // insert target point
    output.points.insert(
      output.points.begin() + insert_target_point_idx, target_point_with_lane_id);

    // insert 0 velocity after target point
    for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) {
      output.points.at(j).point.twist.linear.x =
        std::min(velocity, output.points.at(j).point.twist.linear.x);
    }
    return true;
  }
  return false;
}

bool isClockWise(const Polygon & polygon)
{
  const auto n = polygon.outer().size();

  const double x_offset = polygon.outer().at(0).x();
  const double y_offset = polygon.outer().at(0).y();
  double sum = 0.0;
  for (std::size_t i = 0; i < polygon.outer().size(); ++i) {
    sum +=
      (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) -
      (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset);
  }
  return sum < 0.0;
}

Polygon inverseClockWise(const Polygon & polygon)
{
  const auto & poly = polygon.outer();
  Polygon inverted_polygon;
  inverted_polygon.outer().reserve(poly.size());
  std::reverse_copy(poly.begin(), poly.end(), std::back_inserter(inverted_polygon.outer()));
  return inverted_polygon;
}
}  // namespace behavior_velocity_planner


#include <boost/program_options.hpp>
#include <cstdlib>
#include <thread>
#include <chrono>

#include "EncryptedCoordinator.hpp"
#include "LatticeEncryption.hpp"
#include "Ligero.hpp"

#include "Transport.hpp"
#include "ZkArgument.hpp"
#include "GatheringCoordinator.hpp"
#include "SecretSharingNTT.hpp"

#define NDEBUG

namespace po = boost::program_options;

//==============================================================================
/** A small helper function for wrapping the construction of a client and
 *  contributing to the computation.
 *
 *  This is primarily to ensure the socket construction happens on the same thread
 *  as the socket usage.
 */

using namespace ligero;
using namespace ligero::lattice;
using namespace std::chrono_literals;

// Parameters RSA Ceremony
// ===========================

constexpr auto degree = 1 << 16;
constexpr auto p = 9;
constexpr auto q = 21;
constexpr auto sigma = 8;
constexpr auto lambda = 128;
constexpr auto tau_limit_bit = 175;
constexpr auto tau = 1000;
constexpr auto numCandidates = 2048;

// throughput parties
constexpr auto wait_timeout = 300 * 1000; /* wait 5 min when start */
constexpr auto timeout = 25 * 1000; /* a little longer than test */
constexpr auto data_size = 32 * 1024; /* 32 KB */
constexpr auto nb_max_send = 1024;
constexpr auto parties = 1;

// throughtput coordinator
constexpr auto pbs = 1000;
constexpr auto duration = 20s;
constexpr auto wait_time = 120s;
constexpr auto cleanup_time = 5s;
constexpr auto throughput_cutoff = 100 * 1024; /* 200 KB/s */

// Number of restart
int maxRestart = 10;

// Parameters Zero-Knowledge
// ============================

Statements localStatement = Statements::RSACeremony;

// Settings Zero-Knowledge
RegularInteger p_zk(10514644122014433281ULL);
typedef Fpp_Fixed<p_zk> FieldT;

zksnark::InterleavedRSCode irs;                 // Reed-Solomon Interleaved Code
ligero::MTParameters<FieldT> mtParams;          // Merkle Tree Parameters
constexpr size_t l = 4096;                                // Bloc size
constexpr size_t k = 8192;                                // Degree
constexpr size_t n = 16384;                               // # of Shares

constexpr size_t t = 256;                                 // Columns to Open Up (By default half the columns)
constexpr auto ptNumberEvaluation = 3;

// Random Generator
const hash::HashIntegerSeedGeneratorF<FieldT>& randomGenerator = hash::blake2bIntegerSeedGenerator<FieldT>;
const hash::HashStringSeedGeneratorF& stringGenerator = hash::blake2bStringHash;

// Threading
constexpr size_t maxNumberThreads = 90; // [m5.metal has 96 threads over 48 cores]

expected<Unit> coordinateHelper(EncryptedCoordinator<uint64_t, degree, p, q>& coordinator, ZeroMQCoordinatorTransport& transport, std::string ipAddr, bool passive) {
    // base case
    if (maxRestart < 0) {
        return Error::TOO_MANY_RESTART;
    }

    auto result = coordinator.host(transport, &maxRestart);
    if (hasError(result)) {
        LOG(INFO) << showError(getError(result));
        exit(EXIT_FAILURE);
    }

    if (!passive) {
        printAtXY(1,3,std::string("Active Security: Proof Verification Started."));
        auto response = GatheringCoordinator::gatherAndVerifyArgumentsOfKnowledge<FieldT, uint64_t, degree, p, q>(coordinator, transport, std::string("Prod"), "ligero-simulations/Proofs", 21, false, ipAddr.c_str(), parties);

        transport.myTimers.end(2,"1.a. Overall speed", transport.communicationCost, true);

        // Verify failed and need to restart the protocol
        if (hasError(response)) {
            if (getError(response) == Error::RESTART) {
                transport.myTimers.reset();
                coordinator.clearPublicData();
                printAtXY(1,1,std::string("Restarting RSA Ceremony."));
                return coordinateHelper(coordinator, transport, ipAddr, passive);
            }
            else {
                return getError(response);
            }
        }
        LOG(INFO) << "Verified all proofs; ceremony successful.";
        printAtXY(1,7+firstline,std::string("Verified all proofs; ceremony successful."));

        return Unit{};
    }
    return Unit{};
}

/** An equivalent helper function for the coordinator side of things. */
void coordinate(int parties, std::string ipAddr, int numCandidates, bool passive, ProtocolMode protocolMode) {

	try {
	    ProtocolConfig<uint64_t> config(parties, p, q, degree, sigma, lambda, tau_limit_bit, pbs, protocolMode);

        system("clear");

        // Generate all proofs
        // =====================================================================================================================
        auto coordinator = EncryptedCoordinator<uint64_t, degree, p, q>(config);
        ZeroMQCoordinatorTransport transport(std::string("tcp://") + ipAddr + std::string(":5555"), config.numParties());
        printAtXY(1,1,std::string("Hosting RSA Ceremony."));

        auto throughput = coordinator.hostThroughputTest(transport, {duration, wait_time, cleanup_time, data_size}, throughput_cutoff);
        if (hasError(throughput)) {
            if (getError(throughput) == Error::TOO_FEW_PARTIES) {
                LOG(INFO) << "Too few parties survive throughput test. Aborting";
            }
            else {
                LOG(INFO) << showError(getError(throughput));
            }
            exit(EXIT_FAILURE);
        }

        auto result = coordinateHelper(coordinator, transport, ipAddr, passive);
        if (hasError(result)) {
            LOG(INFO) << "Caught an error trying to coordinate... " << showError(getError(result));
            exit(EXIT_FAILURE);
        }
    }
    catch (zmq::error_t& e) {
        LOG(ERROR) << "Caught an error trying to coordinate... " << e.what();
    }
}

//==============================================================================
int main(int argc, char** argv)
{
    std::string ipAddr("127.0.0.1");
    std::string logfile("default");

    int numBits         = 100;
    int numParties      = 2;
    int numCandidates   = 2048;
    double chiStd       = 3.2;

    bool passive = false;

    // Set Zero-Knowledge Parameters
    irs.k                       = k;       // Degree of the Polynomial
    irs.n                       = n;       // Number of Shares

     mtParams.digestLength       = 32;
     mtParams.zkByteSize         = 8;
     mtParams.hashInnerNodeF     = hash::blake2bTwoToOneHash;
     mtParams.hashLeafContentF   = hash::blake2bFieldElementHash<FieldT>;
     mtParams.hashZKRandomnessF  = hash::blake2bZKElementHash;
     mtParams.leavesNumber       = irs.n;

    try {
        po::options_description desc("Allowed options");
        desc.add_options()
            ("help", "produce help message")
            ("version", "show version")
            ("parties", po::value<int>(), "set the number of participating clients")
            ("logfile", po::value<std::string>(), "sets the logfile name")
            ("ip", po::value<std::string>(), "set the ip address for the coordinator")
            ("bitsize", po::value<int>(), "sets the bit size")
            ("chiStd", po::value<double>(), "sets the std distribution randomness")
            ("numcandidates", po::value<int>(), "sets the number of shares attempted per participant")
            ("mode", po::value<std::string>(), "sets the protocol mode from normal, replay, record, normal by default")
            ("passive", "do not include a zero-knowledge argument");

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

        if (vm.count("help")) {
            LOG(TRACE) << desc << "\n";
            return 0;
        }

        if (vm.count("version")) {
            std::cout << "coordinator version " << std::hex << PROTOCOL_VERSION_NUMBER << std::endl;
            return 0;
        }

        if (vm.count("passive")) {
            passive = true;
        }

        if (vm.count("parties")) {
            numParties = vm["parties"].as<int>();
            LOG(TRACE) << "nb parties = " << numParties;
        }

        if (vm.count("logfile")) {
            logfile = vm["logfile"].as<std::string>();
            LOG(TRACE) << "logfile = " << logfile;
        }

        ProtocolMode protocolMode = ProtocolMode::NORMAL;
        if (vm.count("mode")) {
            std::string mode = vm["mode"].as<std::string>();
            if (mode == "replay") {
                protocolMode = ProtocolMode::REPLAY;
            } else if (mode == "record") {
                protocolMode = ProtocolMode::RECORD;
            } else {
                protocolMode = ProtocolMode::NORMAL;
            }
            LOG(TRACE) << "mode = " << mode;
        }

        if (vm.count("ip")) {
            ipAddr = vm["ip"].as<std::string>();
            LOG(TRACE) << "Coordinator resides at " << ipAddr;
        }

       if (vm.count("numcandidates")) {
            numCandidates = (int)vm["numcandidates"].as<int>();
            LOG(TRACE) << "numCandidates = " << numCandidates;
        }

       if (vm.count("chiStd")) {
            chiStd = (double)vm["chiStd"].as<double>();
            LOG(TRACE) << "chiStd = " << chiStd;
            std::cout << chiStd;
        }

        const int packingFactor = numCandidates;

        // Configure easylogging
        configureEasyLogging(logfile.c_str());

        LOG(TRACE) << std::string(120, '=');
        LOG(TRACE) << "Coordinator started.";

        coordinate(numParties, ipAddr, numCandidates, passive, protocolMode);
    }
    catch(std::exception& e) {
        LOG(FATAL) << "Error: " << e.what();
    }
    catch(...) {
        LOG(FATAL) << "Caught an exception of unknown type.";
    }

    return 0;
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef polyclone_caller_hpp
#define polyclone_caller_hpp

#include <vector>
#include <string>
#include <memory>
#include <functional>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "basics/phred.hpp"
#include "core/types/haplotype.hpp"
#include "core/types/genotype.hpp"
#include "core/models/mutation/coalescent_model.hpp"
#include "core/models/genotype/genotype_prior_model.hpp"
#include "core/models/genotype/individual_model.hpp"
#include "core/models/genotype/subclone_model.hpp"
#include "utils/maths.hpp"
#include "caller.hpp"

namespace octopus {

class GenomicRegion;
class ReadPipe;
class Variant;
class HaplotypeLikelihoodArray;
class VariantCall;

class PolycloneCaller : public Caller
{
public:
    using Caller::CallTypeSet;

    struct Parameters
    {
        boost::optional<CoalescentModel::Parameters> prior_model_params;
        Phred<double> min_variant_posterior, min_refcall_posterior;
        bool deduplicate_haplotypes_with_germline_model = false;
        unsigned max_clones = 3;
        boost::optional<std::size_t> max_genotypes = 10'000;
        boost::optional<unsigned> max_vb_seeds = boost::none; // Use default if none
        std::function<double(unsigned)> clonality_prior = [] (unsigned clonality) { return maths::geometric_pdf(clonality, 0.99); };
        double clone_mixture_prior_concentration = 1;
    };

    PolycloneCaller() = delete;

    PolycloneCaller(Caller::Components&& components,
                    Caller::Parameters general_parameters,
                    Parameters specific_parameters);

    PolycloneCaller(const PolycloneCaller&)            = delete;
    PolycloneCaller& operator=(const PolycloneCaller&) = delete;
    PolycloneCaller(PolycloneCaller&&)                 = delete;
    PolycloneCaller& operator=(PolycloneCaller&&)      = delete;

    ~PolycloneCaller() = default;

private:
    class Latents;
    friend Latents;

    Parameters parameters_;

    struct ModelProbabilities
    {
        double clonal, subclonal;
    };

    using IndexedHaplotypeBlock = MappableBlock<IndexedHaplotype<>>;
    using GenotypeBlock = MappableBlock<Genotype<IndexedHaplotype<>>>;

    std::string do_name() const override;
    CallTypeSet do_call_types() const override;
    unsigned do_min_callable_ploidy() const override;
    unsigned do_max_callable_ploidy() const override;

    std::size_t do_remove_duplicates(HaplotypeBlock& haplotypes) const override;

    std::unique_ptr<Caller::Latents>
    infer_latents(const HaplotypeBlock& haplotypes,
                  const HaplotypeLikelihoodArray& haplotype_likelihoods) const override;

    boost::optional<double>
    calculate_model_posterior(const HaplotypeBlock& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Caller::Latents& latents) const override;

    boost::optional<double>
    calculate_model_posterior(const HaplotypeBlock& haplotypes,
                              const HaplotypeLikelihoodArray& haplotype_likelihoods,
                              const Latents& latents) const;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Caller::Latents& latents) const override;

    std::vector<std::unique_ptr<VariantCall>>
    call_variants(const std::vector<Variant>& candidates, const Latents& latents) const;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Caller::Latents& latents,
                   const ReadPileupMap& pileup) const override;

    std::vector<std::unique_ptr<ReferenceCall>>
    call_reference(const std::vector<Allele>& alleles, const Latents& latents,
                   const ReadPileupMap& pileup) const;

    const SampleName& sample() const noexcept;
    void fit_sublone_model(const HaplotypeBlock& haplotypes,
                           const IndexedHaplotypeBlock& indexed_haplotypes,
                           const HaplotypeLikelihoodArray& haplotype_likelihoods,
                           GenotypePriorModel& genotype_prior_model,
                           const model::IndividualModel::InferredLatents& haploid_latents,
                           GenotypeBlock& prev_genotypes,
                           model::SubcloneModel::InferredLatents& sublonal_inferences) const;

    std::unique_ptr<GenotypePriorModel> make_prior_model(const HaplotypeBlock& haplotypes) const;

    // debug
    void log(const Latents& latents) const;
};

class PolycloneCaller::Latents : public Caller::Latents
{
public:
    using GenotypeBlock = MappableBlock<Genotype<IndexedHaplotype<>>>;

    using HaploidModelInferences = model::IndividualModel::InferredLatents;
    using SubloneModelInferences = model::SubcloneModel::InferredLatents;

    using Caller::Latents::HaplotypeProbabilityMap;
    using Caller::Latents::GenotypeProbabilityMap;

    Latents() = delete;

    Latents(GenotypeBlock haploid_genotypes,
            GenotypeBlock polyploid_genotypes,
            HaploidModelInferences haploid_model_inferences,
            SubloneModelInferences subclone_model_inferences,
            const SampleName& sample,
            const std::function<double(unsigned)>& clonality_prior);

    std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors() const noexcept override;
    std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors() const noexcept override;

private:
    GenotypeBlock haploid_genotypes_, polyploid_genotypes_;
    HaploidModelInferences haploid_model_inferences_;
    SubloneModelInferences subclone_model_inferences_;
    PolycloneCaller::ModelProbabilities model_log_posteriors_;
    SampleName sample_;

    mutable std::shared_ptr<GenotypeProbabilityMap> genotype_posteriors_, genotype_log_posteriors_;
    mutable std::shared_ptr<HaplotypeProbabilityMap> haplotype_posteriors_;

    friend PolycloneCaller;
};

} // namespace octopus

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/dmtbx/triplet_generator.h>
#include <boost/python/class.hpp>

namespace cctbx { namespace dmtbx { namespace boost_python {
namespace {

  struct triplet_generator_wrappers
  {
    typedef triplet_generator<> w_t;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("triplet_generator", no_init)
        .def(init<sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<double> const&,
                  std::size_t,
                  bool,
                  bool>())
        .def("t_den", &w_t::t_den)
        .def("max_relations_per_reflection",&w_t::max_relations_per_reflection)
        .def("sigma_2_only", &w_t::sigma_2_only)
        .def("discard_weights", &w_t::discard_weights)
        .def("n_relations", &w_t::n_relations)
        .def("relations_for", &w_t::relations_for)
        .def("sums_of_amplitude_products", &w_t::sums_of_amplitude_products)
        .def("raw_apply_tangent_formula", &w_t::apply_tangent_formula)
      ;
    }
  };

} // namespace <anonymous>

  void wrap_triplet_generator()
  {
    triplet_generator_wrappers::wrap();
  }

}}} // namespace cctbx::dmtbx::boost_python


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

#pragma once

#include "common/definitions.hpp"
#include "common/component.hpp"
#include "boundings.hpp"

#include <memory>
#if __has_include(<eigen3/Eigen/Core>)
    #include <eigen3/Eigen/Core>
#elif __has_include(<Eigen/Core>)
    #include <Eigen/Core>
#else
    #pragma error "no eigen include"
#endif

namespace ves
{
    namespace Particle
    {
        enum class TYPE : std::int16_t
        {
            ANY = -1,
            UNDEFINED = -1,
            MOBILE = 0,
            FRAME = 1,
            OSMOTIC = 2
        };

        struct Base;
        struct Mobile;
        struct Frame;
        struct Osmotic;
    }
}

struct ves::Particle::Base
    // : public ves::Component<Base>
{

    using cartesian = Eigen::Matrix<REAL,3,1>;
    using box3d = Eigen::AlignedBox<REAL,3>;

    CoordinatesBounding coordinates_bounding;
    OrientationBounding orientation_bounding;

protected:
    Base() = default;

    REAL LJ_attraction_intensity = {1};
    REAL LJ_rejection_intensity = {1};
    cartesian coordinates = {0,0,0};
    cartesian orientation = {0,0,0};

    // nonstd::observer_ptr<Cell> paren_cell {nullptr};

public:
    virtual ~Base() = default;
    // inline virtual Base* getDerived() override { return this; };

    inline bool operator==(Base& other) const { return std::addressof(*this) == std::addressof(other); };
    inline bool operator!=(Base& other) const { return std::addressof(*this) != std::addressof(other); };

    virtual auto getType() const -> TYPE = 0;

    auto getCoordinates() -> decltype(coordinates)&;
    auto getCoordinates() const -> const decltype(coordinates)&;

    auto getOrientation() -> decltype(orientation)&;
    auto getOrientation() const -> const decltype(orientation)&;

    auto getLJAttraction() const -> decltype(LJ_attraction_intensity);
    auto getLJRejection() const -> decltype(LJ_rejection_intensity);

    void setLJAttraction(const decltype(LJ_attraction_intensity));
    void setLJRejection(const decltype(LJ_attraction_intensity));

    virtual bool try_setCoordinates(const cartesian&);
    virtual bool try_setOrientation(const cartesian&);

    void forcefullyShift(const cartesian&);

    virtual std::string getName() const = 0;
};


#include <NDTMatcher.hh>
#include <NDTMatcherF2F.hh>
#include <NDTMap.hh>
#include <OctTree.hh>
#include <AdaptiveOctTree.hh>
#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include "pcl/registration/icp.h"
#include "pcl/filters/voxel_grid.h"

#include <cstdio>
#include <Eigen/Eigen>
#include <PointCloudUtils.hh>
#include <fstream>

#define DEBUG_COVARIANCE 0

using namespace std;

float ranf()           /* ranf() is uniform in 0..1 */
{
    return (float)rand()/(float)RAND_MAX;
}

float box_muller(float m, float s)  /* normal random variate generator */
{
    /* mean m, standard deviation s */
    float x1, x2, w, y1;
    static float y2;
    static int use_last = 0;

    if (use_last)	        /* use value from previous call */
    {
        y1 = y2;
        use_last = 0;
    }
    else
    {
        do
        {
            x1 = 2.0 * ranf() - 1.0;
            x2 = 2.0 * ranf() - 1.0;
            w = x1 * x1 + x2 * x2;
        }
        while ( w >= 1.0 );

        w = sqrt( (-2.0 * log( w ) ) / w );
        y1 = x1 * w;
        y2 = x2 * w;
        use_last = 1;
    }

    return( m + y1 * s );
}

bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
              Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );

    pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
    gr1.setLeafSize(0.1,0.1,0.1);
    gr2.setLeafSize(0.1,0.1,0.1);

    gr1.setInputCloud(m);
    gr2.setInputCloud(f);

    cloud_in->height = 1;
    cloud_in->width = cloud_in->points.size();
    cloud_out->height = 1;
    cloud_out->width = cloud_out->points.size();
    cloud_in->is_dense = false;
    cloud_out->is_dense = false;

    gr1.filter(*cloud_in);
    gr2.filter(*cloud_out);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setMaximumIterations(1000);
    //cout<<"max itr are "<<icp.getMaximumIterations()<<endl;
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);

    icp.setRANSACOutlierRejectionThreshold (2);
    icp.setMaxCorrespondenceDistance(10);
    icp.setTransformationEpsilon(0.00001);
//    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
//    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
//    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

//    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
//	icp.getFitnessScore() << std::endl;
//    std::cout << icp.getFinalTransformation() << std::endl;

    //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
    Tout = (icp.getFinalTransformation()).cast<double>();

    /*    char fname[50];
        snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
        FILE *fout = fopen(fname,"w");
        fprintf(fout,"#VRML V2.0 utf8\n");
        lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
        lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
        lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
        fclose(fout);
    */
    return icp.hasConverged();

}

int
main (int argc, char** argv)
{
    std::ofstream logger ("/home/tsv/ndt_tmp/covariance.m");
    cout.precision(15);

    pcl::PointCloud<pcl::PointXYZ> prev, curr, currHere, currHere2;
    char prevName[500], currName[500];
    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> prevPose, currPose;

    double __res[] = {0.2, 0.4, 1, 2};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
    lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
    lslgeneric::NDTMatcher matcherP2F;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Pr, R;
    struct timeval tv_now;

    gettimeofday(&tv_now, NULL);
    srand(tv_now.tv_usec);

    int N_SAMPLES = 100;

    if(argc != 2)
    {
        std::cout<<"Usage: "<<argv[0]<<" configFile\n";
        return -1;
    }

    FILE *fin = fopen(argv[1],"r");
    double xd,yd,zd, x,y,z,w, ts;
    string prefix;
    char *line = NULL;
    size_t len;
    bool first = true;
    double randX, randY, randZ, randRoll, randPitch, randYaw;
    double dev_x = 0.3 ,dev_y=0.3 ,dev_z =0.3 ,dev_roll =0.1,dev_pitch =0.1,dev_yaw=0.1;

    int ctr = 0;

    //get first line
    int n = getline(&line,&len,fin);
    if(n <=0 ) return -1;
    prefix = line;
    *(prefix.rbegin()) = '\0';

    while(getline(&line,&len,fin) > 0)
    {

        int n = sscanf(line,"%lf %lf %lf %lf %lf %lf %lf %lf",
                       &ts,&xd,&yd,&zd,&x,&y,&z,&w);
        if(n != 8)
        {
            cout<<"wrong format of pose at : "<<line<<endl;
            break;
        }

        if(first)
        {
            //set previous pose to curent pose
            prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
                      Eigen::Quaternion<double>(w,x,y,z);
            first = false;
            ctr++;
            continue;
        }

        currPose =  Eigen::Translation<double,3>(xd,yd,zd)*
                    Eigen::Quaternion<double>(w,x,y,z);

        /*
        r = M_PI*r/180;
        p = M_PI*p/180;
            //due to the way we collected data, offset by 90, shouldn't matter
        y = y+90;
        y = M_PI*y/180;

        if(first) {
            //set previous pose to curent pose
            prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
        	      Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX())*
        	      Eigen::AngleAxis<double>(p,Eigen::Vector3d::UnitY())*
        	      Eigen::AngleAxis<double>(y,Eigen::Vector3d::UnitZ());
            first = false;
            ctr++;
            continue;
        }

        currPose =Eigen::Translation<double,3>(xd,yd,zd)*
            Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX())*
            Eigen::AngleAxis<double>(p,Eigen::Vector3d::UnitY())*
            Eigen::AngleAxis<double>(y,Eigen::Vector3d::UnitZ());
        */

        //compute ground truth relative pose
        Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> P = prevPose.inverse()*currPose;

        cout<<"testing scan "<<ctr-1<<" to "<<ctr<<endl;
#if DEBUG_COVARIANCE
        cout<<"prev pose    : "<<prevPose.translation().transpose()<<" rpy "<<prevPose.rotation().eulerAngles(0,1,2).transpose()<<endl;
        cout<<"curr pose    : "<<currPose.translation().transpose()<<" rpy "<<r<<" "<<p<<" "<<y<<endl;
        cout<<"gt difference: "<<P.translation().transpose()<<" rpy "<<P.rotation().eulerAngles(0,1,2).transpose()<<endl;
#endif

        for( int i=0; i<N_SAMPLES; i++)
        {
            cout<<"itr: "<<i<<endl;
            Pr.setIdentity();
            snprintf(prevName,499,"%s%03d_%03d.wrl",prefix.c_str(),ctr-1,i);
            snprintf(currName,499,"%s%03d_%03d.wrl",prefix.c_str(),ctr,i);
            prev = lslgeneric::readVRML(prevName);
            curr = lslgeneric::readVRML(currName);

            //draw random numbers
            randX = box_muller(0,dev_x);
            randY = box_muller(0,dev_y);
            randZ = box_muller(0,dev_z);
            randRoll = box_muller(0,dev_roll);
            randPitch = box_muller(0,dev_pitch);
            randYaw = box_muller(0,dev_yaw);

            currPose =  Eigen::Translation<double,3>(xd+randX,yd+randY,zd+randZ)*
                        Eigen::Quaternion<double>(w,x,y,z)*
                        Eigen::AngleAxis<double>(randRoll,Eigen::Vector3d::UnitX())*
                        Eigen::AngleAxis<double>(randPitch,Eigen::Vector3d::UnitY())*
                        Eigen::AngleAxis<double>(randYaw,Eigen::Vector3d::UnitZ());
            //compute current pose
            /*currPose =Eigen::Translation<double,3>(xd+randX,yd+randY,zd+randZ)*
            Eigen::AngleAxis<double>(r+randRoll,Eigen::Vector3d::UnitX())*
            Eigen::AngleAxis<double>(p+randPitch,Eigen::Vector3d::UnitY())*
            Eigen::AngleAxis<double>(y+randYaw,Eigen::Vector3d::UnitZ());
            */

            //compute relative pose
            Pr = prevPose.inverse()*currPose;

#if DEBUG_COVARIANCE
            cout<<"curr pose: "<<currPose.translation().transpose()<<" "<<r+randRoll<<" "<<p+randPitch<<" "<<y+randYaw<<endl;
            cout<<"diff: "<<Pr.translation().transpose()<<" rpy "<<Pr.rotation().eulerAngles(0,1,2).transpose()<<endl;
#endif
            bool ret;
            Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> offset;
            /*
            	    //point2NDT
            	    //transform curr scan by Pr
            	    currHere = lslgeneric::transformPointCloud(Pr,curr);
            	    ret = matcherP2F.match(prev,currHere,R);
            	    //compute P[-](Pr[+]R)
            	    offset = P.inverse()*(R*Pr);
            	    //that's the error! log it.
            	    logger<<"pnt2ndt("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";

            	    //ICP
            	    //transform curr scan by Pr
            	    currHere = lslgeneric::transformPointCloud(Pr,curr);
            	    ret = matchICP(prev,currHere,R);
            	    //compute P[-](Pr[+]R)
            	    offset = P.inverse()*(R*Pr);
            	    //that's the error! log it.
            	    logger<<"icp("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
            */
            //NDT2NDT
            //transform curr scan by Pr
            currHere = lslgeneric::transformPointCloud(Pr,curr);
            ret = matcherF2F.match(prev,currHere,R);
#if DEBUG_COVARIANCE
            currHere2 = lslgeneric::transformPointCloud(R,currHere);
            char fname[50];
            snprintf(fname,49,"/home/tsv/ndt_tmp/c_%03d.wrl",ctr);
            FILE *fout = fopen(fname,"w");
            fprintf(fout,"#VRML V2.0 utf8\n");
            lslgeneric::writeToVRML(fout,currHere2,Eigen::Vector3d(0,1,0));
            lslgeneric::writeToVRML(fout,currHere,Eigen::Vector3d(1,0,0));
            lslgeneric::writeToVRML(fout,prev,Eigen::Vector3d(1,1,1));
            fclose(fout);
            cout<<"registration: "<<R.translation().transpose()<<" rpy "<<R.rotation().eulerAngles(0,1,2).transpose()<<endl;
#endif
            //compute P[-](Pr[+]R)
            offset = P.inverse()*(R*Pr);
            //that's the error! log it.
            logger<<"ndt2ndt("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
//	    cout<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<endl;

        }

        Eigen::Matrix<double,6,6> cov;
//	matcherP2F.covariance(prev,curr,P,cov);
//	logger<<"COVpnt2ndt("<<ctr<<",:,:) = ["<<cov.row(0)<<";\n"<<cov.row(1)<<";\n"<<cov.row(2)<<";\n"<<cov.row(3)<<";\n"<<cov.row(4)<<";\n"<<cov.row(5)<<"];\n";
        matcherF2F.covariance(prev,curr,P,cov);
        logger<<"COVndt2ndt("<<ctr<<",:,:) = ["<<cov.row(0)<<";\n"<<cov.row(1)<<";\n"<<cov.row(2)<<";\n"<<cov.row(3)<<";\n"<<cov.row(4)<<";\n"<<cov.row(5)<<"];\n";

        prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
                  Eigen::Quaternion<double>(w,x,y,z);
        ctr++;
    }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2012-2014 Barend Gehrels, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR

#include <boost/range.hpp>

#include <boost/geometry/arithmetic/dot_product.hpp>
#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/expand.hpp>
#include <boost/geometry/algorithms/detail/disjoint/point_box.hpp>
#include <boost/geometry/algorithms/detail/overlay/segment_identifier.hpp>
#include <boost/geometry/algorithms/detail/overlay/get_turn_info.hpp>
#include <boost/geometry/policies/compare.hpp>
#include <boost/geometry/strategies/buffer.hpp>

#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/algorithms/comparable_distance.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace buffer
{

struct turn_get_box
{
    template <typename Box, typename Turn>
    static inline void apply(Box& total, Turn const& turn)
    {
        geometry::expand(total, turn.robust_point);
    }
};

struct turn_ovelaps_box
{
    template <typename Box, typename Turn>
    static inline bool apply(Box const& box, Turn const& turn)
    {
        return ! dispatch::disjoint
            <
                typename Turn::robust_point_type,
                Box
            >::apply(turn.robust_point, box);
    }
};

template <typename Turns, typename Pieces>
class turn_in_piece_visitor
{
    Turns& m_turns; // because partition is currently operating on const input only
    Pieces const& m_pieces; // to check for piece-type

    typedef boost::long_long_type calculation_type;

    template <typename Point>
    static inline bool projection_on_segment(Point const& subject, Point const& p, Point const& q)
    {
        typedef Point vector_type;
        typedef typename geometry::coordinate_type<Point>::type coordinate_type;

        vector_type v = q;
        vector_type w = subject;
        subtract_point(v, p);
        subtract_point(w, p);

        coordinate_type const zero = coordinate_type();
        coordinate_type const c1 = dot_product(w, v);

        if (c1 < zero)
        {
            return false;
        }
        coordinate_type const c2 = dot_product(v, v);
        if (c2 < c1)
        {
            return false;
        }

        return true;
    }

    template <typename Point, typename Piece>
    inline bool on_offsetted(Point const& point, Piece const& piece) const
    {
        typedef typename strategy::side::services::default_strategy
            <
                typename cs_tag<Point>::type
            >::type side_strategy;
        geometry::equal_to<Point> comparator;

        for (int i = 1; i < piece.offsetted_count; i++)
        {
            Point const& previous = piece.robust_ring[i - 1];
            Point const& current = piece.robust_ring[i];

            // The robust ring contains duplicates, avoid applying side on them (will be 0)
            if (! comparator(previous, current))
            {
                int const side = side_strategy::apply(previous, current, point);
                if (side == 0)
                {
                    // Collinear, check if projection falls on it
                    if (projection_on_segment(point, previous, current))
                    {
                        return true;
                    }
                }
            }
        }
        return false;
    }

    template <typename Point, typename Piece>
    static inline
    calculation_type comparable_distance_from_offsetted(Point const& point,
                        Piece const& piece)
    {
        // TODO: pass subrange to dispatch to avoid making copy
        geometry::model::linestring<Point> ls;
        std::copy(piece.robust_ring.begin(),
            piece.robust_ring.begin() + piece.offsetted_count,
            std::back_inserter(ls));
        typename default_comparable_distance_result<Point, Point>::type
            const comp = geometry::comparable_distance(point, ls);

        return static_cast<calculation_type>(comp);
    }

public:

    inline turn_in_piece_visitor(Turns& turns, Pieces const& pieces)
        : m_turns(turns)
        , m_pieces(pieces)
    {}

    template <typename Turn, typename Piece>
    inline void apply(Turn const& turn, Piece const& piece, bool first = true)
    {
        boost::ignore_unused_variable_warning(first);

        if (turn.count_within > 0)
        {
            // Already inside - no need to check again
            return;
        }

        if (piece.type == strategy::buffer::buffered_flat_end
            || piece.type == strategy::buffer::buffered_concave)
        {
            // Turns cannot be inside a flat end (though they can be on border)
            // Neither we need to check if they are inside concave helper pieces
            return;
        }

        bool neighbour = false;
        for (int i = 0; i < 2; i++)
        {
            // Don't compare against one of the two source-pieces
            if (turn.operations[i].piece_index == piece.index)
            {
                return;
            }

            typename boost::range_value<Pieces>::type const& pc
                                = m_pieces[turn.operations[i].piece_index];
            if (pc.left_index == piece.index
                || pc.right_index == piece.index)
            {
                if (pc.type == strategy::buffer::buffered_flat_end)
                {
                    // If it is a flat end, don't compare against its neighbor:
                    // it will always be located on one of the helper segments
                    return;
                }
                neighbour = true;
            }
        }

        int geometry_code = detail::within::point_in_geometry(turn.robust_point, piece.robust_ring);

        if (geometry_code == -1)
        {
            return;
        }
        if (geometry_code == 0 && neighbour)
        {
            return;
        }

        Turn& mutable_turn = m_turns[turn.turn_index];
        if (geometry_code == 0)
        {
            // If it is on the border and they are not neighbours, it should be
            // on the offsetted ring

            if (! on_offsetted(turn.robust_point, piece))
            {
                // It is on the border but not on the offsetted ring.
                // Then it is somewhere on the helper-segments
                // Classify it as "within"
                geometry_code = 1;
                mutable_turn.count_on_helper++; // can still become "near_offsetted"
            }
            else
            {
                mutable_turn.count_on_offsetted++; // value is not used anymore
            }
        }

        if (geometry_code == 1)
        {
            calculation_type const distance
                = comparable_distance_from_offsetted(turn.robust_point, piece);
            if (distance >= 4)
            {
                // This is too far from the border, it counts as "really within"
                mutable_turn.count_within++;
            }
            else
            {
                // Other points count as still "on border" because they might be
                // travelled through, but not used as starting point
                mutable_turn.count_within_near_offsetted++;
            }
        }
    }
};

}} // namespace detail::buffer
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_BUFFER_TURN_IN_PIECE_VISITOR


/*!
  \file gpp_expected_improvement_gpu_test.cpp
  \rst
  Routines to test the functions in gpp_expected_improvement_gpu.cpp.

  The tests verify ExpectedImprovementGPUEvaluator from gpp_expected_improvement_gpu.cpp.

  1. Monte-Carlo EI vs analytic EI validation: the monte-carlo versions are run to "high" accuracy and checked against
     analytic formulae when applicable

  2. GPU EI vs CPU EI: both use monte-carlo version and run consistency check on various random sample points
\endrst*/

#include "gpp_expected_improvement_gpu_test.hpp"

#include <vector>
#include <algorithm>

#include <boost/random/uniform_real.hpp>  // NOLINT(build/include_order)

#include "gpp_common.hpp"
#include "gpp_exception.hpp"
#include "gpp_expected_improvement_gpu.hpp"
#include "gpp_math.hpp"
#include "gpp_random.hpp"
#include "gpp_test_utils.hpp"

namespace optimal_learning {

#ifdef OL_GPU_ENABLED

namespace {

/*!\rst
  Test that the EI + grad EI computation (using MC integration) is consistent
  with the special analytic case of EI when there is only *ONE* potential point
  to sample.

  Generates a set of 40 random test cases for expected improvement with only one potential sample.
  The general EI (which uses MC integration) is evaluated to reasonably high accuracy (while not taking too long to run)
  and compared against the analytic formula version for consistency.  The gradients (spatial) of EI are also checked.

  \return
    number of cases where analytic and monte-carlo EI do not match
\endrst*/
int RunCudaEIConsistencyTests() {
  int total_errors = 0;

  int which_gpu = 1;
  const int num_mc_iter = 20000000;
  const int dim = 3;
  const int num_being_sampled = 0;
  const int num_to_sample = 1;
  const int num_sampled = 7;

  double alpha = 2.80723;
  // set best_so_far to be larger than max(points_sampled_value) (but don't make it huge or stability will be suffer)
  double best_so_far = 10.0;
  bool configure_for_gradients = true;

  UniformRandomGenerator uniform_generator(31278);
  boost::uniform_real<double> uniform_double(0.5, 2.5);

  MockExpectedImprovementEnvironment ei_environment;

  std::vector<double> lengths(dim);
  std::vector<double> noise_variance(num_sampled, 0.0);
  std::vector<double> grad_ei_cuda(dim);
  std::vector<double> grad_ei_one_potential_sample(dim);
  double ei_cuda;
  double ei_one_potential_sample;

  for (int i = 0; i < 40; ++i) {
    ei_environment.Initialize(dim, num_to_sample, num_being_sampled, num_sampled, &uniform_generator);
    for (int j = 0; j < dim; ++j) {
      lengths[j] = uniform_double(uniform_generator.engine);
    }
    SquareExponential sqexp_covariance(dim, alpha, lengths);
    GaussianProcess gaussian_process(sqexp_covariance, ei_environment.points_sampled(), ei_environment.points_sampled_value(), noise_variance.data(), dim, num_sampled);

    OnePotentialSampleExpectedImprovementEvaluator one_potential_sample_ei_evaluator(gaussian_process, best_so_far);
    OnePotentialSampleExpectedImprovementEvaluator::StateType one_potential_sample_ei_state(one_potential_sample_ei_evaluator, ei_environment.points_to_sample(), configure_for_gradients);

    CudaExpectedImprovementEvaluator cuda_ei_evaluator(gaussian_process, num_mc_iter, best_so_far, which_gpu);
    CudaExpectedImprovementEvaluator::StateType cuda_ei_state(cuda_ei_evaluator, ei_environment.points_to_sample(), ei_environment.points_being_sampled(), num_to_sample, num_being_sampled, configure_for_gradients, &uniform_generator);

    ei_cuda = cuda_ei_evaluator.ComputeObjectiveFunction(&cuda_ei_state);
    cuda_ei_evaluator.ComputeGradObjectiveFunction(&cuda_ei_state, grad_ei_cuda.data());
    ei_one_potential_sample = one_potential_sample_ei_evaluator.ComputeObjectiveFunction(&one_potential_sample_ei_state);
    one_potential_sample_ei_evaluator.ComputeGradObjectiveFunction(&one_potential_sample_ei_state, grad_ei_one_potential_sample.data());

    int ei_errors_this_iteration = 0;
    if (!CheckDoubleWithinRelative(ei_cuda, ei_one_potential_sample, 5.0e-3)) {
      ++ei_errors_this_iteration;
    }
    if (ei_errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("in EI on iteration %d\n", i);
    }
    total_errors += ei_errors_this_iteration;

    int grad_ei_errors_this_iteration = 0;
    for (int j = 0; j < dim; ++j) {
      if (!CheckDoubleWithinRelative(grad_ei_cuda[j], grad_ei_one_potential_sample[j], 4.5e-3)) {
        ++grad_ei_errors_this_iteration;
      }
    }

    if (grad_ei_errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("in EI gradients on iteration %d\n", i);
    }
    total_errors += grad_ei_errors_this_iteration;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("comparing MC EI to analytic EI failed with %d total_errors\n\n", total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("comparing MC EI to analytic EI passed\n");
  }
  return total_errors;
}

/*!\rst
  Tests that the general EI + grad EI computation on CPU (using MC integration) is consistent
  with the computation on GPU. We use exactly the same sequences of normal random numbers on
  CPU and GPU so that they are supposed to output the same result even if the number of MC
  iterations is small.

  Generates a set of 10 random test cases for genral q,p-EI computed on cpu vs gpu.
  The computations on cpu and gpu use the same set of normal random numbers for MC
  simulation, so that we can make sure the outputs should be consistent, even with
  a relative small number of MC iteration.

  \return
    number of cases where outputs from cpu and gpu do not match.
\endrst*/
int RunCudaEIvsCpuEITests() {
  int total_errors = 0;

  int which_gpu = 1;
  const int num_mc_iter = 40000;
  const int dim = 3;
  const int num_being_sampled = 4;
  const int num_to_sample = 4;
  const int num_sampled = 20;

  double alpha = 2.80723;
  // set best_so_far to be larger than max(points_sampled_value) (but don't make it huge or stability will be suffer)
  double best_so_far = 10.0;
  bool configure_for_gradients = true;
  bool configure_for_test = true;

  UniformRandomGenerator uniform_generator(31278);
  boost::uniform_real<double> uniform_double(0.5, 2.5);

  MockExpectedImprovementEnvironment ei_environment;

  std::vector<double> lengths(dim);
  std::vector<double> noise_variance(num_sampled, 0.0);
  std::vector<double> grad_ei_cpu(dim*num_to_sample);
  std::vector<double> grad_ei_gpu(dim*num_to_sample);
  std::vector<double> normal_random_table;
  double ei_cpu;
  double ei_gpu;
  int cpu_num_iter;

  for (int i = 0; i < 10; ++i) {
    ei_environment.Initialize(dim, num_to_sample, num_being_sampled, num_sampled, &uniform_generator);
    for (int j = 0; j < dim; ++j) {
      lengths[j] = uniform_double(uniform_generator.engine);
    }
    SquareExponential sqexp_covariance(dim, alpha, lengths);
    GaussianProcess gaussian_process(sqexp_covariance, ei_environment.points_sampled(), ei_environment.points_sampled_value(), noise_variance.data(), dim, num_sampled);

    CudaExpectedImprovementEvaluator cuda_ei_evaluator(gaussian_process, num_mc_iter, best_so_far, which_gpu);
    CudaExpectedImprovementEvaluator::StateType cuda_ei_state(cuda_ei_evaluator, ei_environment.points_to_sample(), ei_environment.points_being_sampled(), num_to_sample, num_being_sampled, configure_for_gradients, &uniform_generator, configure_for_test);

    ei_gpu = cuda_ei_evaluator.ComputeObjectiveFunction(&cuda_ei_state);

    // setup cpu EI computation
    normal_random_table = cuda_ei_state.random_number_ei;
    cpu_num_iter = normal_random_table.size()/ (num_being_sampled + num_to_sample);
    NormalRNGSimulator normal_rng_for_ei(normal_random_table);
    ExpectedImprovementEvaluator ei_evaluator_for_ei(gaussian_process, cpu_num_iter, best_so_far);
    ExpectedImprovementEvaluator::StateType ei_state_for_ei(ei_evaluator_for_ei, ei_environment.points_to_sample(), ei_environment.points_being_sampled(), num_to_sample, num_being_sampled, false, &normal_rng_for_ei);
    ei_cpu = ei_evaluator_for_ei.ComputeObjectiveFunction(&ei_state_for_ei);

    // setup cpu gradEI computation
    cuda_ei_evaluator.ComputeGradObjectiveFunction(&cuda_ei_state, grad_ei_gpu.data());

    normal_random_table = cuda_ei_state.random_number_grad_ei;
    cpu_num_iter = normal_random_table.size()/ (num_being_sampled + num_to_sample);
    NormalRNGSimulator normal_rng_for_grad_ei(normal_random_table);
    ExpectedImprovementEvaluator ei_evaluator_for_grad_ei(gaussian_process, cpu_num_iter, best_so_far);
    ExpectedImprovementEvaluator::StateType ei_state_for_grad_ei(ei_evaluator_for_grad_ei, ei_environment.points_to_sample(), ei_environment.points_being_sampled(), num_to_sample, num_being_sampled, true, &normal_rng_for_grad_ei);
    ei_evaluator_for_grad_ei.ComputeGradObjectiveFunction(&ei_state_for_grad_ei, grad_ei_cpu.data());

    int ei_errors_this_iteration = 0;
    if (!CheckDoubleWithinRelative(ei_cpu, ei_gpu, 1.0e-12)) {
      ++ei_errors_this_iteration;
    }
    if (ei_errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("in EI on iteration %d\n", i);
    }
    total_errors += ei_errors_this_iteration;

    int grad_ei_errors_this_iteration = 0;
    for (int j = 0; j < dim*num_to_sample; ++j) {
      if (!CheckDoubleWithinRelative(grad_ei_cpu[j], grad_ei_gpu[j], 1.0e-12)) {
        ++grad_ei_errors_this_iteration;
      }
    }

    if (grad_ei_errors_this_iteration != 0) {
      OL_PARTIAL_FAILURE_PRINTF("in EI gradients on iteration %d\n", i);
    }
    total_errors += grad_ei_errors_this_iteration;
  }

  if (total_errors != 0) {
    OL_PARTIAL_FAILURE_PRINTF("comparing cpu EI to gpu EI failed with %d total_errors\n\n", total_errors);
  } else {
    OL_PARTIAL_SUCCESS_PRINTF("comparing cpu EI to gpu EI passed\n");
  }
  return total_errors;
}

/*!\rst
  This function is the same as ``ExpectedImprovementOptimizationMultipleSamplesTest``
  in ``gpp_math_test.cpp``. Refer to ``gpp_math_test.cpp`` for detailed documentation.
\endrst*/
int CudaExpectedImprovementOptimizationMultipleSamplesTest() {
  using DomainType = TensorProductDomain;
  const int dim = 3;

  int total_errors = 0;
  int current_errors = 0;

  // gradient descent parameters
  const double gamma = 0.5;
  const double pre_mult = 1.0;
  const double max_relative_change = 1.0;
  const double tolerance = 1.0e-5;
  const int max_gradient_descent_steps = 100;
  const int max_num_restarts = 3;
  const int num_multistarts = 20;
  GradientDescentParameters gd_params(num_multistarts, max_gradient_descent_steps,
                                      max_num_restarts, gamma, pre_mult,
                                      max_relative_change, tolerance);

  // grid search parameters
  int num_grid_search_points = 5000;

  // q,p-EI computation parameters
  const int num_to_sample = 3;
  const int num_being_sampled = 0;

  std::vector<double> points_being_sampled(dim*num_being_sampled);
  int max_int_steps = 10000;

  // random number generators
  UniformRandomGenerator uniform_generator(314);
  boost::uniform_real<double> uniform_double_hyperparameter(0.4, 1.3);
  boost::uniform_real<double> uniform_double_lower_bound(-5.0, -4.5);
  boost::uniform_real<double> uniform_double_upper_bound(4.0, 4.5);

  static const int kMaxNumThreads = 1;
  ThreadSchedule thread_schedule(kMaxNumThreads, omp_sched_static);

  const int num_sampled = 50;
  std::vector<double> noise_variance(num_sampled, 0.002);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(SquareExponential(dim, 1.0, 1.0),
                                                        noise_variance, dim, num_sampled,
                                                        uniform_double_lower_bound,
                                                        uniform_double_upper_bound,
                                                        uniform_double_hyperparameter,
                                                        &uniform_generator);

  // we will optimize over the expanded region
  std::vector<ClosedInterval> domain_bounds(mock_gp_data.domain_bounds);
  ExpandDomainBounds(1.5, &domain_bounds);
  DomainType domain(domain_bounds.data(), dim);

  const int which_gpu = 0;

  // optimize EI using grid search to set the baseline
  bool found_flag = false;
  std::vector<double> grid_search_best_point_set(dim*num_to_sample);
  CudaComputeOptimalPointsToSampleViaLatinHypercubeSearch(*mock_gp_data.gaussian_process_ptr, domain,
                                                          thread_schedule, points_being_sampled.data(),
                                                          num_grid_search_points, num_to_sample,
                                                          num_being_sampled, mock_gp_data.best_so_far,
                                                          max_int_steps, &found_flag, which_gpu,
                                                          &uniform_generator, grid_search_best_point_set.data());
  if (!found_flag) {
    ++total_errors;
  }

  // optimize EI using gradient descent
  found_flag = false;
  bool lhc_search_only = false;
  std::vector<double> best_points_to_sample(dim*num_to_sample);
  CudaComputeOptimalPointsToSample(*mock_gp_data.gaussian_process_ptr, gd_params, domain,
                                   thread_schedule, points_being_sampled.data(),
                                   num_to_sample, num_being_sampled, mock_gp_data.best_so_far,
                                   max_int_steps, lhc_search_only,
                                   num_grid_search_points, &found_flag, which_gpu,
                                   &uniform_generator, best_points_to_sample.data());
  if (!found_flag) {
    ++total_errors;
  }

  // check points are in domain
  RepeatedDomain<DomainType> repeated_domain(domain, num_to_sample);
  if (!repeated_domain.CheckPointInside(best_points_to_sample.data())) {
    ++current_errors;
  }
#ifdef OL_ERROR_PRINT
  if (current_errors != 0) {
    OL_ERROR_PRINTF("ERROR: points were not in domain!  points:\n");
    PrintMatrixTrans(best_points_to_sample.data(), num_to_sample, dim);
    OL_ERROR_PRINTF("domain:\n");
    PrintDomainBounds(domain_bounds.data(), dim);
  }
#endif
  total_errors += current_errors;

  // check points are distinct; points within tolerance are considered non-distinct
  const double distinct_point_tolerance = 1.0e-5;
  current_errors = CheckPointsAreDistinct(best_points_to_sample.data(), num_to_sample, dim, distinct_point_tolerance);
#ifdef OL_ERROR_PRINT
  if (current_errors != 0) {
    OL_ERROR_PRINTF("ERROR: points were not distinct!  points:\n");
    PrintMatrixTrans(best_points_to_sample.data(), num_to_sample, dim);
  }
#endif
  total_errors += current_errors;

  // results
  double ei_optimized, ei_grid_search;
  std::vector<double> grad_ei(dim*num_to_sample);

  // set up evaluators and state to check results
  double tolerance_result = tolerance;
  {
    max_int_steps = 100000000;  // evaluate the final results with high accuracy
    tolerance_result = 7e-3;  // reduce b/c we cannot achieve full accuracy in the monte-carlo case
    // while still having this test run in a reasonable amt of time
    bool configure_for_gradients = true;
    CudaExpectedImprovementEvaluator ei_evaluator(*mock_gp_data.gaussian_process_ptr,
                                                  max_int_steps, mock_gp_data.best_so_far, which_gpu);
    CudaExpectedImprovementEvaluator::StateType ei_state(ei_evaluator, best_points_to_sample.data(),
                                                         points_being_sampled.data(), num_to_sample,
                                                         num_being_sampled, configure_for_gradients,
                                                         &uniform_generator);

    ei_optimized = ei_evaluator.ComputeExpectedImprovement(&ei_state);
    ei_evaluator.ComputeGradExpectedImprovement(&ei_state, grad_ei.data());

    CudaExpectedImprovementEvaluator::StateType ei_state_grid_search(ei_evaluator,
                                                                     grid_search_best_point_set.data(),
                                                                     points_being_sampled.data(), num_to_sample,
                                                                     num_being_sampled, configure_for_gradients,
                                                                     &uniform_generator);
    ei_grid_search = ei_evaluator.ComputeExpectedImprovement(&ei_state_grid_search);
  }

  printf("optimized EI: %.18E, grid_search_EI: %.18E\n", ei_optimized, ei_grid_search);
  printf("grad_EI: "); PrintMatrixTrans(grad_ei.data(), num_to_sample, dim);

  if (ei_optimized < ei_grid_search) {
    ++total_errors;
  }

  current_errors = 0;
  for (const auto& entry : grad_ei) {
    if (!CheckDoubleWithinRelative(entry, 0.0, tolerance_result)) {
      ++current_errors;
    }
  }
  total_errors += current_errors;

  return total_errors;
}

/*!\rst
  This function tests GPU optimizer in the case of 1-EI, in which GPU optimizer
  merely calls optimizer in ``gpp_math.hpp`` since we are not using MC simulation
  anyway. The function definition is mostly copied from ``ExpectedImprovementOptimizationTestCore`` in ``gpp_math_test.cpp`` by preserving
  its 1-EI part.
\endrst*/
int CudaExpectedImprovementOptimizationAnalyticTest() {
  using DomainType = TensorProductDomain;
  const int dim = 3;
  const int which_gpu = 0;

  int total_errors = 0;
  int current_errors = 0;

  // gradient descent parameters
  const double gamma = 0.5;
  const double pre_mult = 1.4;
  const double max_relative_change = 1.0;
  const double tolerance = 1.0e-7;
  const int max_gradient_descent_steps = 1000;
  const int max_num_restarts = 10;
  const int num_multistarts = 20;
  GradientDescentParameters gd_params(num_multistarts, max_gradient_descent_steps, max_num_restarts,
                                      gamma, pre_mult, max_relative_change, tolerance);

  // grid search parameters
  int num_grid_search_points = 10000;

  // 1,p-EI computation parameters
  const int num_to_sample = 1;
  int num_being_sampled = 0;
  int max_int_steps = 6000;

  // random number generators
  UniformRandomGenerator uniform_generator(314);
  boost::uniform_real<double> uniform_double_hyperparameter(0.4, 1.3);
  boost::uniform_real<double> uniform_double_lower_bound(-2.0, 0.5);
  boost::uniform_real<double> uniform_double_upper_bound(1.0, 2.5);

  static const int kMaxNumThreads = 4;
  ThreadSchedule thread_schedule(kMaxNumThreads, omp_sched_static);

  int num_sampled = 20;

  std::vector<double> noise_variance(num_sampled, 0.002);
  MockGaussianProcessPriorData<DomainType> mock_gp_data(SquareExponential(dim, 1.0, 1.0), noise_variance,
                                                        dim, num_sampled, uniform_double_lower_bound,
                                                        uniform_double_upper_bound,
                                                        uniform_double_hyperparameter,
                                                        &uniform_generator);

  // we will optimize over the expanded region
  std::vector<ClosedInterval> domain_bounds(mock_gp_data.domain_bounds);
  ExpandDomainBounds(1.5, &domain_bounds);
  DomainType domain(domain_bounds.data(), dim);

  std::vector<double> points_being_sampled(dim*num_being_sampled);

  // optimize EI
  bool found_flag = false;
  std::vector<double> grid_search_best_point(dim*num_to_sample);
  CudaComputeOptimalPointsToSampleViaLatinHypercubeSearch(*mock_gp_data.gaussian_process_ptr, domain,
                                                          thread_schedule, points_being_sampled.data(),
                                                          num_grid_search_points, num_to_sample,
                                                          num_being_sampled, mock_gp_data.best_so_far,
                                                          max_int_steps, &found_flag, which_gpu,
                                                          &uniform_generator, grid_search_best_point.data());
  if (!found_flag) {
    ++total_errors;
  }

  std::vector<double> next_point(dim*num_to_sample);
  found_flag = false;
  CudaComputeOptimalPointsToSampleWithRandomStarts(*mock_gp_data.gaussian_process_ptr, gd_params,
                                                   domain, thread_schedule, points_being_sampled.data(),
                                                   num_to_sample, num_being_sampled,
                                                   mock_gp_data.best_so_far, max_int_steps,
                                                   &found_flag, which_gpu,
                                                   &uniform_generator, next_point.data());
  if (!found_flag) {
    ++total_errors;
  }

  printf("next best point  : "); PrintMatrixTrans(next_point.data(), num_to_sample, dim);
  printf("grid search point: "); PrintMatrixTrans(grid_search_best_point.data(), num_to_sample, dim);

  // results
  double ei_optimized, ei_grid_search;
  std::vector<double> grad_ei(dim*num_to_sample);

  // set up evaluators and state to check results
  double tolerance_result = tolerance;
  bool configure_for_gradients = true;

  OnePotentialSampleExpectedImprovementEvaluator ei_evaluator(*mock_gp_data.gaussian_process_ptr,
                                                              mock_gp_data.best_so_far);
  OnePotentialSampleExpectedImprovementEvaluator::StateType ei_state(ei_evaluator, next_point.data(),
                                                                     configure_for_gradients);

  ei_optimized = ei_evaluator.ComputeExpectedImprovement(&ei_state);
  ei_evaluator.ComputeGradExpectedImprovement(&ei_state, grad_ei.data());

  ei_state.SetCurrentPoint(ei_evaluator, grid_search_best_point.data());
  ei_grid_search = ei_evaluator.ComputeExpectedImprovement(&ei_state);

  printf("optimized EI: %.18E, grid_search_EI: %.18E\n", ei_optimized, ei_grid_search);
  printf("grad_EI: "); PrintMatrixTrans(grad_ei.data(), num_to_sample, dim);

  if (ei_optimized < ei_grid_search) {
    ++total_errors;
  }

  current_errors = 0;
  for (const auto& entry : grad_ei) {
    if (!CheckDoubleWithinRelative(entry, 0.0, tolerance_result)) {
      ++current_errors;
    }
  }
  total_errors += current_errors;

  return total_errors;
}

}  // end unnamed namespace

/*!\rst
  Invoke all tests for GPU functions.

  \return
    number of test failures: 0 if all is working well.
\endrst*/
int RunGPUTests() {
  int total_errors = 0;
  int error = RunCudaEIConsistencyTests();
  if (error != 0) {
    OL_FAILURE_PRINTF("analytic, Cuda EI do not match for 1 potential sample case\n");
  } else {
    OL_SUCCESS_PRINTF("analytic, Cuda EI match for 1 potential sample case\n");
  }
  total_errors += error;

  error = RunCudaEIvsCpuEITests();
  if (error != 0) {
    OL_FAILURE_PRINTF("cudaEI vs cpuEI consistency check failed\n");
  } else {
    OL_SUCCESS_PRINTF("cudaEI vs cpuEI consistency check succeeded\n");
  }
  total_errors += error;

  error = CudaExpectedImprovementOptimizationAnalyticTest();
  if (error != 0) {
    OL_FAILURE_PRINTF("GPU 1-EI optimization failed\n");
  } else {
    OL_SUCCESS_PRINTF("GPU 1-EI optimization succeeded\n");
  }
  total_errors += error;

  error = CudaExpectedImprovementOptimizationMultipleSamplesTest();
  if (error != 0) {
    OL_FAILURE_PRINTF("GPU optimizer failed\n");
  } else {
    OL_SUCCESS_PRINTF("GPU optimizer succeeded\n");
  }
  total_errors += error;

  return total_errors;
}

#else  // OL_GPU_ENABLED

int RunGPUTests() {
  OL_WARNING_PRINTF("no gpu component is enabled, this test did not run.\n");
  return 0;
}

#endif  // OL_GPU_ENABLED

}  // end namespace optimal_learning


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

#include <boost/config.hpp>
#include <boost/multiprecision/number.hpp>
#include <limits>

// clang-format off

template <class T, class U>
BOOST_CXX14_CONSTEXPR T do_test_constexpr_add_subtract(T a, U b)
{
   a = +b;
   if constexpr(std::numeric_limits<U>::is_signed && std::numeric_limits<T>::is_signed)
      b = -b;
   a += b;
   a += a;
   a -= b;
   a -= a;
   ++a;
   --a;
   T bb(b);
   a += bb++;
   a += bb--;
   a = a + b;
   a += a - b;
   if constexpr(std::numeric_limits<U>::is_signed && std::numeric_limits<T>::is_signed)
      a -= b - -a;
   a += b + a;
   if constexpr(std::numeric_limits<T>::is_signed)
   {
      a = -a;
      if constexpr(std::numeric_limits<U>::is_signed)
         a -= b;
   }
   return a;
}

template <class T>
BOOST_CXX14_CONSTEXPR T test_constexpr_add_subtract(T a)
{
   a += do_test_constexpr_add_subtract(a, a);
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned char>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<signed char>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<char>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<short>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned short>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<int>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned int>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<long>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned long>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<long long>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned long long>(2));
#ifdef BOOST_HAS_INT128
   a += do_test_constexpr_add_subtract(a, static_cast<__int128>(2));
   a += do_test_constexpr_add_subtract(a, static_cast<unsigned __int128>(2));
   a -= do_test_constexpr_add_subtract(a, static_cast<__int128>(2));
   a -= do_test_constexpr_add_subtract(a, static_cast<unsigned __int128>(2));
#endif

   if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_floating_point)
   {
      a += do_test_constexpr_add_subtract(a, static_cast<float>(2));
      a += do_test_constexpr_add_subtract(a, static_cast<double>(2));
      a += do_test_constexpr_add_subtract(a, static_cast<long double>(2));
#ifdef BOOST_HAS_FLOAT128
      a += do_test_constexpr_add_subtract(a, static_cast<__float128>(2));
#endif
   }

   return a;
}

template <class T, class U>
BOOST_CXX14_CONSTEXPR T do_test_constexpr_mul_divide(T a, U b)
{
   a *= b;
   a = a * b;
   if constexpr(std::numeric_limits<T>::is_signed && std::numeric_limits<U>::is_signed)
   {
      a *= -b;
      a = a * -b;
   }
   a /= b;
   a = a / b;
   if constexpr(std::numeric_limits<T>::is_signed && std::numeric_limits<U>::is_signed)
   {
      a /= -b;
      a = a / -b;
   }
   if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_integer && boost::multiprecision::number_category<U>::value == boost::multiprecision::number_kind_integer)
   {
      a %= b;
      a = a % b;
   }
   return a;
}

template <class T>
BOOST_CXX14_CONSTEXPR T test_constexpr_mul_divide(T a)
{
   a += do_test_constexpr_mul_divide(a, a);
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned char>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<signed char>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<char>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<short>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned short>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<int>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned int>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<long>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned long>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<long long>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned long long>(2));
#ifdef BOOST_HAS_INT128
   a += do_test_constexpr_mul_divide(a, static_cast<__int128>(2));
   a += do_test_constexpr_mul_divide(a, static_cast<unsigned __int128>(2));
   a -= do_test_constexpr_mul_divide(a, static_cast<__int128>(2));
   a -= do_test_constexpr_mul_divide(a, static_cast<unsigned __int128>(2));
#endif

   if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_floating_point)
   {
      a += do_test_constexpr_mul_divide(a, static_cast<float>(2));
      a += do_test_constexpr_mul_divide(a, static_cast<double>(2));
      a += do_test_constexpr_mul_divide(a, static_cast<long double>(2));
#ifdef BOOST_HAS_FLOAT128
      a += do_test_constexpr_mul_divide(a, static_cast<__float128>(2));
#endif
    }
   return a;
}

template <class T, class U>
BOOST_CXX14_CONSTEXPR T do_test_constexpr_bitwise(T a, U b)
{
   a |= b;
   a &= b;
   a <<= 2;
   a ^= b;
   a = a | b;
   a = a & b;
   a <<= 2;
   a = a ^ b;
   if constexpr (std::numeric_limits<T>::is_signed == false)
   {
      a = ~a;
      a >>= std::numeric_limits<T>::digits - 3;
   }

   a <<= 5;
   a = a << 2;
   a >>= 5;
   a = a >> 2;

   return a;
}

template <class T>
BOOST_CXX14_CONSTEXPR T test_constexpr_bitwise(T a)
{
   a += do_test_constexpr_bitwise(a, a);
   a += do_test_constexpr_bitwise(a, static_cast<unsigned char>(2));
   a += do_test_constexpr_bitwise(a, static_cast<signed char>(2));
   a += do_test_constexpr_bitwise(a, static_cast<char>(2));
   a += do_test_constexpr_bitwise(a, static_cast<short>(2));
   a += do_test_constexpr_bitwise(a, static_cast<unsigned short>(2));
   a += do_test_constexpr_bitwise(a, static_cast<int>(2));
   a += do_test_constexpr_bitwise(a, static_cast<unsigned int>(2));
   a += do_test_constexpr_bitwise(a, static_cast<long>(2));
   a += do_test_constexpr_bitwise(a, static_cast<unsigned long>(2));
   a += do_test_constexpr_bitwise(a, static_cast<long long>(2));
   a += do_test_constexpr_bitwise(a, static_cast<unsigned long long>(2));
#ifdef BOOST_HAS_INT128
   a += do_test_constexpr_bitwise(a, static_cast<__int128>(2));
   a += do_test_constexpr_bitwise(a, static_cast<unsigned __int128>(2));
#endif

   return a;
}

template <class T, class U>
BOOST_CXX14_CONSTEXPR T do_test_constexpr_logical(T a, U b)
{
   T result(0);
   if(a || b)
      ++result;
   if(b || a)
      ++result;
   if(a && b)
      ++result;
   if(b && a)
      ++result;
   if(a)
      ++result;
   if(!a)
      ++result;
   return result;
}

template <class T>
BOOST_CXX14_CONSTEXPR T test_constexpr_logical(T a)
{
   a += do_test_constexpr_logical(a, a);
   a += do_test_constexpr_logical(a, static_cast<unsigned char>(2));
   a += do_test_constexpr_logical(a, static_cast<signed char>(2));
   a += do_test_constexpr_logical(a, static_cast<char>(2));
   a += do_test_constexpr_logical(a, static_cast<short>(2));
   a += do_test_constexpr_logical(a, static_cast<unsigned short>(2));
   a += do_test_constexpr_logical(a, static_cast<int>(2));
   a += do_test_constexpr_logical(a, static_cast<unsigned int>(2));
   a += do_test_constexpr_logical(a, static_cast<long>(2));
   a += do_test_constexpr_logical(a, static_cast<unsigned long>(2));
   a += do_test_constexpr_logical(a, static_cast<long long>(2));
   a += do_test_constexpr_logical(a, static_cast<unsigned long long>(2));
#ifdef BOOST_HAS_INT128
   a += do_test_constexpr_logical(a, static_cast<__int128>(2));
   a += do_test_constexpr_logical(a, static_cast<unsigned __int128>(2));
   a -= do_test_constexpr_logical(a, static_cast<__int128>(2));
   a -= do_test_constexpr_logical(a, static_cast<unsigned __int128>(2));
#endif

   return a;
}

template <class T, class U>
BOOST_CXX14_CONSTEXPR T do_test_constexpr_compare(T a, U b)
{
   T result(0);
   if(a == b)
      ++result;
   if(b == a)
      ++result;
   if(a != b)
      ++result;
   if(b != a)
      ++result;
   if(a < b)
      ++result;
   if(b < a)
      ++result;
   if(a <= b)
      ++result;
   if(b <= a)
      ++result;
   if(a > b)
      ++result;
   if(b > a)
      ++result;
   if(a >= b)
      ++result;
   if(b >= a)
      ++result;

   T u(b);
   if(u == a)
      ++result;

   return result;
}

template <class T>
BOOST_CXX14_CONSTEXPR T test_constexpr_compare(T a)
{
   a += do_test_constexpr_compare(a, a);
   a += do_test_constexpr_compare(a, static_cast<unsigned char>(2));
   a += do_test_constexpr_compare(a, static_cast<signed char>(2));
   a += do_test_constexpr_compare(a, static_cast<char>(2));
   a += do_test_constexpr_compare(a, static_cast<short>(2));
   a += do_test_constexpr_compare(a, static_cast<unsigned short>(2));
   a += do_test_constexpr_compare(a, static_cast<int>(2));
   a += do_test_constexpr_compare(a, static_cast<unsigned int>(2));
   a += do_test_constexpr_compare(a, static_cast<long>(2));
   a += do_test_constexpr_compare(a, static_cast<unsigned long>(2));
   a += do_test_constexpr_compare(a, static_cast<long long>(2));
   a += do_test_constexpr_compare(a, static_cast<unsigned long long>(2));
#ifdef BOOST_HAS_INT128
   a += do_test_constexpr_compare(a, static_cast<__int128>(2));
   a += do_test_constexpr_compare(a, static_cast<unsigned __int128>(2));
   a -= do_test_constexpr_compare(a, static_cast<__int128>(2));
   a -= do_test_constexpr_compare(a, static_cast<unsigned __int128>(2));
#endif

   if constexpr (boost::multiprecision::number_category<T>::value == boost::multiprecision::number_kind_floating_point)
   {
      a += do_test_constexpr_compare(a, static_cast<float>(2));
      a += do_test_constexpr_compare(a, static_cast<double>(2));
      a += do_test_constexpr_compare(a, static_cast<long double>(2));
#ifdef BOOST_HAS_FLOAT128
      a += do_test_constexpr_compare(a, static_cast<__float128>(2));
#endif
    }
   return a;
}

// clang-format on


/*
Copyright (C) 2019 Quaternion Risk Management Ltd
All rights reserved.

This file is part of ORE, a free-software/open-source library
for transparent pricing and risk analysis - http://opensourcerisk.org

ORE is free software: you can redistribute it and/or modify it
under the terms of the Modified BSD License.  You should have received a
copy of the license along with this program.
The license is also available online at <http://opensourcerisk.org>

This program is distributed on the basis that it will form a useful
contribution to risk analytics and model standardisation, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/dynamicyoyoptionletvolatilitystructure.hpp
\brief dynamic yoy inflation optionlet volatility structure
\ingroup termstructures
*/

#pragma once

#include <boost/make_shared.hpp>
#include <ql/termstructures/volatility/inflation/yoyinflationoptionletvolatilitystructure.hpp>
#include <ql/termstructures/volatility/smilesection.hpp>
#include <qle/termstructures/dynamicstype.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Converts YoYOptionletVolatilitySurface with fixed reference date into a floating reference date term structure.
/*! Different ways of reacting to time decay can be specified.

\warning No checks are performed that the supplied YoYOptionletVolatilitySurface has a fixed reference date

\ingroup termstructures
*/

class DynamicYoYOptionletVolatilitySurface : public YoYOptionletVolatilitySurface {
public:
    DynamicYoYOptionletVolatilitySurface(const boost::shared_ptr<YoYOptionletVolatilitySurface>& source,
                                         ReactionToTimeDecay decayMode = ConstantVariance);

protected:
    Volatility volatilityImpl(Time optionTime, Rate strike) const override;
    Rate minStrike() const override;
    Rate maxStrike() const override;
    Date maxDate() const override;

private:
    const boost::shared_ptr<YoYOptionletVolatilitySurface> source_;
    ReactionToTimeDecay decayMode_;
    const Date originalReferenceDate_;
};

} // namespace QuantExt


// Lidar Pass trough filter
// TODO: get the urdf model from LCM:

#include <stdio.h>
#include <inttypes.h>
#include <iostream>
#include <Eigen/Dense>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/vector.hpp>

#include <lcm/lcm-cpp.hpp>

#include <bot_frames/bot_frames.h>
#include <bot_param/param_client.h>
#include <bot_frames_cpp/bot_frames_cpp.hpp>
#include <bot_lcmgl_client/lcmgl.h>

#include <pointcloud_tools/pointcloud_vis.hpp> // visualize pt clds
#include <pointcloud_tools/pointcloud_lcm.hpp> // unpack lidar to xyz
#include <lcmtypes/drc_lcmtypes.hpp>
#include "lcmtypes/bot_core.hpp"
#include <ConciseArgs>

using namespace std;
using namespace Eigen;
using namespace boost::assign; // bring 'operator+()' into scope

class Pass{
  public:
    Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_);

    ~Pass(){
    }
  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    bool verbose_;

    void affordanceCollectionHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::affordance_collection_t* msg);
    void headPoseHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg);

    BotParam* botparam_;
    BotFrames* botframes_;
    bot::frames* frames_cpp_;
    bot_lcmgl_t* lcmgl_;

    Isometry3dTime current_poseT_;

    pointcloud_vis* pc_vis_;
    pointcloud_lcm* pc_lcm_;
    int vis_counter_; // used for visualization

};

Pass::Pass(boost::shared_ptr<lcm::LCM> &lcm_, bool verbose_):
    lcm_(lcm_), verbose_(verbose_),
    current_poseT_(0, Eigen::Isometry3d::Identity()){
  botparam_ = bot_param_new_from_server(lcm_->getUnderlyingLCM(), 0);
  botframes_= bot_frames_get_global(lcm_->getUnderlyingLCM(), botparam_);

  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "lidar-pt");
  lcm_->subscribe( "AFFORDANCE_COLLECTION" ,&Pass::affordanceCollectionHandler,this);
  lcm_->subscribe("POSE_HEAD",&Pass::headPoseHandler,this);

  // Vis Config:
  pc_vis_ = new pointcloud_vis( lcm_->getUnderlyingLCM() );
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Null - World",5,1) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Affordance - World"           ,1,1, 1000,1, { 0.0, 1.0, 0.0} ));

  pc_vis_->obj_cfg_list.push_back( obj_cfg(1002,"Head - World",5,1) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1003,"Affordance - Head"           ,1,1, 1002,1, { 0.0, 1.0, 0.0} ));

  vis_counter_ =0;
  cout << "Finished setting up\n";
}

// Get look angles to relative point
// Yaw: positive to the left
// Pitch: positive up
void getLookAngles(Eigen::Vector3d rel_point, double &yaw, double &pitch){
  yaw  = atan2( rel_point(1) , rel_point(0) );
  pitch= atan2( rel_point(2) , rel_point(0) );
}

void Pass::affordanceCollectionHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::affordance_collection_t* msg){
  cout << "Affordances: "  << " "  << msg->naffs << "\n";

  int id=1;

  int64_t utime = current_poseT_.utime;

  /// Visualise incoming point in world frame:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::PointXYZRGB pt_in;
  pt_in.x = msg->affs[id].params[0];
  pt_in.y = msg->affs[id].params[1];
  pt_in.z = msg->affs[id].params[2];
  scan_cloud->points.push_back(pt_in);
  Eigen::Isometry3d null_pose;
  null_pose.setIdentity();
  Isometry3dTime null_poseT = Isometry3dTime(utime, null_pose);
  pc_vis_->pose_to_lcm_from_list(1000, null_poseT);
  pc_vis_->ptcld_to_lcm_from_list(1001, *scan_cloud, utime, utime);

  /// Find the position of the affordance relative to the sensor head:
  /// The only interesting thing:
  Vector3d point(msg->affs[id].params[0], msg->affs[id].params[1], msg->affs[id].params[2]) ;
  Vector3d rel_point = current_poseT_.pose.inverse() *point;
  double yaw,pitch;
  getLookAngles(rel_point,yaw,pitch);
  std::cout << "Affordance[0] " << msg->affs[id].name << " is: \n";
  std::cout << "pitch: " << pitch << " and yaw " << yaw << "\n";

  /// Visualize outgoing point in pose frame:
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_local (new pcl::PointCloud<pcl::PointXYZRGB> ());
  pcl::PointXYZRGB pt_out;
  pt_out.x = rel_point(0);
  pt_out.y = rel_point(1);
  pt_out.z = rel_point(2);
  cloud_local->points.push_back(pt_out);
  pc_vis_->pose_to_lcm_from_list(1002, current_poseT_);
  pc_vis_->ptcld_to_lcm_from_list(1003, *cloud_local, utime, utime);
}

void Pass::headPoseHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::pose_t* msg){
  current_poseT_.pose.setIdentity();
  current_poseT_.pose.translation() << msg->pos[0], msg->pos[1], msg->pos[2];
  Eigen::Quaterniond m;
  m  = Eigen::Quaterniond(msg->orientation[0],msg->orientation[1],msg->orientation[2],msg->orientation[3]);
  current_poseT_.pose.rotate(m);
  current_poseT_.utime = msg->utime;

}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "lidar-passthrough");
  bool verbose=FALSE;
  parser.add(verbose, "v", "verbose", "Verbosity");
  parser.parse();
  cout << verbose << " is verbose\n";

  boost::shared_ptr<lcm::LCM> lcm(new lcm::LCM);
  if(!lcm->good()){
    std::cerr <<"ERROR: lcm is not good()" <<std::endl;
  }

  Pass app(lcm,verbose);
  cout << "Ready to find " << endl << "============================" << endl;
  while(0 == lcm->handle());
  return 0;
}


/*
 * STLParser.cpp
 *
 *  Created on: 30.11.2015
 *      Author: lutz
 */

#include "STLParser.h"
#include <iostream>
#include <fstream>
#include <string>

#include <armadillo>
#include <cassert>
#include <array>

namespace kinematicTree
{
namespace visual
{
namespace stl
{

STLParser::STLParser()
{
}

STLParser::~STLParser()
{
}

// helper parsers
namespace {
	arma::colvec3 parseVector(std::ifstream& fileStream) {
		arma::colvec3 vector;
		int i(0);
		while ((not fileStream.eof()) && (i < 3)) {
			double val;
			fileStream >> val;
			vector(i) = val;
			++i;
		}
		return vector;
	}

	std::string readWord(std::ifstream& fileStream) {
		std::string word;
		fileStream >> word;
		return word;
	}

	void parseWord(std::ifstream& fileStream, std::string const& word) {
		std::string w;
		fileStream >> w;
		if (w != word) {
			throw (w == word);
		}
	}

	bool tryParseWord(std::ifstream& fileStream, std::string const& word) {
		std::string w;
		int pos = fileStream.tellg();
		fileStream >> w;

		const bool match = w == word;
		if (not match) {
			fileStream.seekg(pos);
		}

		return match;
	}

	bool parseVertex(std::ifstream& fileStream, arma::colvec3& vertex) {
		if (tryParseWord(fileStream, "vertex")) {
			vertex = parseVector(fileStream);
			return true;
		}
		return false;
	}

	bool parseFacetASCII(std::ifstream& fileStream, kinematicTree::visual::mesh::Facet& facet) {
		if (tryParseWord(fileStream, "facet")) {
			parseWord(fileStream, "normal");
			facet.mNormal = parseVector(fileStream);
			parseWord(fileStream, "outer");
			parseWord(fileStream, "loop");

			arma::colvec3 vertex;
			while (parseVertex(fileStream, vertex)) {
				facet.mVertices.push_back(vertex);
			}
			assert(facet.mVertices.size() == 3);
			parseWord(fileStream, "endloop");
			parseWord(fileStream, "endfacet");
			return true;
		} else {
			return false;
		}
	}

	kinematicTree::visual::mesh::Facet parseFacetBinary(std::ifstream& fileStream) {
		kinematicTree::visual::mesh::Facet ret;
		std::array<float, 3> normalVec;
		std::array<float, 9> vertices;
		uint16_t attrs;

		fileStream.read((char*)normalVec.data(), sizeof(normalVec));
		fileStream.read((char*)vertices.data(), sizeof(vertices));
		fileStream.read((char*)&attrs, sizeof(attrs));

		for (uint i(0); i < 3; ++i) {
			ret.mNormal(i) = normalVec[i];
			arma::colvec3 vertex;
			for (uint j(0); j < 3; ++j) {
				vertex(j) = vertices[3 * i + j];
			}
			ret.mVertices.push_back(vertex);
		}

		return ret;
	}

	kinematicTree::visual::mesh::Mesh parseASCII(std::ifstream& fileStream) {
		kinematicTree::visual::mesh::Mesh mesh;
		mesh.setName(readWord(fileStream)); // the next string is a dummy string
		// parse the entire solid
		while (1) {
			kinematicTree::visual::mesh::Facet facet;
			if (parseFacetASCII(fileStream, facet)) {
				mesh.addFacet(facet);
			} else {
				break;
			}
		}
		parseWord(fileStream, "endsolid");
		parseWord(fileStream, mesh.getName());
		return mesh;
	}

	kinematicTree::visual::mesh::Mesh parseBinary(std::ifstream& fileStream) {
		kinematicTree::visual::mesh::Mesh mesh;
		mesh.setName(""); // the next string is a dummy string
		char header[80];
		fileStream.read(header, sizeof(header));
		uint32_t numTriangles;
		fileStream.read((char*)&numTriangles, sizeof(numTriangles));

		std::streampos curPos = fileStream.tellg();
		fileStream.seekg( 0, std::ios::end );
		std::streampos endPos = fileStream.tellg();
		int expectedBytes = numTriangles * (12 * sizeof(float) + 2);
		assert(endPos - curPos == expectedBytes);
		fileStream.seekg(curPos);
		for (uint i(0); i < numTriangles; ++i) {
			mesh.addFacet(parseFacetBinary(fileStream));
		}
		return mesh;
	}
}

kinematicTree::visual::mesh::Mesh STLParser::parse(std::string const& file)
{
	std::ifstream stlFile(file);
	kinematicTree::visual::mesh::Mesh mesh;
	if (not stlFile.eof()) {
		if (tryParseWord(stlFile, "solid")) {
			try {
				mesh = parseASCII(stlFile);
			} catch (...) {
				stlFile.seekg(0);
				mesh = parseBinary(stlFile);
			}
		} else {
			mesh =parseBinary(stlFile);
		}
	}
	return mesh;
}

void STLParser::dump(kinematicTree::visual::mesh::Mesh const& mesh, std::string const& file) {
	std::ofstream fileStream(file);
	char header[80] = {};
	fileStream.write(header, sizeof(header));
	uint32_t numTriangles = mesh.getFacets().size();
	fileStream.write((char const*)&numTriangles, sizeof(numTriangles));

	for (auto const& facet : mesh.getFacets()) {
		uint16_t attrs{};

		for (uint i(0); i < 3; ++i) {
			float f = facet.mNormal(i);
			fileStream.write((char const*)&f, sizeof(f));
		}

		for (uint i(0); i < 3; ++i) {
			for (uint j(0); j < 3; ++j) {
				float f = facet.mVertices[i](j);
				fileStream.write((char const*)&f, sizeof(f));
			}
		}
		fileStream.write((char const*)&attrs, sizeof(attrs));
	}
}

}
}
}