// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#ifdef _MSC_VER
#pragma warning(disable:4244)
#pragma warning(disable:4267)
#pragma warning(disable:4996)
#endif

#include <iostream>
#include <fstream>
#include <string>
#include <iomanip>

#include <boost/program_options.hpp>
namespace po = boost::program_options;

#include <vw/Image/ImageView.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Image/Filter.h>
#include <vw/FileIO/DiskImageResource.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/Cartography/GeoReference.h>

using namespace vw;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
  template<> struct PixelFormatID<Vector3f>  { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

/// \file maskFromIntersectError.cc Generates a good pixel mask of all pixels with
///                                 intersection error below a threshold.

/// Functor to evaluate if the sum squared value of a pixel is below a threshold
/// - In this program it is evaluating intersection error and generating a mask.
class SumSquaredThreshFunctor  : public ReturnFixedType<PixelGray<uint8> >
{
public:
  /// Construct with the thresholds.
  SumSquaredThreshFunctor(const std::vector<float> &thresholdValues, bool scale=false) : m_scaleVal(1.0)
  {
    // Record squared copies of each threshold value
    m_ValsSquared.resize(thresholdValues.size());
    for (size_t i=0; i<m_ValsSquared.size(); ++i)
      m_ValsSquared[i] = thresholdValues[i]*thresholdValues[i];

    // Compute scale value
    // - Number of output levels = 1 + number of thresholds
    // - Outputs will be 0, 1*m_scaleVal, 2*m_scaleVal, ...
    if (scale)
      m_scaleVal = 255.0f / float(thresholdValues.size() + 1); // Add one since the highest level is reserved for NODATA
  }

  float getScaleValue() const { return m_scaleVal; }

  /// Assign each pixel a value based on the threshold values.
  inline PixelGray<uint8> operator() (PixelMask<PixelRGB<float> > const &p) const
  {
    // The color 255 is reserved for invalid pixels
    if (!is_valid(p))
      return PixelGray<uint8>(255);

    float valueSquared = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
    uint8 bin = 0;
    for (size_t i=0; i<m_ValsSquared.size(); ++i)
    {
      if (valueSquared >= m_ValsSquared[i]) // Value exceeds this threshold
        bin = i+1;  // Set it to threshold index
      else  // Get out of the loop
        break;
    }
    // Now apply the scale value to the output
    return PixelGray<uint8>(bin*m_scaleVal);
  }

private:
  std::vector<float> m_ValsSquared;
  float m_scaleVal;
};

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

  std::string inputImagePath="", outputImagePath="", legendPath="";

  std::vector<float> thresholdValues;
  bool scaleOutput;
  po::options_description general_options("Options");
  general_options.add_options()
    ("help,h",         "Display this help message")
    ("scaleOutput,s",  po::bool_switch(&scaleOutput)->default_value(false),    "Scale output up to 255")
    ("legend,l",       po::value<std::string>(&legendPath)->default_value(""), "Path to save legend text file to")
    ("thresholds",     po::value<std::vector<float> >(&thresholdValues)->multitoken(),
                         "Set one or more pixel threshold values (must be >0 and increasing value)");

  po::options_description positional("");
  positional.add_options()
    ("input-image",    po::value(&inputImagePath),  "Path to input  image file")
    ("output-image",   po::value(&outputImagePath), "Path to output image file");

  po::positional_options_description positional_desc;
  positional_desc.add("input-image",  1);
  positional_desc.add("output-image", 1);

  std::string usage("[options] <input-image> <output-image>\n");
  po::variables_map vm;
  try {
    po::options_description all_options;
    all_options.add(general_options).add(positional);

    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).style( po::command_line_style::unix_style ).run(), vm );

    po::notify( vm );
  } catch (po::error const& e) {
    vw::vw_throw( vw::ArgumentErr() << "Error parsing input:\n"
                  << e.what() << "\n" << usage << general_options );
  }

  if ( !vm.count("input-image") || !vm.count("output-image") || !vm.count("thresholds"))
    vw_throw( vw::ArgumentErr() << "Requires <input-image>, <output-image>, and threshold inputs in order to proceed.\n\n"
              << usage << general_options );

  // Verify that the threshold values are sorted and > 0
  float lastVal = 0;
  for (size_t i=0; i<thresholdValues.size(); ++i)
  {
    if (thresholdValues[i] <= lastVal)
      vw_throw( vw::ArgumentErr() << "Threshold values must be greater than zero and in increasing order.\n\n");
    lastVal = thresholdValues[i];
  }

  try {

    //TODO: Check if the input image is one or three channels
    // Set up input from file
    vw::DiskImageView<PixelRGB<float> > inputImage(inputImagePath);
    //vw::DiskImageView<Vector3> inputImage(inputImagePath);

    // Attempt to extract nodata value
    float noDataValue = -1;
    SrcImageResource *imageResource = DiskImageResource::open(inputImagePath);
    if ( imageResource->has_nodata_read() ) {
      noDataValue = imageResource->nodata_read();
      std::cout << "\t--> Extracted nodata value from file: " << noDataValue << ".\n";
    }
    else
      vw_throw( vw::ArgumentErr() << "Failed to extract NODATA value!\n\n");

    // Set up thresholding function
    SumSquaredThreshFunctor threshFunctor(thresholdValues, scaleOutput);
    ImageViewRef<PixelGray<vw::uint8> > outputView = vw::per_pixel_filter(create_mask( inputImage, noDataValue),
                                                                          threshFunctor);

    // Set up output file
    boost::scoped_ptr<DiskImageResource> r(DiskImageResource::create(outputImagePath, outputView.format()));

    // Copy georeference information from the input image
    vw::cartography::GeoReference georef;
    vw::cartography::read_georeference(georef, inputImagePath);
    write_georeference( *r, georef );

//write_georeferenced_image -> look up
    // Do everything!
    vw::block_write_image( *r, outputView,
                          vw::TerminalProgressCallback( "maskFromIntersectError", "Generating mask:") );

    // Write out a legend text file if the user requested it
    if (legendPath.size() > 0) {
      std::ofstream legendFile(legendPath.c_str());
      legendFile << "Error threshold, Pixel value" << std::endl;
      legendFile << "0, 0" << std::endl;
      for (size_t i=0; i<thresholdValues.size(); ++i)
        legendFile << thresholdValues[i] << ", " << (int)((i+1) * threshFunctor.getScaleValue()) << std::endl;
      legendFile << "NODATA, 255" << std::endl;
      legendFile.close();

      if (legendFile.fail()) // Handle errors writing the legend file
        vw_throw( vw::ArgumentErr() << "Error writing threshold values to location " << legendPath << "\n\n");
    }

  }
  catch (const Exception& e) {
    std::cerr << "Error: " << e.what() << std::endl;
  }

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_PREDECESSOR_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_PREDECESSOR_HPP_INCLUDED

#include <boost/simd/ieee/functions/predecessor.hpp>
#include <boost/simd/include/functions/scalar/bitfloating.hpp>
#include <boost/simd/include/functions/scalar/bitinteger.hpp>
#include <boost/simd/include/functions/scalar/minusone.hpp>
#include <boost/simd/include/functions/scalar/subs.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/simd/include/functions/scalar/is_gez.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)
                                    , (scalar_< arithmetic_<A0> >)
                                      (scalar_< integer_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      if (Valmin<A0>()+a1 > a0) return Valmin<A0>();
      return a0-a1;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)
                                    , (scalar_< arithmetic_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return minusone(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)(A1)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< integer_<A1> >)
                                    )
  {

    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(2)
    {
      typedef typename dispatch::meta::as_integer<A0>::type itype;
      BOOST_ASSERT_MSG(is_gez(a1), "predecessor rank must be non negative");
      if (is_nan(a0)) return a0;
      return bitfloating(subs(bitinteger(a0), itype(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( predecessor_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                    )
  {

    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      if (is_nan(a0)) return a0;
      return bitfloating(minusone(bitinteger(a0)));
    }
  };
} } }

#endif


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PipelineRecomputeIrTest2

#include <memory>

#include "pipeline_recompute_string.hpp"
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/graph.hpp>
#include <popart/ir.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/ipucopy.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/nll.hpp>
#include <popart/op/restore.hpp>
#include <popart/op/stash.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

BOOST_AUTO_TEST_CASE(PipelineRecomputeIrTest2) {

  bool withLogging = true;

  using namespace popart;

  enum class NlType { Sin, Sigmoid };

  auto test = [withLogging](NlType nlt, bool recomp) {
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();
    TensorInfo info{"FLOAT", std::vector<int64_t>{8, 4}};
    std::vector<float> wVals(4 * 8, 1.0f);
    ConstVoidData wData = {wVals.data(), info};

    auto input1 = builder->addInputTensor(info);
    auto w1     = builder->addInitializedInputTensor(wData);

    auto act = aiOnnx.add({input1, w1});

    auto getPipe = [nlt, &aiOnnx, &aiGraphcore](TensorId act, VGraphId vgid) {
      //    >-------- [8,4] ---------------------------------------
      //             /     \                                       |
      //      slice left  slice right                              |
      //          scale          scale                             |
      //          [6,4]            [6,4]--------                   |
      //             |                          \                  |
      //             |                         / \
      //            / \                       /   \                |
      //           /   \                     /     \
      //          /     \             slice left  slice right      |
      //   slice left  slice right         scale   scale
      //        scale   scale            [4,4]       [4,4]
      //      [4,4]       [4,4]             |          |           |
      //        |           |            postnl      postnl
      //      postnl      postnl            |        /
      //        \          /                |      /
      //         \        /                 |    /                 |
      //          \      /                 matmul
      //           matmul                   /
      //             |                  sigmoid
      //           sigmoid              /
      //              \                |                           |
      //                ------------- cat [8,4] ---------->       add
      //                                    |                      |
      //                                    |                    sigmoid
      //                                    |                      |
      //                                    ------------------->  add
      //                                                           |
      //                                                           -->
      //                where postnl is either sin or sigmoid
      // The difference between sin and sigmoid, is that the
      // gradients require the input and output, respectively

      auto postnl = [&aiOnnx, nlt](TensorId id) {
        if (nlt == NlType::Sigmoid) {
          return aiOnnx.sigmoid({id});
        } else {
          return aiOnnx.sin({id});
        }
      };
      (void)vgid;

      auto actIn = act;
      auto act0  = aiOnnx.slice({act}, {6, 4}, {0, 0}, {0, 1});
      act0       = aiGraphcore.scale({act0}, 0.6);

      auto act1 = aiOnnx.slice({act}, {8, 4}, {2, 0}, {0, 1});
      act1      = aiGraphcore.scale({act1}, 0.7);

      auto act00 = aiOnnx.slice({act0}, {4, 4}, {0, 0}, {0, 1});
      act00      = aiGraphcore.scale({act00}, 0.8);
      act00      = postnl(act00);

      auto act01 = aiOnnx.slice({act0}, {6, 4}, {2, 0}, {0, 1});
      act01      = aiGraphcore.scale({act01}, 0.9);
      act01      = postnl(act01);

      auto act10 = aiOnnx.slice({act1}, {4, 4}, {0, 0}, {0, 1});
      act10      = aiGraphcore.scale({act10}, 1.1);
      act10      = postnl(act10);

      auto act11 = aiOnnx.slice({act1}, {6, 4}, {2, 0}, {0, 1});
      act11      = aiGraphcore.scale({act11}, 1.2);
      act11      = postnl(act11);

      act0 = aiOnnx.matmul({act00, act01});
      act0 = aiOnnx.sigmoid({act0});

      act1 = aiOnnx.matmul({act10, act11});
      act1 = aiOnnx.sigmoid({act1});

      auto cat = aiOnnx.concat({act0, act1}, 0);
      act      = aiOnnx.add({cat, actIn});
      act      = aiOnnx.sigmoid({act});

      act = aiOnnx.add({act, cat});

      return act;
    };

    act = getPipe(act, 0);
    act = getPipe(act, 1);
    act = getPipe(act, 2);
    act = getPipe(act, 3);

    auto l1 = builder->aiGraphcoreOpset1().l1loss({act}, 0.1);

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);
    auto dataFlow   = DataFlow(100, {{act, AnchorReturnType("All")}});

    SessionOptions userOptions;
    userOptions.virtualGraphMode     = VirtualGraphMode::Auto;
    userOptions.enableOutlining      = false;
    userOptions.enablePipelining     = true;
    if (recomp) {
      userOptions.autoRecomputation = RecomputationType::Standard;
    }

    constexpr int64_t nIpus{4};

    std::map<std::string, std::string> deviceOpts{
        {"numIPUs", std::to_string(nIpus)}};

    auto optimizer = ConstSGD(0.01);

    auto device = createTestDevice(TEST_TARGET, nIpus);

    Patterns patterns(PatternsLevel::Default);
    patterns.enableMatMulOp(false);
    patterns.enableMatMulLhsGradOp(false);
    patterns.enableMatMulRhsGradOp(false);

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                l1,
                &optimizer,
                *device,
                userOptions,
                patterns});

    auto sched =
        ir.getMainGraph().getOpSchedule({}, RequireOptimalSchedule::Yes);

    std::vector<int64_t> stashIpus(nIpus, 0);
    for (auto op : sched) {
      if (dynamic_cast<StashOp *>(op)) {
        ++stashIpus[op->getVirtualGraphId()];
      }

      // Backwards pass Ops must not be Recompute
      if (op->fromLoss == PathFromLoss::Yes) {
        BOOST_CHECK(op->settings.recomputeType == RecomputeType::Checkpoint);
      }
    }

    for (auto ipu = 0; ipu < nIpus - 1; ++ipu) {
      if (nlt == NlType::Sigmoid && recomp == false) {
        BOOST_CHECK(stashIpus[ipu] == 7);
      }

      else if (nlt == NlType::Sigmoid && recomp == true) {
        // Two of the sigmoid outputs will be recomputed
        BOOST_CHECK(stashIpus[ipu] == 5);
      }
    }

    if (withLogging) {
      std::array<std::stringstream, nIpus> sss;
      pipeline_recompute_util::fillLogStreams(sss, sched);
      for (int64_t ipu = 0; ipu < nIpus; ++ipu) {
        std::cout << "On IPU " << ipu << std::endl;
        std::cout << sss[ipu].str() << "\n\n" << std::endl;
      }
    }
  };

  // Assumptions made in this test:
  // - The auto-sharder will put 1 of the above pipes on each IPU.
  // - Sin grad requires Sin's input and Sigmoid grad requires Sigmoid's output
  test(NlType::Sigmoid, true);
  test(NlType::Sigmoid, false);

  // testing with NlType::Sin assumes that the Cos for the backwards pass is
  // always scheduled in the forwards pass, which seems like a bad long-term
  // assumption. So not testing NlType::Sin
}


#include "simple.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "control/util.h"

namespace sailbot {
namespace control {

namespace {
  bool in_bounds(float alpha, float min, float max) {
    float diff = util::norm_angle(alpha - min);
    return diff > 0 && diff < util::norm_angle(max - min);
  }
}

SimpleControl::SimpleControl(bool do_rudder)
    : Node(0.01),
      do_rudder_(do_rudder),
      sail_msg_(AllocateMessage<msg::SailCmd>()),
      rudder_msg_(AllocateMessage<msg::RudderCmd>()),
      heel_msg_(AllocateMessage<msg::HeelCmd>()),
      rigid_msg_(AllocateMessage<msg::RigidWingCmd>()),
      boat_state_(AllocateMessage<msg::BoatState>()),
      consts_msg_(AllocateMessage<msg::ControllerConstants>()),
      heading_(0),
      sail_cmd_("sail_cmd", true),
      rudder_cmd_("rudder_cmd", true),
      heel_cmd_("heel_cmd", true),
      rigid_cmd_("rigid_wing_cmd", true),
      consts_queue_("control_consts", true) {

  consts_msg_->set_max_rudder(0.5);
  consts_msg_->set_rudder_kp(0.5);
  consts_msg_->set_rudder_ki(0.02);
  consts_msg_->set_winch_kp(13);
  consts_msg_->set_sail_heel_k(0);

  consts_msg_->set_nominal_heel(0.0);

  consts_msg_->set_rigid_port_servo_pos(15);
  consts_msg_->set_rigid_starboard_servo_pos(85);
  {
    std::unique_lock<std::mutex> l(consts_mutex_);
    consts_queue_.send(consts_msg_);
  }

  rigid_msg_->set_state(msg::RigidWingCmd_WingState_MANUAL);
  rigid_msg_->set_heel(0);
  rigid_msg_->set_max_heel(1);
  RegisterHandler<msg::BoatState>("boat_state", [this](const msg::BoatState &msg) {
    std::unique_lock<std::mutex> l(boat_state_mutex_);
    *boat_state_ = msg;
  });
  RegisterHandler<msg::ControllerConstants>(
      "control_consts", [this](const msg::ControllerConstants &msg) {
        if (msg.has_max_rudder() && msg.has_rudder_kp() && msg.has_winch_kp() &&
            msg.has_sail_heel_k() && msg.has_rigid_port_servo_pos() &&
            msg.has_rigid_starboard_servo_pos() && msg.has_nominal_heel()) {
          std::unique_lock<std::mutex> l(consts_mutex_);
          *consts_msg_ = msg;
        }
      });
  RegisterHandler<msg::Vector3f>("wind", [this](const msg::Vector3f &msg) {
    wind_x_ = msg.x();
    wind_y_ = msg.y();
  });
  RegisterHandler<msg::HeadingCmd>("heading_cmd", [this](const msg::HeadingCmd &msg) {
    if (msg.has_heading()) {
      heading_ = msg.heading();
    }
    if (msg.has_extra_sail()) {
      extra_sail_ = msg.extra_sail();
    }
    rudder_integrator_ = 0.0;
    VLOG(2) << "Got heading cmd: " << heading_.load();
  });
  RegisterHandler<msg::ControlMode>("control_mode",
                                    [this](const msg::ControlMode &mode) {
    if (mode.has_rigid_mode()) {
      switch (mode.rigid_mode()) {
      case msg::ControlMode::AUTO:
        auto_rigid_wing_ = true;
        break;
      default:
        auto_rigid_wing_ = false;
        break;
      }
    }
  });
  RegisterHandler<msg::SBUS>("sbus_value", [this](const msg::SBUS &sbus) {
    // TODO(james): Have separate mutex or atomic variable just for rigid wing.
    std::unique_lock<std::mutex> l(boat_state_mutex_);
    rigid_msg_->set_servo_pos(sbus.channel(1) * 100. / 2050.);
  });
}

void SimpleControl::Iterate() {
  ++counter_;
  std::unique_lock<std::mutex> l(boat_state_mutex_);
  std::unique_lock<std::mutex> lc(consts_mutex_);

  // Set angle of attack to be ~.4
  float vx = boat_state_->vel().x();
  float vy = boat_state_->vel().y();
  float yaw = boat_state_->euler().yaw();
  float heel = boat_state_->euler().roll();
  float heel_rate = boat_state_->omega().x();
  VLOG(2) << "yaw: " << yaw << " heel: " << heel;

  float goal_heading = heading_;
  float cur_heading = yaw; // vel > 0.1 ? std::atan2(vy, vx) : yaw;
  double heading_err = util::norm_angle(goal_heading - cur_heading);

  // For alphaw, if sailing straight into wind, =0, if wind is coming from port
  // (ie, on port tack), alphaw is positive, if coming from starboard, is
  // negative/between pi and 2pi.
//  float alphaw = util::norm_angle(std::atan2(vy - wind_y_, vx - wind_x_) - yaw);
  float alphaw = util::norm_angle(std::atan2(wind_y_, -wind_x_));
  float wind_source_dir = std::atan2(-wind_y_, -wind_x_);
  //float alphasail = util::norm_angle(alphaw - boat_state_->internal().sail());
  float cursail = boat_state_->internal().sail();
  double turning_sail =
      (alphaw > 0.0 ? 1.0 : -1.0) * 0.9 * util::Clip(heading_err, -0.5, 0.5);
  // If we are on port tack, want alphasail > 0, if on starboard, alphasail < 0.
  float goal = std::min(std::max(std::abs(alphaw)- 0.8, 0.), 1.5) +
               consts_msg_->sail_heel_k() * std::abs(heel)
               + extra_sail_
               + turning_sail;
  VLOG(2) << "Alphaw: " << alphaw << " alphas: " << cursail
          << " goals: " << goal;
  // TODO(james): Temporary for testing:
  //goal = std::abs(goal_heading);
  //alphasail = boat_state_->internal().sail();
  float sail_err = util::norm_angle(goal - cursail);
  sail_msg_->set_voltage(consts_msg_->winch_kp() * sail_err /
                         std::sqrt(std::abs(sail_err)));
  sail_msg_->set_pos(goal);
  if (auto_rigid_wing_) {
    rigid_msg_->set_servo_pos(wind_source_dir > 0
                                  ? consts_msg_->rigid_port_servo_pos()
                                  : consts_msg_->rigid_starboard_servo_pos());
  }

  float vel = std::sqrt(vx * vx + vy * vy);
  double max_rudder =
      vel < 0 ? 0.3 : (vel < 1.0 ? consts_msg_->max_rudder()
                                 : 0.8 * consts_msg_->max_rudder());
  //float boat_heading = std::atan2(vy, vx);
  rudder_integrator_ =
      util::Clip(rudder_integrator_ + heading_err * dt, -1.5, 1.5);
  double goal_rudder =
      -util::Clip(consts_msg_->rudder_kp() * heading_err +
                      consts_msg_->rudder_ki() * rudder_integrator_,
                  -max_rudder, max_rudder);
  VLOG(2) << "goalh: " << goal_heading << " goal_rudder: " << goal_rudder;
  rudder_msg_->set_pos(goal_rudder);
  rudder_msg_->set_vel(1. * (goal_rudder - boat_state_->internal().rudder()));
  sail_cmd_.send(sail_msg_);
  if (auto_rigid_wing_ && (counter_ % int(.1 / dt)) == 0)
    rigid_cmd_.send(rigid_msg_);
  if (do_rudder_) {
    rudder_cmd_.send(rudder_msg_);
  }
  if (std::abs(alphaw) > 1.4) {
    // Don't send new heel command when we are sailing pretty much downwind;
    // prevents oscillations which some people don't seem to like.
    double heel_goal = consts_msg_->nominal_heel() * util::Sign(wind_source_dir) +
                       0 * util::Clip(1.0 * heading_err, -0.25, 0.25);
    heel_msg_->set_heel(heel_goal);
    heel_cmd_.send(heel_msg_);
  }
  if (counter_ % 100 == 0) {
    consts_queue_.send(consts_msg_);
  }
}

}  // control
}  // sailbot


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_COMPARE_LESS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_COMPARE_LESS_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
    Function object implementing compare_less capabilities

    Returns a logical scalar that is the result of the lexicographic
    test for < on all elements of the entries

    It is probably not what you wish. Have a look to @ref is_less

    @par Semantic:

    For every parameters of type T0:

    @code
    bool r = compare_less(x,y);
    @endcode

    is similar to:

    @code
    bool r = false;
    for(std::size_t i=0;i<cardinal_of<A0>;++i)
    {
      if (x[i] <  y[i])  {r =  true; break;}
      if (y[i] <  x[i])  {r = false; break;}
    }
    @endcode

    @see is_less
  **/
    bool compare_less(Value const& v0, Value const& v1);
} }
#endif

#include <boost/simd/function/scalar/compare_less.hpp>
#include <boost/simd/function/simd/compare_less.hpp>

#endif


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2017.
// Modifications copyright (c) 2017 Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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_TEST_DISJOINT_HPP
#define BOOST_GEOMETRY_TEST_DISJOINT_HPP

#include <iostream>
#include <string>
#include <boost/variant/variant.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/disjoint.hpp>
#include <boost/geometry/io/wkt/read.hpp>

struct no_strategy {};

template <typename Geometry1, typename Geometry2, typename Strategy>
bool call_disjoint(Geometry1 const& geometry1,
                   Geometry2 const& geometry2,
                   Strategy const& strategy)
{
    return bg::disjoint(geometry1, geometry2, strategy);
}

template <typename Geometry1, typename Geometry2>
bool call_disjoint(Geometry1 const& geometry1,
                   Geometry2 const& geometry2,
                   no_strategy)
{
    return bg::disjoint(geometry1, geometry2);
}

template <typename G1, typename G2, typename Strategy>
void check_disjoint(std::string const& id,
                    std::string const& wkt1,
                    std::string const& wkt2,
                    G1 const& g1,
                    G2 const& g2,
                    bool expected,
                    Strategy const& strategy)
{
    bool detected = call_disjoint(g1, g2, strategy);
    if (id.empty())
    {
        BOOST_CHECK_MESSAGE(detected == expected,
            "disjoint: " << wkt1 << " and " << wkt2
            << " -> Expected: " << expected
            << " detected: " << detected);
    }
    else
    {
        BOOST_CHECK_MESSAGE(detected == expected,
            "disjoint: " << id
            << " -> Expected: " << expected
            << " detected: " << detected);
    }
}

template <typename G1, typename G2>
void test_disjoint(std::string const& id,
                   std::string const& wkt1,
                   std::string const& wkt2,
                   bool expected)
{
    G1 g1;
    bg::read_wkt(wkt1, g1);

    G2 g2;
    bg::read_wkt(wkt2, g2);

    boost::variant<G1> v1(g1);
    boost::variant<G2> v2(g2);

    typedef typename bg::strategy::disjoint::services::default_strategy
        <
            G1, G2
        >::type strategy_type;

    check_disjoint(id, wkt1, wkt2, g1, g2, expected, no_strategy());
    check_disjoint(id, wkt1, wkt2, g1, g2, expected, strategy_type());
    check_disjoint(id, wkt1, wkt2, g1, g2, expected, no_strategy());
    check_disjoint(id, wkt1, wkt2, v1, g2, expected, no_strategy());
    check_disjoint(id, wkt1, wkt2, g1, v2, expected, no_strategy());
    check_disjoint(id, wkt1, wkt2, v1, v2, expected, no_strategy());
}

template <typename G1, typename G2>
void test_geometry(std::string const& wkt1,
                   std::string const& wkt2,
                   bool expected)
{
    test_disjoint<G1, G2>("", wkt1, wkt2, expected);
}

#endif // BOOST_GEOMETRY_TEST_DISJOINT_HPP


#include <boost/numeric/ublas/functional.hpp>


/* Copyright (C) 2010 Ion Torrent Systems, Inc. All Rights Reserved */
#include <armadillo>
#include <iostream>
#include <vector>
#include <string>

#include "IonH5Arma.h"
#include "Mask.h"
#include "Utils.h"
#include "NumericalComparison.h"

using namespace std;
using namespace arma;
/**
 * Some stats about mask comparisons
 */
class MaskDiff {
public:

  /** Basic constructor. */
  MaskDiff() {
    Init(MaskNone, "MaskNone");
  }

  /** Constructor for type of mask entry. */
  MaskDiff(uint16_t type, const std::string &n) {
    Init(type,n);
  }

  /** Initialize with a type of mask entry. */
  void Init(uint16_t type, const std::string &n) {
    entry = type;
    diff = 0;
    added = 0;
    deleted = 0;
    seen = 0;
    same = 0;
    name = n;
  }

  /** Compare two entries of mask types. */
  void Compare(uint16_t r1, uint16_t r2) {
    seen++;
    if (r1 == r2) {
      same++;
    }
    else {
      diff++;
      if (r1 >= 1) {
	added++;
      }
      else {
	deleted++;
      }
    }
  }

  void Out(std::ostream &out, float tag_percent=3.0f) {
    char buff[256];
    size_t diff = seen - same;
    char tag = ' ';
    if ((added*100.0/seen + deleted*100.0/seen)/2.0f >= tag_percent) {
      tag = '*';
    }
    snprintf(buff, sizeof(buff), "%-12s\t%8u (%6.2f%%)\t%8u (%6.2f%%)\t%8u (%6.2f%%)%c", name.c_str(),
	     (uint32_t)diff, (diff*100.0)/seen,
	     (uint32_t)added, (added*100.0)/seen,
	     (uint32_t)deleted, (deleted*100.0)/seen, tag);
    out << buff << endl;
  }

  /** Are the two sets of entries the same? */
  bool IsSame() { return diff == 0; }

  std::string name;
  size_t diff;
  size_t same;
  uint16_t entry;
  size_t added;
  size_t deleted;
  size_t seen;

};

/** Everbody's favorite function. */
int main(int argc, const char *argv[]) {
  if (argc != 5) {
    cout << "Wrong number of arguments.\nusage:\n\tCompareBf mask1.bin mask2.bin sep1.h5 sep2.h5\n";
    exit(1);
  }
  uint16_t mask_types[] = {MaskBead, MaskEmpty, MaskLib, MaskTF, MaskPinned, MaskIgnore};
  const char *mask_names[] = {"MaskBead", "MaskEmpty", "MaskLib", "MaskTF", "MaskPinned", "MaskIgnore"};
  vector<MaskDiff> mask_compare(ArraySize(mask_types));
  for (size_t i = 0; i < mask_compare.size(); i++) {
    mask_compare[i].Init(mask_types[i], mask_names[i]);
  }
  Mask m1, m2;
  m1.SetMask(argv[1]);
  m2.SetMask(argv[2]);
  size_t num_wells = m1.H() * m1.W();
  for (size_t wIx = 0; wIx < num_wells; wIx++) {
    for (size_t cIx = 0; cIx < mask_compare.size(); cIx++) {
      mask_compare[cIx].Compare(m1[wIx] & mask_compare[cIx].entry, m2[wIx] & mask_compare[cIx].entry);
    }
  }

  Mat<float> s1, s2;
  string suffix = ":/separator/summary";
  string path1 = argv[3] + suffix;
  string path2 = argv[4] + suffix;
  const char *col_names[] = {"index","t0","snr","mad","sd","bfmetric","a","c","g","t","peak","flag","goodlive","isref","buffmetric"};
  H5Arma::ReadMatrix(path1, s1);
  H5Arma::ReadMatrix(path2, s2);
  vector<NumericalComparison<float> > column_compare(s1.n_cols);
  size_t num_names = ArraySize(col_names);
  for (size_t cIx = 0; cIx < num_names; cIx++) {
    column_compare[cIx].SetName(col_names[cIx]);
  }
  for (size_t cIx = num_names; cIx < column_compare.size(); cIx++) {
    string tmp = "unknown";
    column_compare[cIx].SetName(tmp + ToStr(cIx - num_names));
  }

  for (size_t rIx = 0; rIx < s1.n_rows; rIx++) {
    for (size_t cIx = 0; cIx < s1.n_cols; cIx++) {
      float v1 = s1.at(rIx,cIx);
      float v2 = s2.at(rIx,cIx);
      if (isfinite(v1*v2)) {
	column_compare[cIx].AddPair(v1,v2);
      }
    }
  }

  cout << "Separator Comparison" << endl
       << "--------- ----------" << endl;
  for (size_t cIx = 0; cIx < column_compare.size(); cIx++) {
    column_compare[cIx].Out(cout);
  }
  cout << endl;
  cout << "Mask Comparison" << endl
       << "---- ----------" << endl;
  bool same = true;
  for (size_t cIx = 0; cIx < mask_compare.size(); cIx++) {
    same &= mask_compare[cIx].IsSame();
    mask_compare[cIx].Out(cout);
  }
  if (!same) {
    cout << "Differences found." << endl;
    exit(1);
  }
  cout << "Same." << endl;
  return 0;
}


#include <boost/python.hpp>
#include <numpy/arrayobject.h>
#include <numpy_eigen.h>
#include "fouriertransform.h"

PyObject* ArminForwardFourierTransform(PyObject *py_ftau, const double beta,
                                  PyObject *py_ftail, const int num_matsubara) {
  Eigen::VectorXd ftau, ftail;
  numpy::from_numpy(py_ftau, ftau);
  numpy::from_numpy(py_ftail, ftail);

  Eigen::VectorXcd fmat;
  FourierTransformer fourier(beta, ftail);
  fourier.Armin_forward_ft(ftau, fmat, num_matsubara);

  return numpy::to_numpy(fmat);
}

PyObject* ForwardFourierTransform(PyObject *py_ftau, const double beta,
                                  PyObject *py_ftail, const int num_matsubara) {
  Eigen::VectorXd ftau, ftail;
  numpy::from_numpy(py_ftau, ftau);
  numpy::from_numpy(py_ftail, ftail);

  Eigen::VectorXcd fmat;
  FourierTransformer fourier(beta, ftail);
  fourier.forward_ft(ftau, fmat, num_matsubara);

  return numpy::to_numpy(fmat);
}

PyObject* BackwardFourierTransform(PyObject *py_fmat, const double beta,
                                   PyObject *py_ftail, const int num_tau) {
  Eigen::VectorXcd fmat;
  Eigen::VectorXd ftail;
  numpy::from_numpy(py_fmat, fmat);
  numpy::from_numpy(py_ftail, ftail);

  Eigen::VectorXd ftau;
  FourierTransformer fourier(beta, ftail);
  fourier.backward_ft(fmat, ftau, num_tau);
  return numpy::to_numpy(ftau);
}

BOOST_PYTHON_MODULE(fourier)
{
  boost::python::def("Armin_forward_ft", ArminForwardFourierTransform);
  boost::python::def("forward_ft", ForwardFourierTransform);
  boost::python::def("backward_ft", BackwardFourierTransform);
}


/**
 ** Copyright (c) 2014 Illumina, Inc.
 **
 ** This file is part of Illumina's Enhanced Artificial Genome Engine (EAGLE),
 ** covered by the "BSD 2-Clause License" (see accompanying LICENSE file)
 **/

#include <iostream>
#include <string>
#include <vector>
#include <boost/assign.hpp>

using namespace std;
using boost::assign::list_of;

#include "Helpers.hh"

#include "RegistryName.hh"
#include "testIntervalGenerator.hh"

using eagle::model::RandomSequenceGenerator;
using eagle::model::RandomIntervalGenerator;
using eagle::model::UniformIntervalGenerator;

CPPUNIT_TEST_SUITE_NAMED_REGISTRATION( TestIntervalGenerator, registryName("IntervalGenerator"));

void TestIntervalGenerator::setUp()
{
}

void TestIntervalGenerator::tearDown()
{
}

void TestIntervalGenerator::testReproducibleRandomness()
{
    // This test checks if we can expect reproducible results from the random number generator
    RandomSequenceGenerator gen( 5, 100 );

    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 3lu );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 23lu );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 29lu );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 36lu );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 42lu );
    CPPUNIT_ASSERT_THROW( gen.getNext(), eagle::common::EagleException );
}

void TestIntervalGenerator::testLargeRandomPositions()
{
    // This test relies on reproducible results from the random number generator (previous test)
    // to test that large numbers (larger than 32bits) don't get truncated anywhere
    RandomSequenceGenerator gen( 3, 15000000000000000000ul ); // large unsigned long, 64 bits or more

    CPPUNIT_ASSERT_EQUAL( gen.getNext(),  6263893078224449536ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(),  9941931729156847616ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext(), 11114242261213317120ul );
}

void TestIntervalGenerator::testRandomIntervals()
{
    // Check that it works with small chromosomes in the middle of large ones
    const std::vector<unsigned long> contigLengths        = list_of(10000000000ul)(50ul)(100ul)(10000000000ul)(50ul);
    const unsigned long              readCount            = 1000;
    const unsigned int               minFragmentLength    = 10;
    const unsigned int               medianFragmentLength = 20;
    const unsigned int               maxFragmentLength    = 30;
    RandomIntervalGenerator gen( contigLengths, readCount, minFragmentLength, medianFragmentLength, maxFragmentLength, false);

    // Check that numbers get generated in increasing order
    unsigned long lastPos = 0;
    for (unsigned int i=0; i<readCount; ++i)
    {
        unsigned long pos = gen.getNext().first;
        CPPUNIT_ASSERT( pos >= lastPos );
        lastPos = pos;
    }
}

void TestIntervalGenerator::testUniformIntervals()
{
    // Check that it works with small chromosomes in the middle of large ones
    const std::vector<unsigned long> contigLengths        = list_of(14ul)(100000000000ul)(50ul)(100ul)(10000000000ul)(50ul);
    const unsigned int               medianFragmentLength = 10;
    const double                     step                 = 0.75;
    unsigned long                    readCount            = 1000;
    UniformIntervalGenerator gen( contigLengths, medianFragmentLength, step, readCount, false);

    CPPUNIT_ASSERT_EQUAL( readCount, 146666666880ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  0ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  0ul ); // Here and below, we check that it takes into account the floating point step
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  1ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  2ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  3ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  3ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first,  4ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first, 14ul ); // Here we check that it jumps to next contig because of the fragment length
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first, 14ul ); // Here we check that the jump did reset the position to 14.0 and not to 14.25
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first, 15ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first, 16ul );
    CPPUNIT_ASSERT_EQUAL( gen.getNext().first, 17ul );
}

void TestIntervalGenerator::testRandomIntervalEnd()
{
    // We want to check that random intervals get properly enumerated and that the conversion enumeration to interval is not bugged
    // The difficulty is that these intervals are kept private,
    // and that the conversion only happens when an interval gets generated, which is done in a random way
    // This tests tries to go through all the random intervals by building an environment with only a few intervals
    // We generate many intervals to achieve a complete coverage, and we check that they are all present in the expected order
    const std::vector<unsigned long> contigLengths        = list_of(6ul)(4ul);
    const unsigned long              readCount            = 1000;
    const unsigned int               minFragmentLength    = 2;
    const unsigned int               medianFragmentLength = 3;
    const unsigned int               maxFragmentLength    = 4;

    const std::vector< std::pair< unsigned long, unsigned int > > expectedIntervals = list_of
        (std::make_pair(0ul,2))
        (std::make_pair(0ul,3))
        (std::make_pair(0ul,4))

        (std::make_pair(1ul,2))
        (std::make_pair(1ul,3))
        (std::make_pair(1ul,4))

        (std::make_pair(2ul,2))
        (std::make_pair(2ul,3))
        (std::make_pair(2ul,4))

        (std::make_pair(3ul,2))
        (std::make_pair(3ul,3))

        (std::make_pair(4ul,2))

        (std::make_pair(6ul,2))
        (std::make_pair(6ul,3))
        (std::make_pair(6ul,4))

        (std::make_pair(7ul,2))
        (std::make_pair(7ul,3))

        (std::make_pair(8ul,2))
         ;

    RandomIntervalGenerator gen( contigLengths, readCount, minFragmentLength, medianFragmentLength, maxFragmentLength, false);

    // Check that numbers get generated in increasing order
    unsigned int lastIntervalNum = 0;
    for (unsigned int i=0; i<readCount; ++i)
    {
        std::pair< unsigned long, unsigned int > interval = gen.getNext();
        //        std::clog << "int: " << interval.first << "," << interval.second << std::endl;
        if (interval != expectedIntervals[lastIntervalNum])
        {
            if (lastIntervalNum < expectedIntervals.size()-1)
            {
                ++lastIntervalNum;
            }
            if (interval != expectedIntervals[lastIntervalNum])
            {
                std::cerr << "expected  interval: [" << expectedIntervals[lastIntervalNum].first << "," << expectedIntervals[lastIntervalNum].second << "]" << std::endl;
                std::cerr << "generated interval: [" << interval.first << "," << interval.second << "]" << std::endl;
                CPPUNIT_ASSERT( false );
            }
        }
    }
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// other samrai stuff
#include <HierarchyDataOpsManager.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/linear_partitioner.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>
#include <libmesh/periodic_boundary.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/LEInteractor.h>
#include <ibtk/StableCentroidPartitioner.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// test stuff
#include "../tests.h"

// Like spread_02, but explicitly check the values spread from a constant field
// when the structure lives on multiple levels. This shows that the correct value is spread away from
IBTK::Point
exact_forcing(const IBTK::Point&)
{
    IBTK::Point result;
    result[0] = 1.0;
    result[1] = 1.0;
#if NDIM == 3
    result[2] = 1.0;
#endif
    return result;
}

int
main(int argc, char** argv)
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    // suppress warnings caused by using a refinement ratio of 4 and not
    // setting up coarsening correctly
    SAMRAI::tbox::Logger::getInstance()->setWarning(false);

    PetscOptionsSetValue(nullptr, "-ksp_rtol", "1e-16");
    PetscOptionsSetValue(nullptr, "-ksp_atol", "1e-16");

    // prevent a warning about timer initializations
    TimerManager::createManager(nullptr);
    {
        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");

        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Create a simple FE mesh.
        std::vector<std::unique_ptr<ReplicatedMesh> > meshes;
        meshes.emplace_back(new ReplicatedMesh(init.comm(), NDIM));
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        const std::string elem_str = input_db->getString("ELEM_TYPE");
        const auto elem_type = Utility::string_to_enum<ElemType>(elem_str);

        // This test only supports one type of mesh.
        {
            ReplicatedMesh& mesh = *meshes[0];
            const double L = input_db->getDouble("L");
            if (NDIM == 2)
                MeshTools::Generation::build_square(
                    mesh, int(L / ds), int(L / (4.0 * ds)), 0.0, L, 0.0, L / 4.0, elem_type);
            else
                MeshTools::Generation::build_cube(mesh,
                                                  int(L / ds),
                                                  int(L / (4.0 * ds)),
                                                  int(L / (4.0 * ds)),
                                                  0.0,
                                                  L,
                                                  0.0,
                                                  L / 4.0,
                                                  0.0,
                                                  L / 4.0,
                                                  elem_type);

            // Assign boundary elements to a finer patch level
            MeshBase::element_iterator el_end = mesh.active_elements_end();
            for (MeshBase::element_iterator el = mesh.active_elements_begin(); el != el_end; ++el)
            {
#if LIBMESH_VERSION_LESS_THAN(1, 7, 0)
                const libMesh::Point centroid = (*el)->centroid();
#else
                const libMesh::Point centroid = (*el)->vertex_average();
#endif
                if (centroid(0) < 0.5)
                {
                    (*el)->subdomain_id() = 2;
                }
                else
                {
                    (*el)->subdomain_id() = 1;
                }
            }
        }

        // metis does a good job partitioning, but the partitioning relies on
        // random numbers: the seed changed in libMesh commit
        // 98cede90ca8837688ee13aac5e299a3765f083da (between 1.3.1 and
        // 1.4.0). Hence, to achieve consistent partitioning, use a simpler partitioning scheme:
        for (const auto& mesh : meshes)
        {
            mesh->prepare_for_use();
            StableCentroidPartitioner partitioner;
            partitioner.partition(*mesh);
        }

        std::size_t n_elem = 0;
        for (const auto& mesh : meshes) n_elem += mesh->n_active_elem();
        plog << "Number of elements: " << n_elem << std::endl;

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"), false);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy =
            new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry, false);
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();

        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"),
                false);
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"),
                false);
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }

        std::vector<libMesh::MeshBase*> mesh_ptrs;
        for (auto& mesh : meshes)
        {
            mesh_ptrs.emplace_back(mesh.get());
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           mesh_ptrs,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           false);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator,
                                              false);
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        time_integrator->registerVisItDataWriter(visit_data_writer);

        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer,
                                        false);

        ib_method_ops->initializeFEEquationSystems();
        ib_method_ops->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // only regrid if we are in parallel - do this to avoid weird problems
        // with inconsistent initial partitionings (see
        // IBFEMethod::d_skip_initial_workload_log)
        if (IBTK_MPI::getNodes() != 1) time_integrator->regridHierarchy();

        // Now for the actual test. Set up a new variable containing ghost data:
        VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
        const Pointer<SAMRAI::hier::Variable<NDIM> > f_var = time_integrator->getBodyForceVariable();
        const Pointer<VariableContext> f_ghost_ctx = var_db->getContext("f_ghost");

        int n_ghosts = input_db->keyExists("IB_DELTA_FUNCTION") ?
                           LEInteractor::getMinimumGhostWidth(input_db->getString("IB_DELTA_FUNCTION")) :
                           3;
        if (app_initializer->getComponentDatabase("IBFEMethod")->keyExists("min_ghost_cell_width"))
        {
            n_ghosts = app_initializer->getComponentDatabase("IBFEMethod")->getInteger("min_ghost_cell_width");
        }
        const int f_ghost_idx = var_db->registerVariableAndContext(f_var, f_ghost_ctx, n_ghosts);

        for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
        {
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            level->allocatePatchData(f_ghost_idx);
            for (PatchLevel<NDIM>::Iterator p(level); p; p++)
            {
                Pointer<Patch<NDIM> > patch = level->getPatch(p());
                Pointer<SideData<NDIM, double> > f_data = patch->getPatchData(f_ghost_idx);
                f_data->fillAll(0.0);
            }
        }

        // synch ghost data
        using InterpolationTransactionComponent = HierarchyGhostCellInterpolation::InterpolationTransactionComponent;
        std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
        ghost_cell_components[0] = InterpolationTransactionComponent(f_ghost_idx,
                                                                     "CONSERVATIVE_LINEAR_REFINE",
                                                                     true,
                                                                     "CONSERVATIVE_COARSEN",
                                                                     "LINEAR",
                                                                     false,
                                                                     {}, // f_bc_coefs
                                                                     nullptr);
        HierarchyGhostCellInterpolation ghost_fill_op;
        ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
        ghost_fill_op.fillData(/*time*/ 0.0);

        const double dt = time_integrator->getMaximumTimeStepSize();
        time_integrator->preprocessIntegrateHierarchy(
            time_integrator->getIntegratorTime(), time_integrator->getIntegratorTime() + dt, 1 /*???*/);
        for (unsigned int part_n = 0; part_n < meshes.size(); ++part_n)
        {
            const std::string& F_name = IBFEMethod::FORCE_SYSTEM_NAME;
            const std::string& X_name = IBFEMethod::COORDS_SYSTEM_NAME;
            EquationSystems* equation_systems = ib_method_ops->getFEDataManager(part_n)->getEquationSystems();
            System& F_system = equation_systems->get_system(F_name);
            System& X_system = equation_systems->get_system(X_name);

            const unsigned int n_vars = F_system.n_vars();
            TBOX_ASSERT(n_vars == NDIM);
            const DofMap& F_dof_map = F_system.get_dof_map();
            const DofMap& X_dof_map = X_system.get_dof_map();

            PetscVector<double>& current_F = dynamic_cast<libMesh::PetscVector<double>&>(F_system.get_vector("half"));
            PetscVector<double>& current_X = *dynamic_cast<PetscVector<double>*>(X_system.current_local_solution.get());

            std::vector<dof_id_type> X_idxs;
            std::vector<dof_id_type> F_idxs;
            // Unlike elements, nodes are uniquely allocated to different processors
            const ReplicatedMesh& mesh = *meshes[part_n];
            for (auto node_iter = mesh.local_nodes_begin(); node_iter != mesh.local_nodes_end(); ++node_iter)
            {
                IBTK::Point X;
                for (unsigned int var_n = 0; var_n < n_vars; ++var_n)
                {
                    IBTK::get_nodal_dof_indices(X_dof_map, *node_iter, var_n, X_idxs);
                    X[var_n] = current_X(X_idxs[0]);
                }

                const auto F = exact_forcing(X);
                for (unsigned int var_n = 0; var_n < n_vars; ++var_n)
                {
                    IBTK::get_nodal_dof_indices(F_dof_map, *node_iter, var_n, F_idxs);
                    current_F.set(F_idxs[0], F[var_n]);
                }
            }
            current_F.close();
            *F_system.solution = current_F;
        }

        std::ostringstream out;
        if (IBTK_MPI::getNodes() != 1)
        {
            // partitioning is only relevant when there are multiple processors
            Pointer<PatchLevel<NDIM> > patch_level =
                patch_hierarchy->getPatchLevel(patch_hierarchy->getFinestLevelNumber());
            const BoxArray<NDIM> boxes = patch_level->getBoxes();
            plog << "hierarchy boxes:\n";
            for (int i = 0; i < boxes.size(); ++i) plog << boxes[i] << '\n';
            // rank is only relevant when there are multiple processors
            out << "\nrank: " << IBTK_MPI::getRank() << '\n';
        }

        // Here is the actual test:

        const double data_time = time_integrator->getIntegratorTime() + dt / 2;
        RobinPhysBdryPatchStrategy* bdry_op = time_integrator->getVelocityPhysBdryOp();
        ib_method_ops->spreadForce(f_ghost_idx, bdry_op, {}, data_time);

        // velocity interpolation requires that ghost data be present
        ghost_fill_op.fillData(/*time*/ 0.0);

        // Now put it back on the structure:
        ib_method_ops->interpolateVelocity(f_ghost_idx, {}, {}, data_time);

        // The rest is just bookkeeping and some visualization.

        // convert SC to CC for plotting purposes:
        Pointer<CellVariable<NDIM, double> > f_cc_var = new CellVariable<NDIM, double>("f_cc", NDIM);
        const int f_ghost_cc_idx = var_db->registerVariableAndContext(f_cc_var, f_ghost_ctx, IntVector<NDIM>(0));
        visit_data_writer->registerPlotQuantity("f_ghost", "VECTOR", f_ghost_cc_idx);

        for (unsigned int d = 0; d < NDIM; ++d)
        {
            visit_data_writer->registerPlotQuantity("f_ghost_" + std::to_string(d), "SCALAR", f_ghost_cc_idx, d);
        }

        HierarchyMathOps hier_math_ops("hier_math_ops", patch_hierarchy);
        // This is what we get for SAMRAI inventing its own type system and then
        // only half-implementing it
        Pointer<SideVariable<NDIM, double> > temp = f_var;
        TBOX_ASSERT(temp);
        // Allocate data on each level of the patch hierarchy.
        for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
        {
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            level->allocatePatchData(f_ghost_cc_idx, 0.0);
        }

        hier_math_ops.interp(f_ghost_cc_idx, f_cc_var, f_ghost_idx, temp, nullptr, 0.0, /*synch_cf_interface=*/true);

        visit_data_writer->writePlotData(patch_hierarchy, 0, 0.0);

#ifdef LIBMESH_HAVE_EXODUS_API
        {
            std::unique_ptr<ExodusII_IO> exodus_io(new ExodusII_IO(*meshes[0]));
            EquationSystems* equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();
            exodus_io->write_timestep("out.ex2", *equation_systems, 1, 0.0);
        }
#endif

        {
            const int ln = patch_hierarchy->getFinestLevelNumber();
            Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
            for (PatchLevel<NDIM>::Iterator p(level); p; p++)
            {
                bool printed_value = false;
                std::ostringstream patch_out;
                patch_out << "patch number " << p() << '\n';
                patch_out.precision(16);
                Pointer<Patch<NDIM> > patch = level->getPatch(p());
                Pointer<SideData<NDIM, double> > f_data = patch->getPatchData(f_ghost_idx);
                const Box<NDIM> patch_box = patch->getBox();

                // same as SideData::print, but elides values close to 1.
                for (int axis = 0; axis < NDIM; ++axis)
                {
                    patch_out << "Array side normal = " << axis << std::endl;
                    for (int d = 0; d < f_data->getDepth(); ++d)
                    {
                        patch_out << "Array depth = " << d << std::endl;
                        const ArrayData<NDIM, double>& data = f_data->getArrayData(axis);
                        for (SideIterator<NDIM> i(patch_box, axis); i; i++)
                        {
                            const double value = data(i(), d);
                            if (std::abs(value - 1.0) > 1e-2)
                            {
                                patch_out << "array" << i() << " = " << value << '\n';
                                printed_value = true;
                            }
                        }
                    }
                }
                if (printed_value) out << patch_out.str();
            }
        }
        IBTK_MPI::barrier();
        print_strings_on_plog_0(out.str());
    } // cleanup dynamically allocated objects prior to shutdown
} // main


#include <boost/test/unit_test.hpp>
#include "../src/Lenstra.h"
#include "../src/WeierstrassModel.h"
#include "../src/EdwardsModel.h"

struct TestFixture {
    TestFixture() {
        options = std::make_shared<Options>();
        options->weierstrass = true;
        *options->composite_number = NTL::ZZ(1000730021);
        weierstrass_model = std::make_shared<WeierstrassModel>(options);
        edwards_model = std::make_shared<EdwardsModel>(options);
    }
    std::shared_ptr<Options> options;
    std::shared_ptr<AbstractModel> weierstrass_model;
    std::shared_ptr<AbstractModel> edwards_model;
};

BOOST_FIXTURE_TEST_SUITE(lenstra_test, TestFixture)

BOOST_AUTO_TEST_CASE(test_weierstrass) {
    Lenstra test(options, weierstrass_model);
    auto result = test.factorize();
    BOOST_TEST((result == 100003 || result == 10007));
}

BOOST_AUTO_TEST_CASE(test_edwards) {
    Lenstra test(options, edwards_model);
    auto result = test.factorize();
    BOOST_TEST((result == 100003 || result == 10007));
}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_scenario_contact_phase_hpp__
#define __multicontact_api_scenario_contact_phase_hpp__

#include "multicontact-api/scenario/fwd.hpp"
#include "multicontact-api/scenario/contact-patch.hpp"

#include "multicontact-api/serialization/archive.hpp"
#include "multicontact-api/serialization/eigen-matrix.hpp"
#include "multicontact-api/serialization/spatial.hpp"
#include "multicontact-api/geometry/second-order-cone.hpp"
#include "multicontact-api/geometry/linear-cone.hpp"
#include "multicontact-api/container/ref.hpp"

#include <boost/array.hpp>

namespace multicontact_api
{
  namespace scenario
  {

    template<typename _Scalar, int _dim>
    struct ContactPhaseTpl : public serialization::Serializable< ContactPhaseTpl<_Scalar,_dim> >
    {
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      typedef _Scalar Scalar;
      typedef ContactPatchTpl<Scalar> ContactPatch;
      typedef boost::array<ContactPatch,_dim> ContactPatchArray;
      typedef std::vector< container::comparable_reference_wrapper<ContactPatch> > ContactPatchVector;
      typedef std::vector< container::comparable_reference_wrapper<ContactPatch const> > ConstContactPatchVector;
      typedef geometry::SecondOrderCone<_Scalar,6> SOC6;
      typedef geometry::WrenchConeTpl<_Scalar> WrenchCone;
      typedef Eigen::DenseIndex Index;
      typedef typename ContactPatch::SE3 SE3;
      typedef Eigen::Matrix<double,6,Eigen::Dynamic> Matrix6x;

      enum { dim = _dim };

      /// \brief Default constructor
      ContactPhaseTpl()
      : m_sowc_placement(SE3::Identity())
      {}

      /// \brief Copy constructor
      template<typename S2>
      ContactPhaseTpl(const ContactPhaseTpl<S2,dim> & other)
      : m_contact_patches(other.m_contact_patches)
      , m_sowc(other.m_sowc)
      , m_sowc_placement(other.m_sowc_placement)
      , m_lwc(other.m_lwc)
      {}

      const ContactPatchArray & contact_patches() const { return m_contact_patches; }
      ContactPatchArray & contact_patches() { return m_contact_patches; }

      /// \Returns a reference of the second order wrench cone
      const SOC6 & sowc() const { return m_sowc; }
      SOC6 & sowc() { return m_sowc; }

      const SE3 & sowcPlacement() const { return m_sowc_placement; }
      SE3 & sowcPlacement() { return m_sowc_placement; }

      const Matrix6x & doubleDescription() const { return m_double_description; }
      Matrix6x & doubleDescription() { return m_double_description; }

      const WrenchCone & lwc() const { return m_lwc; }
      WrenchCone & lwc() { return m_lwc; }

      /// \returns the number of active patches
      Index numActivePatches() const
      {
        Index num_active = 0;
        for(typename ContactPatchArray::const_iterator it = m_contact_patches.begin();
            it != m_contact_patches.end(); ++it)
          if(it->active()) num_active++;

        return num_active;
      }

      /// \returns the number of inactive patches
      Index numInactivePatches() const
      { return dim - numActivePatches(); }

      template<typename S2>
      bool operator==(const ContactPhaseTpl<S2,dim> & other) const
      {
        return
        m_contact_patches == other.m_contact_patches
        && m_lwc == other.m_lwc
        && m_sowc == other.m_sowc
        ;
      }

      template<typename S2>
      bool operator!=(const ContactPhaseTpl<S2,dim> & other) const
      { return !(*this == other); }

      ContactPatchArray m_contact_patches; // TODO: set protected

      ///
      /// \brief Returns the first active patch.
      ///
      /// \remark This method is useful one looks for the lonely active patch.
      ///
      /// \returns the first active patch.
      ///
      const ContactPatch & getActivePatch() const
      {
        for(typename ContactPatchArray::const_iterator it = m_contact_patches.begin();
            it != m_contact_patches.end(); ++it)
          if(it->active()) return *it;
      }
      ContactPatch & getActivePatch()
      { return const_cast<ContactPatch &>(static_cast<const ContactPhaseTpl*>(this)->getActivePatch()); }

      ContactPatchVector getActivePatches()
      {
        ContactPatchVector res; res.reserve((size_t)dim);
        for(typename ContactPatchArray::iterator it = m_contact_patches.begin();
            it != m_contact_patches.end(); ++it)
          if(it->active()) res.push_back(typename ContactPatchVector::value_type(*it));

        return res;
      }

    protected:

      /// \brief Second Order Wrench Cone (SOWC) representing the Minkoski sum of the patch linear wrench cone.
      SOC6 m_sowc;
      SE3 m_sowc_placement;

      Matrix6x m_double_description;

      /// \brief Linear Wrench Cone (LWC) representing the Minkoski sum of the patch linear wrench cone.
      WrenchCone m_lwc;

    private:

      // Serialization of the class
      friend class boost::serialization::access;

      template<class Archive>
      void save(Archive & ar, const unsigned int /*version*/) const
      {
        for(typename ContactPatchArray::const_iterator it = m_contact_patches.begin();
            it != m_contact_patches.end(); ++it)
          ar & boost::serialization::make_nvp("contact_patch",*it);

        ar & boost::serialization::make_nvp("lwc",m_lwc);
        ar & boost::serialization::make_nvp("sowc",m_sowc);
        ar & boost::serialization::make_nvp("sowc_placement",m_sowc_placement);
        ar & boost::serialization::make_nvp("double_description",m_double_description);
      }

      template<class Archive>
      void load(Archive & ar, const unsigned int /*version*/)
      {
        for(typename ContactPatchArray::iterator it = m_contact_patches.begin();
            it != m_contact_patches.end(); ++it)
          ar >> boost::serialization::make_nvp("contact_patch",*it);

        ar >> boost::serialization::make_nvp("lwc",m_lwc);
        ar >> boost::serialization::make_nvp("sowc",m_sowc);
        ar >> boost::serialization::make_nvp("sowc_placement",m_sowc_placement);
        ar >> boost::serialization::make_nvp("double_description",m_double_description);
      }

      BOOST_SERIALIZATION_SPLIT_MEMBER()
    };
  }
}

#endif // ifndef __multicontact_api_scenario_contact_phase_hpp__


/*  $Id: millingcutter.cpp 781 2011-07-12 10:03:54Z anders.e.e.wallin $
 *
 *  Copyright 2010 Anders Wallin (anders.e.e.wallin "at" gmail.com)
 *
 *  This file is part of OpenCAMlib.
 *
 *  OpenCAMlib is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  OpenCAMlib is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with OpenCAMlib.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <boost/foreach.hpp>

#include "millingcutter.h"
#include "numeric.h"

namespace ocl
{

MillingCutter* MillingCutter::offsetCutter(double d) const {
    assert(0); // DON'T call me
    return  NULL;
}

// general purpose vertex-drop which delegates to this->height(r) of subclass
bool MillingCutter::vertexDrop(CLPoint &cl, const Triangle &t) const {
    bool result = false;
    BOOST_FOREACH( const Point& p, t.p) {           // test each vertex of triangle
        double q = cl.xyDistance(p);                // distance in XY-plane from cl to p
        if ( q <= radius ) {                        // p is inside the cutter
            CCPoint cc_tmp(p, VERTEX);
            if ( cl.liftZ( p.z - this->height(q), cc_tmp ) )
                result = true;
        }
    }
    return result;
}

// general purpose facet-drop which calls xy_normal_length(), normal_length(),
// and center_height() on the subclass
bool MillingCutter::facetDrop(CLPoint &cl, const Triangle &t) const { // Drop cutter at (cl.x, cl.y) against facet of Triangle t
    Point normal = t.upNormal(); // facet surface normal
    if ( isZero_tol( normal.z ) )  // vertical surface
        return false;  //can't drop against vertical surface
    assert( isPositive( normal.z ) );

    if ( ( isZero_tol(normal.x) ) && ( isZero_tol(normal.y) ) ) { // horizontal plane special case
        CCPoint cc_tmp( cl.x, cl.y, t.p[0].z, FACET);
        return cl.liftZ_if_inFacet(cc_tmp.z, cc_tmp, t);
    } else { // general case
        // plane containing facet:  a*x + b*y + c*z + d = 0, so
        // d = -a*x - b*y - c*z, where  (a,b,c) = surface normal
        double d = - normal.dot(t.p[0]);
        normal.normalize(); // make length of normal == 1.0
        Point xyNormal( normal.x, normal.y, 0.0);
        xyNormal.xyNormalize();
        // define the radiusvector which points from the cc-point to the cutter-center
        Point radiusvector = this->xy_normal_length*xyNormal + this->normal_length*normal;
        CCPoint cc_tmp = cl - radiusvector; // NOTE xy-coords right, z-coord is not.
        cc_tmp.z = (1.0/normal.z)*(-d-normal.x*cc_tmp.x-normal.y*cc_tmp.y); // cc-point lies in the plane.
        cc_tmp.type = FACET;
        double tip_z = cc_tmp.z + radiusvector.z - this->center_height;
        return cl.liftZ_if_inFacet(tip_z, cc_tmp, t);
    }
}

// edge-drop function which calls the sub-class MillingCutter::singleEdgeDrop on each
// edge of the input Triangle t.
bool MillingCutter::edgeDrop(CLPoint &cl, const Triangle &t) const {
    bool result = false;
    for (int n=0;n<3;n++) { // loop through all three edges
        int start=n;      // index of the start-point of the edge
        int end=(n+1)%3;  // index of the end-point of the edge
        const Point p1 = t.p[start];
        const Point p2 = t.p[end];
        if ( !isZero_tol( p1.x - p2.x) || !isZero_tol( p1.y - p2.y) ) {
            const double d = cl.xyDistanceToLine(p1,p2);
            if (d<=radius)  // potential contact with edge
                if ( this->singleEdgeDrop(cl,p1,p2,d) )
                    result=true;
        }
    }
    return result;
}

// "dual" edge-drop problems
// cylinder: zero diam edge/ellipse, r-radius cylinder, find r-offset == cl  (ITO surface XY-slice is a circle)
// sphere: zero diam cylinder. ellipse around edge, find offset == cl (ITO surface slice is ellipse) (?)
// toroid: radius2 diam edge, radius1 cylinder, find radius1-offset-ellipse=cl (ITO surf slice is offset ellipse) (this is the offset-ellipse problem)
// cone: ??? (how is this an ellipse??)
bool MillingCutter::singleEdgeDrop(CLPoint& cl, const Point& p1, const Point& p2, double d) const {
    Point v = p2 - p1; // vector along edge, from p1 -> p2
    Point vxy( v.x, v.y, 0.0);
    vxy.xyNormalize(); // normalized XY edge vector
    // figure out u-coordinates of p1 and p2 (i.e. x-coord in the rotated system)
    Point sc = cl.xyClosestPoint( p1, p2 );
    assert( ( (cl-sc).xyNorm() - d ) < 1E-6 );
    // edge endpoints in the new coordinate system, in these coordinates, CL is at origo
    Point up1( (p1-sc).dot(vxy) , d, p1.z); // d, distance to line, is the y-coord in the rotated system
    Point up2( (p2-sc).dot(vxy) , d, p2.z);
    CC_CLZ_Pair contact = this->singleEdgeDropCanonical( up1, up2 ); // the subclass handles this
    CCPoint cc_tmp( sc + contact.first * vxy, EDGE); // translate back into original coord-system
    cc_tmp.z_projectOntoEdge(p1,p2);
    return cl.liftZ_if_InsidePoints( contact.second , cc_tmp , p1, p2);
}

// general purpose vertexPush, delegates to this->width(h)
bool MillingCutter::vertexPush(const Fiber& f, Interval& i, const Triangle& t) const {
    bool result = false;
    BOOST_FOREACH( const Point& p, t.p) {
        if (this->singleVertexPush(f,i,p, VERTEX) )
            result = true;
    }
    if ( this->vertexPushTriangleSlice() ) { // special case for CylCutter
        Point p1, p2;
        if ( t.zslice_verts(p1, p2, f.p1.z) ) {
            p1.z = p1.z + 1E-3; // dirty trick...
            p2.z = p2.z + 1E-3; // ...which will not affect results, unless cutter.length < 1E-3
            if ( this->singleVertexPush(f,i,p1, VERTEX_CYL) )
                result = true;
            if (this->singleVertexPush(f,i,p2, VERTEX_CYL))
                result = true;
        }
    }
    return result;
}

bool MillingCutter::singleVertexPush(const Fiber& f, Interval& i, const Point& p, CCType cctyp) const {
    bool result = false;
    if ( ( p.z > f.p1.z ) && ( p.z <= (f.p1.z+ this->getLength()) ) ) { // p.z is within cutter
        Point pq = p.xyClosestPoint(f.p1, f.p2); // closest point on fiber
        double q = (p-pq).xyNorm(); // distance in XY-plane from fiber to p
        double h = p.z - f.p1.z;
        assert( h>= 0.0);
        double cwidth = this->width( h );
        if ( q <= cwidth ) { // we are going to hit the vertex p
            double ofs = sqrt( square( cwidth ) - square(q) ); // distance along fiber
            Point start = pq - ofs*f.dir;
            Point stop  = pq + ofs*f.dir;
            CCPoint cc_tmp( p, cctyp );
            i.updateUpper( f.tval(stop) , cc_tmp );
            i.updateLower( f.tval(start) , cc_tmp );
            result = true;
        }
    }
    return result;
}

bool MillingCutter::facetPush(const Fiber& fib, Interval& i,  const Triangle& t) const {
    return generalFacetPush(this->normal_length,
                            this->center_height,
                            this->xy_normal_length,
                            fib,i,t);
}

// general purpose facetPush
bool MillingCutter::generalFacetPush(double normal_length,
                                     double center_height,
                                     double xy_normal_length,
                                     const Fiber& fib,
                                     Interval& i,
                                     const Triangle& t)
                                     const {
    bool result = false;
    Point normal = t.upNormal(); // facet surface normal, pointing up
    if ( normal.zParallel() ) // normal points in z-dir
        return result; //can't push against horizontal plane, stop here.
    normal.normalize();
    Point xy_normal = normal;
    xy_normal.z = 0;
    xy_normal.xyNormalize();

    //   find a point on the plane from which radius2*normal+radius1*xy_normal lands on the fiber+radius2*Point(0,0,1)
    //   (u,v) locates a point on the triangle facet    v0+ u*(v1-v0)+v*(v2-v0)    u,v in [0,1]
    //   t locates a point along the fiber:             p1 + t*(p2-p1)             t in [0,1]
    //
    //   facet-point + r2 * n + r1* xy_n = fiber-point + r2*Point(0,0,1)
    //   =>
    //   v0+ u*(v1-v0)+v*(v2-v0) + r2 * n + r1* xy_n  = p1 + t*(p2-p1) + r2*Point(0,0,1)
    //
    //   v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x  = p1x + t*(p2x-p1x)          p2x-p1x==0 for Y-fiber
    //   v0y + u*(v1y-v0y) + v*(v2y-v0y) + r2*ny + r1*xy_n.y  = p1y + t*(p2y-p1y)          p2y-p1y==0 for X-fiber
    //   v0z + u*(v1z-v0z) + v*(v2z-v0z) + r2*nz              = p1z + t*(p2z-p1z) + r2    (p2z-p1z)==0 for XY-fibers!!
    //   X-fiber:
    //   v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x  = p1x + t*(p2x-p1x)
    //   v0y + u*(v1y-v0y) + v*(v2y-v0y) + r2*ny + r1*xy_n.y  = p1y                    solve these  two for (u,v)
    //   v0z + u*(v1z-v0z) + v*(v2z-v0z) + r2*nz              = p1z + r2               and substitute above for t
    //   or
    //   [ (v1y-v0y)    (v2y-v0y) ] [ u ] = [ -v0y - r2*ny - r1*xy_n.y + p1y     ]
    //   [ (v1z-v0z)    (v2z-v0z) ] [ v ] = [ -v0z - r2*nz + p1z + r2            ]
    //
    //   Y-fiber:
    //   [ (v1x-v0x)    (v2x-v0x) ] [ u ] = [ -v0x - r2*nx - r1*xy_n.x + p1x     ]

    double a;
    double b;
    double c = t.p[1].z - t.p[0].z;
    double d = t.p[2].z - t.p[0].z;
    double e;
    double f = -t.p[0].z - normal_length*normal.z + fib.p1.z + center_height;
    // note: the xy_normal does not have a z-component, so omitted here.

    double u;
    double v;
    // a,b,e depend on the fiber:
    if ( fib.p1.y == fib.p2.y ) { // XFIBER
        a = t.p[1].y - t.p[0].y;
        b = t.p[2].y - t.p[0].y;
        e = -t.p[0].y - normal_length*normal.y - xy_normal_length*xy_normal.y + fib.p1.y;
        if (!two_by_two_solver(a,b,c,d,e,f,u,v))
            return result;
        CCPoint cc = t.p[0] + u*(t.p[1]-t.p[0]) + v*(t.p[2]-t.p[0]);
        cc.type = FACET;
        if ( ! cc.isInside( t ) )
            return result;
        // v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x = p1x + t*(p2x-p1x)
        // =>
        // t = 1/(p2x-p1x) * ( v0x + r2*nx + r1*xy_n.x - p1x +  u*(v1x-v0x) + v*(v2x-v0x)       )
        assert( !isZero_tol( fib.p2.x - fib.p1.x )  );
        double tval = (1.0/( fib.p2.x - fib.p1.x )) * ( t.p[0].x + normal_length*normal.x + xy_normal_length*xy_normal.x - fib.p1.x
                                                        + u*(t.p[1].x-t.p[0].x)+v*(t.p[2].x-t.p[0].x) );
        if ( tval < 0.0 || tval > 1.0  ) {
            std::cout << "MillingCutter::facetPush() tval= " << tval << " error!?\n";
            //std::cout << " cutter: " << *this << "\n";
            std::cout << " triangle: " << t << "\n";
            std::cout << " fiber: " << fib << "\n";
        }
        assert( tval > 0.0 && tval < 1.0 );
        i.updateUpper( tval  , cc );
        i.updateLower( tval  , cc );
        result = true;
    } else if (fib.p1.x == fib.p2.x) { // YFIBER
        a = t.p[1].x - t.p[0].x;
        b = t.p[2].x - t.p[0].x;
        e = -t.p[0].x - normal_length*normal.x - xy_normal_length*xy_normal.x + fib.p1.x;
        if (!two_by_two_solver(a,b,c,d,e,f,u,v))
            return result;
        CCPoint cc = t.p[0] + u*(t.p[1]-t.p[0]) + v*(t.p[2]-t.p[0]);
        cc.type = FACET;
        if ( ! cc.isInside( t ) )
            return result;
        assert( !isZero_tol( fib.p2.y - fib.p1.y )  );
        double tval = (1.0/( fib.p2.y - fib.p1.y )) * ( t.p[0].y + normal_length*normal.y + xy_normal_length*xy_normal.y - fib.p1.y
                                                        + u*(t.p[1].y-t.p[0].y)+v*(t.p[2].y-t.p[0].y) );
        if ( tval < 0.0 || tval > 1.0  ) {
            std::cout << "MillingCutter::facetPush() tval= " << tval << " error!?\n";
        }
        assert( tval > 0.0 && tval < 1.0 );
        i.update( tval, cc );
        result = true;
    } else {
        assert(0);
    }

    return result;
}

bool MillingCutter::edgePush(const Fiber& f, Interval& i,  const Triangle& t) const {
    bool result = false;
    for (int n=0;n<3;n++) { // loop through all three edges
        int start=n;
        int end=(n+1)%3;
        const Point p1 = t.p[start]; // edge is from p1 to p2
        const Point p2 = t.p[end];
        if ( this->singleEdgePush(f,i,p1,p2))
            result = true;
    }
    return result;
}

bool MillingCutter::singleEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    bool result = false;
    if ( this->horizEdgePush(f,i,p1,p2) )
        result = true;
    else {
        if ( this->shaftEdgePush(f,i,p1,p2) )
            result = true;
        if ( this->generalEdgePush(f,i,p1,p2) )
            result = true;
    }
    return result;
}

// this is used for the cylindrical shaft of Cyl, Ball, Bull, Cone
bool MillingCutter::shaftEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    // push cutter along Fiber f in contact with edge p1-p2
    // contact with cylindrical cutter shaft
    double u,v;
    bool result = false;
    if ( xy_line_line_intersection(p1, p2, u, f.p1, f.p2, v ) ) { // find XY-intersection btw fiber and edge
        Point q = p1 + u*(p2-p1); // edge/fiber intersection point, on edge
        // Point q = f.p1 + v*(f.p2-f.p1); // q on fiber
        // from q, go v_cc*xy_tangent, then r*xy_normal, and end up on fiber:
        // q + v_cc*tangent + r*xy_normal = p1 + t_cl*(p2-p1)
        Point xy_tang=p2-p1;
        xy_tang.z=0;
        xy_tang.xyNormalize();
        Point xy_normal = xy_tang.xyPerp();
        Point q1 = q  + radius*xy_normal;
        Point q2 = q1 + (p2-p1);
        double u_cc, t_cl;
        if ( xy_line_line_intersection( q1 , q2, u_cc, f.p1, f.p2, t_cl ) ) {
            double t_cl1 = t_cl;            // cc_tmp1 = q +/- u_cc*(p2-p1);
            double t_cl2 = v + (v-t_cl);
            if ( calcCCandUpdateInterval(t_cl1, u_cc, q, p1, p2, f, i, f.p1.z+center_height, EDGE_SHAFT) )
                result = true;
            if ( calcCCandUpdateInterval(t_cl2, -u_cc, q, p1, p2, f, i, f.p1.z+center_height, EDGE_SHAFT) )
                result = true;
        }
    }
    //std::cout << " shaftEdgePush = " << result << "\n";
    return result;
}

// this is the horizontal edge case
bool MillingCutter::horizEdgePush(const Fiber& f, Interval& i,  const Point& p1, const Point& p2) const {
    bool result=false;
    double h = p1.z - f.p1.z; // height of edge above fiber
    if ( (h > 0.0) ) {
        if ( isZero_tol( p2.z-p1.z ) ) { // this is the horizontal-edge special case
            double eff_radius = this->width( h ); // the cutter acts as a cylinder with eff_radius
            // contact this cylinder/circle against edge in xy-plane
            double qt;      // fiber is f.p1 + qt*(f.p2-f.p1)
            double qv;      // line  is p1 + qv*(p2-p1)
            if (xy_line_line_intersection( p1 , p2, qv, f.p1, f.p2, qt ) ) {
                Point q = p1 + qv*(p2-p1); // the intersection point
                // from q, go v-units along tangent, then eff_r*normal, and end up on fiber:
                // q + ccv*tangent + r*normal = p1 + clt*(p2-p1)
                double ccv, clt;
                Point xy_tang=p2-p1;
                xy_tang.z=0;
                xy_tang.xyNormalize();
                Point xy_normal = xy_tang.xyPerp();
                Point q1 = q+eff_radius*xy_normal;
                Point q2 = q1+(p2-p1);
                if ( xy_line_line_intersection( q1 , q2, ccv, f.p1, f.p2, clt ) ) {
                    double t_cl1 = clt;
                    double t_cl2 = qt + (qt - clt );
                    if ( calcCCandUpdateInterval(t_cl1, ccv, q, p1, p2, f, i, f.p1.z, EDGE_HORIZ) )
                        result = true;
                    if ( calcCCandUpdateInterval(t_cl2, -ccv, q, p1, p2, f, i, f.p1.z, EDGE_HORIZ) )
                        result = true;
                }
            }
        }
    }
    //std::cout << " horizEdgePush = " << result << "\n";
    return result;
}

bool MillingCutter::calcCCandUpdateInterval( double t, double u, const Point& q, const Point& p1, const Point& p2,
                                             const Fiber& f, Interval& i, double height, CCType cctyp) const {
    CCPoint cc_tmp = q+u*(p2-p1);
    cc_tmp.type = cctyp;
    return i.update_ifCCinEdgeAndTrue( t, cc_tmp, p1, p2, (cc_tmp.z >= height)  );
}

bool MillingCutter::pushCutter(const Fiber& f, Interval& i, const Triangle& t) const {
    //std::cout << " vertexPush() ";
    bool v = vertexPush(f,i,t);
    //std::cout << " done.\n";
    //std::cout << " facetPush() ";
    bool fa = facetPush(f,i,t);
    //std::cout << " done.\n";
    //std::cout << " edgePush() ";
    bool e = edgePush(f,i,t);
    //std::cout << " done.\n";
    return v || fa || e;
}

// call vertex, facet, and edge drop methods on input Triangle t
bool MillingCutter::dropCutter(CLPoint &cl, const Triangle &t) const {
    bool facet, vertex, edge;
    /* // alternative ordering of the tests:
    if (cl.below(t))
        vertexDrop(cl,t);

    // optimisation: if we are now above the triangle we don't need facet and edge
    if ( cl.below(t) ) {
        facetDrop(cl,t);
        edgeDrop(cl,t);
    }*/

    if (cl.below(t)) {
        facet = facetDrop(cl,t);
        if (!facet) {
            vertex = vertexDrop(cl,t);
            if ( cl.below(t) ) {
                edge = edgeDrop(cl,t);
            }
        }
    }

    return ( facet || vertex || edge );
}

// TESTING ONLY, don't use for real
bool MillingCutter::dropCutterSTL(CLPoint &cl, const STLSurf &s) const {
    bool result=false;
    BOOST_FOREACH( const Triangle& t, s.tris) {
        if ( this->dropCutter(cl,t) )
            result = true;
    }
    return result;
}

// overlap test: does cutter at cl.x cl.y overlap in the xy-plane with triangle t
bool MillingCutter::overlaps(Point &cl, const Triangle &t) const {
    if ( t.bb.maxpt.x < cl.x-radius )
        return false;
    else if ( t.bb.minpt.x > cl.x+radius )
        return false;
    else if ( t.bb.maxpt.y < cl.y-radius )
        return false;
    else if ( t.bb.minpt.y > cl.y+radius )
        return false;
    else
        return true;
}

} // end namespace
// end file cutter.cpp


#ifndef LIBLEARNING_RANDOMFOREST_FOREST_H
#define LIBLEARNING_RANDOMFOREST_FOREST_H
#include "common-libraries.hpp"
#include "tree.hpp"
#include <boost/ptr_container/serialize_ptr_vector.hpp>
#if VERBOSE_TREE_PROGRESS
#include <cstdio>
#endif

namespace liblearning {
namespace RandomForest {

template <typename NodeT>
class RandomForest {
public:
    typedef typename NodeT::ParamType ParamType;
    typedef typename NodeT::FeatureType FeatureType;
    typedef Tree<NodeT> TreeType;
    ParamType params;

    std::vector<uint8_t> was_oob_data;
    DataView2D<uint8_t> was_oob;

    boost::ptr_vector< Tree<NodeT> > trees;

    RandomForest() {}
    RandomForest(ParamType const& params) : params(params) {}

    template<typename SplitGenerator>
    void train(DataView2D<FeatureType> samples,
               DataView2D<int> labels,
               DataView2D<int> train_sample_idxes,
               SplitGenerator const& split_generator,
               size_t seed_start = 1,
               bool register_oob = true
               )
    {
        trees.clear();
        params.n_classes  = *std::max_element(&labels(0,0), &labels(0,0)+labels.num_elements()) + 1;
        params.n_features = samples.cols;
        params.n_samples  = samples.rows;

        std::vector<int> sample_idxes;

        if (train_sample_idxes.empty()) {
            // no indexes were passed, generate vector with all indexes
            sample_idxes.resize(params.n_samples);
            for (size_t i_sample = 0; i_sample < params.n_samples; ++i_sample) {
                sample_idxes[i_sample] = i_sample;
            }
        } else {
            // copy indexes
            sample_idxes.assign(&train_sample_idxes(0,0), &train_sample_idxes(0,0)+train_sample_idxes.num_elements());
        }

        size_t n_idxes = sample_idxes.size();
        params.n_in_bag_samples = n_idxes * (1 - params.sample_reduction);

        // Random distribution over indexes
        UniformIntDist dist(0, n_idxes - 1);

        // Store for each sample and each tree if sample was used for tree
        if (register_oob) {
            was_oob_data.assign(n_idxes*params.n_trees, 1);
            was_oob = DataView2D<uint8_t>(&was_oob_data[0], n_idxes, params.n_trees);
        }

        for (size_t i_tree = 0; i_tree < params.n_trees; ++i_tree) {
#if VERBOSE_TREE_PROGRESS
            std::printf("Training tree %zu/%zu, max depth %zu\n", i_tree+1, params.n_trees, params.max_depth);
#endif
            // new tree
            trees.push_back(new TreeType(&params));
            // initialize random generator with sequential seeds (one for each
            // tree)
            RandomGen gen(seed_start + i_tree);
            // Bagging: draw random sample indexes used for this tree
            std::vector<int> in_bag_samples(params.n_in_bag_samples);
            for (size_t i_sample = 0; i_sample < in_bag_samples.size(); ++i_sample) {
                int random_idx = dist(gen);
                in_bag_samples[i_sample] = sample_idxes[random_idx];
                if (register_oob && was_oob(random_idx, i_tree)) {
                    was_oob(random_idx, i_tree) = 0;
                }
            }
#ifdef TREE_GRAPHVIZ_STREAM
            TREE_GRAPHVIZ_STREAM << "digraph Tree {" << std::endl;
#endif
            // Train the tree
            trees.back().train(samples, labels, &in_bag_samples[0], in_bag_samples.size(), split_generator, gen);
#ifdef TREE_GRAPHVIZ_STREAM
            TREE_GRAPHVIZ_STREAM << "}" << std::endl << std::endl;
#endif
        }
    }
    int evaluate(FeatureType const* sample, float* results) {
        // initialize output probabilities to 0
        std::fill_n(results, params.n_classes, 0);
        // accumulate votes of the trees
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            float const* tree_result = trees[i_tree].evaluate(sample);
            for (size_t i_cls = 0; i_cls < params.n_classes; ++i_cls) {
                results[i_cls] += tree_result[i_cls];
            }
        }
        float best_val   = 0.0;
        int   best_class = 0;
        float scale      = 1.0 / trees.size();
        for (size_t i_cls = 0; i_cls < params.n_classes; ++i_cls) {
            // divide by number of trees to normalize probability
            results[i_cls] *= scale;
            // determine best class
            if (results[i_cls] > best_val) {
                best_val = results[i_cls];
                best_class = i_cls;
            }
        }
        return best_class;
    }
#if 0
    float similarity_endnode(float const* sample_1, float const* sample_2) {
        double sum = 0.0;
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            sum += trees[i_tree].similarity_endnode(sample_1, sample_2);
        }
        return sum/trees.size();
    }
    float similarity_path(float const* sample_1, float const* sample_2) {
        double sum = 0.0;
        for (size_t i_tree = 0; i_tree < trees.size(); ++i_tree) {
            sum += trees[i_tree].similarity_path(sample_1, sample_2);
        }
        return sum/trees.size();
    }
#endif
    template <typename Archive>
    void serialize(Archive& ar, unsigned /* version */)
    {
        ar & BOOST_SERIALIZATION_NVP(params);
        ar & BOOST_SERIALIZATION_NVP(trees);
    }
};

}
}
#endif