/* Copyright 2015 Google Inc. All Rights Reserved.

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.
==============================================================================*/

// Computing label Op

#include <stdio.h>
#include <cfloat>
#include <math.h>
#include <time.h>
#include <algorithm>
#include <Eigen/Geometry>
#include "opencv2/opencv.hpp"

#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"

#define VERTEX_CHANNELS 3

using namespace tensorflow;
typedef Eigen::ThreadPoolDevice CPUDevice;

REGISTER_OP("Houghvotinggpu")
    .Attr("T: {float, double}")
    .Attr("is_train: int")
    .Attr("threshold_vote: int")
    .Attr("skip_pixels: int")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("bottom_extents: T")
    .Input("bottom_meta_data: T")
    .Input("bottom_gt: T")
    .Output("top_box: T")
    .Output("top_pose: T")
    .Output("top_target: T")
    .Output("top_weight: T")
    .Output("top_domain: int32");

REGISTER_OP("HoughvotinggpuGrad")
    .Attr("T: {float, double}")
    .Input("bottom_label: int32")
    .Input("bottom_vertex: T")
    .Input("grad: T")
    .Output("output_label: T")
    .Output("output_vertex: T");

int clamp(int val, int min_val, int max_val)
{
  return std::max(min_val, std::min(max_val, val));
}

void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes);
inline std::vector<cv::Point3f> getBB3D(const cv::Vec<float, 3>& extent);
inline cv::Rect getBB2D(int imageWidth, int imageHeight, const std::vector<cv::Point3f>& bb3D, const cv::Mat& camMat, const cv::Mat& rvec, const cv::Mat& tvec);
inline float getIoU(const cv::Rect& bb1, const cv::Rect bb2);
inline float angle_distance(cv::Point2f x, cv::Point2f n, cv::Point2f p);

void hough_voting(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds,
  int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 14> >& outputs);

void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds,
  const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 14> > outputs);

inline void compute_width_height(const int* labelmap, const float* vertmap, cv::Point2f center,
  std::vector<std::vector<cv::Point3f>> bb3Ds, cv::Mat camMat, float inlierThreshold,
  int height, int width, int channel, int num_classes, int & bb_width, int & bb_height, float & bb_distance);

// cuda functions
void HoughVotingLaucher(OpKernelContext* context,
    const int* labelmap, const float* vertmap, const float* extents, const float* meta_data, const float* gt,
    const int batch_index, const int height, const int width, const int num_classes, const int num_gt,
    const int is_train, const float inlierThreshold, const int labelThreshold, const int votingThreshold, const int skip_pixels,
    float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain, int* num_rois, const Eigen::GpuDevice& d);

void allocate_outputs(OpKernelContext* context, Tensor* top_box_tensor, Tensor* top_pose_tensor, Tensor* top_target_tensor, Tensor* top_weight_tensor, Tensor* top_domain_tensor, Tensor* top_rois_tensor, int num_classes)
{
  int num = 1024;
  int dims[2];

  dims[0] = num;
  dims[1] = 7;
  TensorShape output_shape;
  TensorShapeUtils::MakeShape(dims, 2, &output_shape);
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape, top_box_tensor));

  dims[1] = 7;
  TensorShape output_shape_1;
  TensorShapeUtils::MakeShape(dims, 2, &output_shape_1);
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape_1, top_pose_tensor));

  dims[1] = 4 * num_classes;
  TensorShape output_shape_2;
  TensorShapeUtils::MakeShape(dims, 2, &output_shape_2);
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape_2, top_target_tensor));
  OP_REQUIRES_OK(context, context->allocate_temp(DT_FLOAT, output_shape_2, top_weight_tensor));

  TensorShape output_shape_3;
  TensorShapeUtils::MakeShape(&num, 1, &output_shape_3);
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_3, top_domain_tensor));

  int len = 1;
  TensorShape output_shape_4;
  TensorShapeUtils::MakeShape(&len, 1, &output_shape_4);
  OP_REQUIRES_OK(context, context->allocate_temp(DT_INT32, output_shape_4, top_rois_tensor));
}

void reset_outputs(float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain, int* num_rois, int num_classes);
void copy_num_rois(int* num_rois, int* num_rois_device);

void copy_outputs(float* top_box, float* top_pose, float* top_target, float* top_weight, int* top_domain,
  float* top_box_final, float* top_pose_final, float* top_target_final, float* top_weight_final, int* top_domain_final, int num_classes, int num_rois);

void set_gradients(float* top_label, float* top_vertex, int batch_size, int height, int width, int num_classes);

template <typename Device, typename T>
class HoughvotinggpuOp : public OpKernel {
 public:
  explicit HoughvotinggpuOp(OpKernelConstruction* context) : OpKernel(context) {
    // Get the pool height
    OP_REQUIRES_OK(context,
                   context->GetAttr("is_train", &is_train_));
    // Check that pooled_height is positive
    OP_REQUIRES(context, is_train_ >= 0,
                errors::InvalidArgument("Need is_train >= 0, got ",
                                        is_train_));
    OP_REQUIRES_OK(context,
                   context->GetAttr("threshold_vote", &threshold_vote_));
    OP_REQUIRES_OK(context,
                   context->GetAttr("skip_pixels", &skip_pixels_));

  }

  // bottom_label: (batch_size, height, width)
  // bottom_vertex: (batch_size, height, width, 3 * num_classes)
  // top_box: (num, 7) i.e., batch_index, cls, x1, y1, x2, y2, score
  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);
    const Tensor& bottom_extents = context->input(2);

    // format of the meta_data
    // intrinsic matrix: meta_data[0 ~ 8]
    // inverse intrinsic matrix: meta_data[9 ~ 17]
    // pose_world2live: meta_data[18 ~ 29]
    // pose_live2world: meta_data[30 ~ 41]
    // voxel step size: meta_data[42, 43, 44]
    // voxel min value: meta_data[45, 46, 47]
    const Tensor& bottom_meta_data = context->input(3);
    auto meta_data = bottom_meta_data.flat<T>();

    const Tensor& bottom_gt = context->input(4);
    const float* gt = bottom_gt.flat<float>().data();

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;
    int num_meta_data = bottom_meta_data.dim_size(3);
    int num_gt = bottom_gt.dim_size(0);

    // for each image, run hough voting
    std::vector<cv::Vec<float, 14> > outputs;
    const float* extents = bottom_extents.flat<float>().data();

    // bb3Ds
    std::vector<std::vector<cv::Point3f>> bb3Ds;
    getBb3Ds(extents, bb3Ds, num_classes);

    int index_meta_data = 0;
    float fx, fy, px, py;
    for (int n = 0; n < batch_size; n++)
    {
      const int* labelmap = bottom_label.flat<int>().data() + n * height * width;
      const float* vertmap = bottom_vertex.flat<float>().data() + n * height * width * VERTEX_CHANNELS * num_classes;
      fx = meta_data(index_meta_data + 0);
      fy = meta_data(index_meta_data + 4);
      px = meta_data(index_meta_data + 2);
      py = meta_data(index_meta_data + 5);
      hough_voting(labelmap, vertmap, bb3Ds, n, height, width, num_classes, is_train_, fx, fy, px, py, outputs);
      index_meta_data += num_meta_data;
    }

    if (outputs.size() == 0)
    {
      std::cout << "no detection" << std::endl;
      // add a dummy detection to the output
      cv::Vec<float, 14> roi;
      roi(0) = 0;
      roi(1) = -1;
      outputs.push_back(roi);
    }

    // Create output tensors
    // top_box
    int dims[2];
    dims[0] = outputs.size();
    dims[1] = 7;
    TensorShape output_shape;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape);

    Tensor* top_box_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_box_tensor));
    float* top_box = top_box_tensor->template flat<float>().data();

    // top_pose
    dims[1] = 7;
    TensorShape output_shape_pose;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_pose);

    Tensor* top_pose_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_pose, &top_pose_tensor));
    float* top_pose = top_pose_tensor->template flat<float>().data();

    // top target
    dims[1] = 4 * num_classes;
    TensorShape output_shape_target;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_target);

    Tensor* top_target_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(2, output_shape_target, &top_target_tensor));
    float* top_target = top_target_tensor->template flat<float>().data();
    memset(top_target, 0, outputs.size() * 4 * num_classes *sizeof(T));

    // top weight
    Tensor* top_weight_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(3, output_shape_target, &top_weight_tensor));
    float* top_weight = top_weight_tensor->template flat<float>().data();
    memset(top_weight, 0, outputs.size() * 4 * num_classes *sizeof(T));

    // top domain
    int num = outputs.size();
    TensorShape output_shape_domain;
    TensorShapeUtils::MakeShape(&num, 1, &output_shape_domain);
    Tensor* top_domain_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(4, output_shape_domain, &top_domain_tensor));
    int* top_domain = top_domain_tensor->template flat<int>().data();
    memset(top_domain, 0, outputs.size() * sizeof(int));

    for(int n = 0; n < outputs.size(); n++)
    {
      cv::Vec<float, 14> roi = outputs[n];

      for (int i = 0; i < 7; i++)
        top_box[n * 7 + i] = roi(i);

      for (int i = 0; i < 7; i++)
        top_pose[n * 7 + i] = roi(7 + i);

      if (num_gt == 0)
        top_domain[n] = 1;
      else
        top_domain[n] = 0;
    }

    if (is_train_)
      compute_target_weight(height, width, top_target, top_weight, bb3Ds, gt, num_gt, num_classes, fx, fy, px, py, outputs);
  }
 private:
  int is_train_;
  int threshold_vote_;
  int skip_pixels_;
};

REGISTER_KERNEL_BUILDER(Name("Houghvotinggpu").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotinggpuOp<CPUDevice, float>);

template <class T>
class HoughvotinggpuOp<Eigen::GpuDevice, T> : public OpKernel {
 public:
  typedef Eigen::GpuDevice Device;

  explicit HoughvotinggpuOp(OpKernelConstruction* context) : OpKernel(context)
  {
    // Get the pool height
    OP_REQUIRES_OK(context,
                   context->GetAttr("is_train", &is_train_));
    // Check that pooled_height is positive
    OP_REQUIRES(context, is_train_ >= 0,
                errors::InvalidArgument("Need is_train >= 0, got ",
                                        is_train_));
    OP_REQUIRES_OK(context,
                   context->GetAttr("threshold_vote", &threshold_vote_));
    OP_REQUIRES_OK(context,
                   context->GetAttr("skip_pixels", &skip_pixels_));
  }

  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    const Tensor& bottom_extents = context->input(2);
    const float* extents = bottom_extents.flat<float>().data();

    // format of the meta_data
    // intrinsic matrix: meta_data[0 ~ 8]
    // inverse intrinsic matrix: meta_data[9 ~ 17]
    // pose_world2live: meta_data[18 ~ 29]
    // pose_live2world: meta_data[30 ~ 41]
    // voxel step size: meta_data[42, 43, 44]
    // voxel min value: meta_data[45, 46, 47]
    const Tensor& bottom_meta_data = context->input(3);

    const Tensor& bottom_gt = context->input(4);
    const float* gt = bottom_gt.flat<float>().data();

    int batch_size = bottom_label.dim_size(0);
    int height = bottom_label.dim_size(1);
    int width = bottom_label.dim_size(2);
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;
    int num_meta_data = bottom_meta_data.dim_size(3);
    int num_gt = bottom_gt.dim_size(0);

    float inlierThreshold = 0.9;
    int labelThreshold = 500;
    Tensor top_box_tensor_tmp, top_pose_tensor_tmp, top_target_tensor_tmp, top_weight_tensor_tmp, top_domain_tensor_tmp, num_rois_tensor_tmp;
    allocate_outputs(context, &top_box_tensor_tmp, &top_pose_tensor_tmp, &top_target_tensor_tmp, &top_weight_tensor_tmp,
      &top_domain_tensor_tmp, &num_rois_tensor_tmp, num_classes);
    float* top_box = top_box_tensor_tmp.flat<float>().data();
    float* top_pose = top_pose_tensor_tmp.flat<float>().data();
    float* top_target = top_target_tensor_tmp.flat<float>().data();
    float* top_weight = top_weight_tensor_tmp.flat<float>().data();
    int* top_domain = top_domain_tensor_tmp.flat<int>().data();
    int* num_rois_device = num_rois_tensor_tmp.flat<int>().data();
    reset_outputs(top_box, top_pose, top_target, top_weight, top_domain, num_rois_device, num_classes);

    for (int n = 0; n < batch_size; n++)
    {
      const int* labelmap = bottom_label.flat<int>().data() + n * height * width;
      const float* vertmap = bottom_vertex.flat<float>().data() + n * height * width * VERTEX_CHANNELS * num_classes;
      const float* meta_data = bottom_meta_data.flat<float>().data() + n * num_meta_data;
      HoughVotingLaucher(context, labelmap, vertmap, extents, meta_data, gt, n, height, width, num_classes, num_gt,
        is_train_, inlierThreshold, labelThreshold, threshold_vote_, skip_pixels_,
        top_box, top_pose, top_target, top_weight, top_domain, num_rois_device, context->eigen_device<Eigen::GpuDevice>());
    }

    int num_rois;
    copy_num_rois(&num_rois, num_rois_device);
    // dummy output
    if (num_rois == 0)
      num_rois = 1;

    // Create output tensors
    // top_box
    int dims[2];
    dims[0] = num_rois;
    dims[1] = 7;
    TensorShape output_shape;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape);

    Tensor* top_box_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_box_tensor));
    float* top_box_final = top_box_tensor->flat<float>().data();

    // top_pose
    dims[1] = 7;
    TensorShape output_shape_pose;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_pose);

    Tensor* top_pose_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_pose, &top_pose_tensor));
    float* top_pose_final = top_pose_tensor->flat<float>().data();

    // top target
    dims[1] = 4 * num_classes;
    TensorShape output_shape_target;
    TensorShapeUtils::MakeShape(dims, 2, &output_shape_target);

    Tensor* top_target_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(2, output_shape_target, &top_target_tensor));
    float* top_target_final = top_target_tensor->flat<float>().data();

    // top weight
    Tensor* top_weight_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(3, output_shape_target, &top_weight_tensor));
    float* top_weight_final = top_weight_tensor->flat<float>().data();

    // top domain
    TensorShape output_shape_domain;
    TensorShapeUtils::MakeShape(&num_rois, 1, &output_shape_domain);
    Tensor* top_domain_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(4, output_shape_domain, &top_domain_tensor));
    int* top_domain_final = top_domain_tensor->template flat<int>().data();

    copy_outputs(top_box, top_pose, top_target, top_weight, top_domain, top_box_final,
      top_pose_final, top_target_final, top_weight_final, top_domain_final, num_classes, num_rois);
  }
 private:
  int is_train_;
  int threshold_vote_;
  int skip_pixels_;
};

REGISTER_KERNEL_BUILDER(Name("Houghvotinggpu").Device(DEVICE_GPU).TypeConstraint<float>("T"), HoughvotinggpuOp<Eigen::GpuDevice, float>);

// compute gradient
template <class Device, class T>
class HoughvotinggpuGradOp : public OpKernel {
 public:
  explicit HoughvotinggpuGradOp(OpKernelConstruction* context) : OpKernel(context) {
  }

  void Compute(OpKernelContext* context) override
  {
    // Grab the input tensor
    const Tensor& bottom_label = context->input(0);
    const Tensor& bottom_vertex = context->input(1);

    // data should have 5 dimensions.
    OP_REQUIRES(context, bottom_label.dims() == 3,
                errors::InvalidArgument("label must be 3-dimensional"));

    OP_REQUIRES(context, bottom_vertex.dims() == 4,
                errors::InvalidArgument("vertex must be 4-dimensional"));

    // batch size
    int batch_size = bottom_label.dim_size(0);
    // height
    int height = bottom_label.dim_size(1);
    // width
    int width = bottom_label.dim_size(2);
    // num of classes
    int num_classes = bottom_vertex.dim_size(3) / VERTEX_CHANNELS;

    // construct the output shape
    TensorShape output_shape = bottom_label.shape();
    Tensor* top_label_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(0, output_shape, &top_label_tensor));
    float* top_label = top_label_tensor->flat<float>().data();

    TensorShape output_shape_1 = bottom_vertex.shape();
    Tensor* top_vertex_tensor = NULL;
    OP_REQUIRES_OK(context, context->allocate_output(1, output_shape_1, &top_vertex_tensor));
    float* top_vertex = top_vertex_tensor->flat<float>().data();

    set_gradients(top_label, top_vertex, batch_size, height, width, num_classes);
  }
};

// REGISTER_KERNEL_BUILDER(Name("HoughvotinggpuGrad").Device(DEVICE_CPU).TypeConstraint<float>("T"), HoughvotinggpuGradOp<CPUDevice, float>);
REGISTER_KERNEL_BUILDER(Name("HoughvotinggpuGrad").Device(DEVICE_GPU).TypeConstraint<float>("T"), HoughvotinggpuGradOp<Eigen::GpuDevice, float>);

void hough_voting(const int* labelmap, const float* vertmap, std::vector<std::vector<cv::Point3f>> bb3Ds,
  int batch, int height, int width, int num_classes, int is_train,
  float fx, float fy, float px, float py, std::vector<cv::Vec<float, 14> >& outputs)
{
  float inlierThreshold = 0.9;
  int votingThreshold = 50;

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  // initialize hough space
  int* hough_space = (int*)malloc(sizeof(int) * height * width * num_classes);
  memset(hough_space, 0, height * width * num_classes);

  int* flags = (int*)malloc(sizeof(int) * num_classes);
  memset(flags, 0, num_classes);

  // for each pixel
  for (int x = 0; x < width; x++)
  {
    for (int y = 0; y < height; y++)
    {
      int c = labelmap[y * width + x];
      if (c > 0)
      {
        flags[c] = 1;
        // read the predict center direction
        int offset = VERTEX_CHANNELS * c + VERTEX_CHANNELS * num_classes * (y * width + x);
        float u = vertmap[offset];
        float v = vertmap[offset + 1];
        float norm = sqrt(u * u + v * v);
        u /= norm;
        v /= norm;

        // voting
	float delta = 1.0 / fabs(u);
        float cx = x;
        float cy = y;
        while(1)
        {
          cx += delta * u;
          cy += delta * v;
          int center_x = int(cx);
          int center_y = int(cy);
          if (center_x >= 0 && center_x < width && center_y >= 0 && center_y < height)
          {
            offset = c + num_classes * (center_y * width + center_x);
            hough_space[offset] += 1;
          }
          else
            break;
        }
      }
    }
  }

  // find the maximum in hough space
  for (int c = 1; c < num_classes; c++)
  {
    if (flags[c])
    {
      int max_vote = 0;
      int max_x, max_y;
      for (int x = 0; x < width; x++)
      {
        for (int y = 0; y < height; y++)
        {
          int offset = c + num_classes * (y * width + x);
          if (hough_space[offset] > max_vote)
          {
            max_vote = hough_space[offset];
            max_x = x;
            max_y = y;
          }
        }
      }
      if (max_vote < votingThreshold)
        continue;

      // center
      cv::Point2f center(max_x, max_y);
      int bb_width, bb_height;
      float bb_distance;
      compute_width_height(labelmap, vertmap, center, bb3Ds, camMat, inlierThreshold, height, width, c, num_classes, bb_width, bb_height, bb_distance);

      // construct output
      cv::Vec<float, 14> roi;
      roi(0) = batch;
      roi(1) = c;

      // bounding box
      float scale = 0.05;
      roi(2) = center.x - bb_width * (0.5 + scale);
      roi(3) = center.y - bb_height * (0.5 + scale);
      roi(4) = center.x + bb_width * (0.5 + scale);
      roi(5) = center.y + bb_height * (0.5 + scale);

      // score
      roi(6) = max_vote;

      // pose
      float rx = (center.x - px) / fx;
      float ry = (center.y - py) / fy;
      roi(7) = 1;
      roi(8) = 0;
      roi(9) = 0;
      roi(10) = 0;
      roi(11) = rx * bb_distance;
      roi(12) = ry * bb_distance;
      roi(13) = bb_distance;

      outputs.push_back(roi);

      if (is_train)
      {
        // add jittering rois
        float x1 = roi(2);
        float y1 = roi(3);
        float x2 = roi(4);
        float y2 = roi(5);
        float ww = x2 - x1;
        float hh = y2 - y1;

        // (-1, -1)
        roi(2) = x1 - 0.05 * ww;
        roi(3) = y1 - 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (+1, -1)
        roi(2) = x1 + 0.05 * ww;
        roi(3) = y1 - 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (-1, +1)
        roi(2) = x1 - 0.05 * ww;
        roi(3) = y1 + 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (+1, +1)
        roi(2) = x1 + 0.05 * ww;
        roi(3) = y1 + 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (0, -1)
        roi(2) = x1;
        roi(3) = y1 - 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (-1, 0)
        roi(2) = x1 - 0.05 * ww;
        roi(3) = y1;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (0, +1)
        roi(2) = x1;
        roi(3) = y1 + 0.05 * hh;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);

        // (+1, 0)
        roi(2) = x1 + 0.05 * ww;
        roi(3) = y1;
        roi(4) = roi(2) + ww;
        roi(5) = roi(3) + hh;
        outputs.push_back(roi);
      }
    }
  }
}

inline float angle_distance(cv::Point2f x, cv::Point2f n, cv::Point2f p)
{
  return n.dot(x - p) / (cv::norm(n) * cv::norm(x - p));
}

inline void compute_width_height(const int* labelmap, const float* vertmap, cv::Point2f center,
  std::vector<std::vector<cv::Point3f>> bb3Ds, cv::Mat camMat, float inlierThreshold,
  int height, int width, int channel, int num_classes, int & bb_width, int & bb_height, float & bb_distance)
{
  float d = 0;
  int count = 0;

  // for each pixel
  std::vector<float> dx;
  std::vector<float> dy;
  for (int x = 0; x < width; x++)
  {
    for (int y = 0; y < height; y++)
    {
      if (labelmap[y * width + x] == channel)
      {
        cv::Point2f point(x, y);

        // read out object coordinate
        int offset = VERTEX_CHANNELS * channel + VERTEX_CHANNELS * num_classes * (y * width + x);
        float u = vertmap[offset];
        float v = vertmap[offset + 1];
        float distance = exp(vertmap[offset + 2]);
        float norm = sqrt(u * u + v * v);
        u /= norm;
        v /= norm;
        cv::Point2f direction(u, v);

        // inlier check
        if(angle_distance(center, direction, point) > inlierThreshold)
        {
          dx.push_back(fabs(point.x - center.x));
          dy.push_back(fabs(point.y - center.y));
          d += distance;
          count++;
        }
      }
    }
  }
  bb_distance = d / count;

  // estimate a projection
  cv::Mat tvec(3, 1, CV_64F);
  cv::Mat rvec(3, 1, CV_64F);
  for(int i = 0; i < 3; i++)
  {
    tvec.at<double>(i, 0) = 0;
    rvec.at<double>(i, 0) = 0;
  }
  tvec.at<double>(2, 0) = bb_distance;
  std::vector<cv::Point2f> bb2D;
  cv::projectPoints(bb3Ds[channel-1], rvec, tvec, camMat, cv::Mat(), bb2D);

  // get min-max of projected vertices
  int minX = 1e8;
  int maxX = -1e8;
  int minY = 1e8;
  int maxY = -1e8;
  for(int i = 0; i < bb2D.size(); i++)
  {
    minX = std::min((float) minX, bb2D[i].x);
    minY = std::min((float) minY, bb2D[i].y);
    maxX = std::max((float) maxX, bb2D[i].x);
    maxY = std::max((float) maxY, bb2D[i].y);
  }
  cv::Rect bb = cv::Rect(0, 0, (maxX - minX + 1), (maxY - minY + 1));

  std::vector<float>::iterator it;
  it = std::remove_if(dx.begin(), dx.end(), std::bind2nd(std::greater<float>(), std::max(bb.width, bb.height) ));
  dx.erase(it, dx.end());

  it = std::remove_if(dy.begin(), dy.end(), std::bind2nd(std::greater<float>(), std::max(bb.width, bb.height) ));
  dy.erase(it, dy.end());

  std::sort(dx.begin(), dx.end());
  std::sort(dy.begin(), dy.end());

  bb_width = 2 * dx[int(dx.size() * 0.95)];
  bb_height = 2 * dy[int(dy.size() * 0.95)];
}

// compute the pose target and weight
void compute_target_weight(int height, int width, float* target, float* weight, std::vector<std::vector<cv::Point3f>> bb3Ds,
  const float* poses_gt, int num_gt, int num_classes, float fx, float fy, float px, float py, std::vector<cv::Vec<float, 14> > outputs)
{
  int num = outputs.size();
  float threshold = 0.2;

  // camera matrix
  cv::Mat_<float> camMat = cv::Mat_<float>::zeros(3, 3);
  camMat(0, 0) = fx;
  camMat(1, 1) = fy;
  camMat(2, 2) = 1.f;
  camMat(0, 2) = px;
  camMat(1, 2) = py;

  // compute the gt boxes
  std::vector<cv::Rect> bb2Ds_gt(num_gt);
  for (int i = 0; i < num_gt; i++)
  {
    Eigen::Quaternionf quaternion(poses_gt[i * 13 + 6], poses_gt[i * 13 + 7], poses_gt[i * 13 + 8], poses_gt[i * 13 + 9]);
    Eigen::Matrix3f rmatrix = quaternion.toRotationMatrix();
    cv::Mat rmat_trans = cv::Mat(3, 3, CV_32F, rmatrix.data());
    cv::Mat rmat;
    cv::transpose(rmat_trans, rmat);
    cv::Mat rvec(3, 1, CV_64F);
    cv::Rodrigues(rmat, rvec);
    cv::Mat tvec(3, 1, CV_64F);
    tvec.at<double>(0, 0) = poses_gt[i * 13 + 10];
    tvec.at<double>(1, 0) = poses_gt[i * 13 + 11];
    tvec.at<double>(2, 0) = poses_gt[i * 13 + 12];

    int objID = int(poses_gt[i * 13 + 1]);
    std::vector<cv::Point3f> bb3D = bb3Ds[objID-1];
    bb2Ds_gt[i] = getBB2D(width, height, bb3D, camMat, rvec, tvec);
  }

  for (int i = 0; i < num; i++)
  {
    cv::Vec<float, 14> roi = outputs[i];
    int batch_id = int(roi(0));
    int class_id = int(roi(1));

    // find the gt index
    int gt_ind = -1;
    for (int j = 0; j < num_gt; j++)
    {
      int gt_batch = int(poses_gt[j * 13 + 0]);
      int gt_id = int(poses_gt[j * 13 + 1]);
      if(class_id == gt_id && batch_id == gt_batch)
      {
        gt_ind = j;
        break;
      }
    }

    if (gt_ind == -1)
      continue;

    // compute bounding box overlap
    float x1 = roi(2);
    float y1 = roi(3);
    float x2 = roi(4);
    float y2 = roi(5);
    cv::Rect bb2D(x1, y1, x2-x1, y2-y1);

    float overlap = getIoU(bb2D, bb2Ds_gt[gt_ind]);
    if (overlap < threshold)
      continue;

    target[i * 4 * num_classes + 4 * class_id + 0] = poses_gt[gt_ind * 13 + 6];
    target[i * 4 * num_classes + 4 * class_id + 1] = poses_gt[gt_ind * 13 + 7];
    target[i * 4 * num_classes + 4 * class_id + 2] = poses_gt[gt_ind * 13 + 8];
    target[i * 4 * num_classes + 4 * class_id + 3] = poses_gt[gt_ind * 13 + 9];

    weight[i * 4 * num_classes + 4 * class_id + 0] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 1] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 2] = 1;
    weight[i * 4 * num_classes + 4 * class_id + 3] = 1;
  }
}

// get 3D bounding boxes
void getBb3Ds(const float* extents, std::vector<std::vector<cv::Point3f>>& bb3Ds, int num_classes)
{
  // for each object
  for (int i = 1; i < num_classes; i++)
  {
    cv::Vec<float, 3> extent;
    extent(0) = extents[i * 3];
    extent(1) = extents[i * 3 + 1];
    extent(2) = extents[i * 3 + 2];

    bb3Ds.push_back(getBB3D(extent));
  }
}

inline std::vector<cv::Point3f> getBB3D(const cv::Vec<float, 3>& extent)
{
  std::vector<cv::Point3f> bb;
  float xHalf = extent[0] * 0.5;
  float yHalf = extent[1] * 0.5;
  float zHalf = extent[2] * 0.5;

  bb.push_back(cv::Point3f(xHalf, yHalf, zHalf));
  bb.push_back(cv::Point3f(-xHalf, yHalf, zHalf));
  bb.push_back(cv::Point3f(xHalf, -yHalf, zHalf));
  bb.push_back(cv::Point3f(-xHalf, -yHalf, zHalf));

  bb.push_back(cv::Point3f(xHalf, yHalf, -zHalf));
  bb.push_back(cv::Point3f(-xHalf, yHalf, -zHalf));
  bb.push_back(cv::Point3f(xHalf, -yHalf, -zHalf));
  bb.push_back(cv::Point3f(-xHalf, -yHalf, -zHalf));

  return bb;
}

inline cv::Rect getBB2D(int imageWidth, int imageHeight, const std::vector<cv::Point3f>& bb3D, const cv::Mat& camMat, const cv::Mat& rvec, const cv::Mat& tvec)
{
  // project 3D bounding box vertices into the image
  std::vector<cv::Point2f> bb2D;
  cv::projectPoints(bb3D, rvec, tvec, camMat, cv::Mat(), bb2D);

  // get min-max of projected vertices
  int minX = imageWidth - 1;
  int maxX = 0;
  int minY = imageHeight - 1;
  int maxY = 0;

  for(unsigned j = 0; j < bb2D.size(); j++)
  {
    minX = std::min((float) minX, bb2D[j].x);
    minY = std::min((float) minY, bb2D[j].y);
    maxX = std::max((float) maxX, bb2D[j].x);
    maxY = std::max((float) maxY, bb2D[j].y);
  }

  // clamp at image border
  minX = clamp(minX, 0, imageWidth - 1);
  maxX = clamp(maxX, 0, imageWidth - 1);
  minY = clamp(minY, 0, imageHeight - 1);
  maxY = clamp(maxY, 0, imageHeight - 1);

  return cv::Rect(minX, minY, (maxX - minX + 1), (maxY - minY + 1));
}

inline float getIoU(const cv::Rect& bb1, const cv::Rect bb2)
{
  cv::Rect intersection = bb1 & bb2;
  return (intersection.area() / (float) (bb1.area() + bb2.area() - intersection.area()));
}


/**
 *    Copyright (C) 2015 MongoDB, Inc.
 *
 *    This program is free software: you can redistribute it and/or  modify
 *    it under the terms of the GNU Affero General Public License, version 3,
 *    as published by the Free Software Foundation.
 *
 *    This program 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 Affero General Public License for more details.
 *
 *    You should have received a copy of the GNU Affero General Public License
 *    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *    As a special exception, the copyright holders give permission to link the
 *    code of portions of this program with the OpenSSL library under certain
 *    conditions as described in each individual source file and distribute
 *    linked combinations including the program with the OpenSSL library. You
 *    must comply with the GNU Affero General Public License in all respects for
 *    all of the code used other than as permitted herein. If you modify file(s)
 *    with this exception, you may extend this exception to your version of the
 *    file(s), but you are not obligated to do so. If you do not wish to do so,
 *    delete this exception statement from your version. If you delete this
 *    exception statement from all source files in the program, then also delete
 *    it in the license file.
 */

#define MONGO_LOG_DEFAULT_COMPONENT ::mongo::logger::LogComponent::kQuery

#include "mongo/platform/basic.h"

#include "mongo/db/pipeline/document_source_sample_from_random_cursor.h"

#include <boost/math/distributions/beta.hpp>

#include "mongo/db/client.h"
#include "mongo/db/pipeline/document.h"
#include "mongo/db/pipeline/expression.h"
#include "mongo/db/pipeline/expression_context.h"
#include "mongo/db/pipeline/value.h"
#include "mongo/util/log.h"

namespace mongo {
using boost::intrusive_ptr;

DocumentSourceSampleFromRandomCursor::DocumentSourceSampleFromRandomCursor(
    const intrusive_ptr<ExpressionContext>& pExpCtx,
    long long size,
    std::string idField,
    long long nDocsInCollection)
    : DocumentSource(pExpCtx),
      _size(size),
      _idField(std::move(idField)),
      _seenDocs(pExpCtx->getValueComparator().makeUnorderedValueSet()),
      _nDocsInColl(nDocsInCollection) {}

const char* DocumentSourceSampleFromRandomCursor::getSourceName() const {
    return "$sampleFromRandomCursor";
}

namespace {
/**
 * Select a random value drawn according to the distribution Beta(alpha=1, beta=N). The kth smallest
 * value of a sample of size N from a Uniform(0, 1) distribution has a Beta(k, N + 1 - k)
 * distribution, so the return value represents the smallest value from such a sample. This is also
 * the expected distance between the values drawn from a uniform distribution, which is how it is
 * being used here.
 */
double smallestFromSampleOfUniform(PseudoRandom* prng, size_t N) {
    boost::math::beta_distribution<double> betaDist(1.0, static_cast<double>(N));
    double p = prng->nextCanonicalDouble();
    return boost::math::quantile(betaDist, p);
}
}  // namespace

DocumentSource::GetNextResult DocumentSourceSampleFromRandomCursor::getNext() {
    pExpCtx->checkForInterrupt();

    if (_seenDocs.size() >= static_cast<size_t>(_size))
        return GetNextResult::makeEOF();

    auto nextResult = getNextNonDuplicateDocument();
    if (!nextResult.isAdvanced()) {
        return nextResult;
    }

    // Assign it a random value to enable merging by random value, attempting to avoid bias in that
    // process.
    auto& prng = pExpCtx->opCtx->getClient()->getPrng();
    _randMetaFieldVal -= smallestFromSampleOfUniform(&prng, _nDocsInColl);

    MutableDocument md(nextResult.releaseDocument());
    md.setRandMetaField(_randMetaFieldVal);
    if (pExpCtx->needsMerge) {
        // This stage will be merged by sorting results according to this random metadata field, but
        // the merging logic expects to sort by the sort key metadata.
        md.setSortKeyMetaField(BSON("" << _randMetaFieldVal));
    }
    return md.freeze();
}

DocumentSource::GetNextResult DocumentSourceSampleFromRandomCursor::getNextNonDuplicateDocument() {
    // We may get duplicate documents back from the random cursor, and should not return duplicate
    // documents, so keep trying until we get a new one.
    const int kMaxAttempts = 100;
    for (int i = 0; i < kMaxAttempts; ++i) {
        auto nextInput = pSource->getNext();
        switch (nextInput.getStatus()) {
            case GetNextResult::ReturnStatus::kAdvanced: {
                auto idField = nextInput.getDocument()[_idField];
                uassert(28793,
                        str::stream()
                            << "The optimized $sample stage requires all documents have a "
                            << _idField
                            << " field in order to de-duplicate results, but encountered a "
                               "document without a "
                            << _idField
                            << " field: "
                            << nextInput.getDocument().toString(),
                        !idField.missing());

                if (_seenDocs.insert(std::move(idField)).second) {
                    return nextInput;
                }
                LOG(1) << "$sample encountered duplicate document: "
                       << nextInput.getDocument().toString();
                break;  // Try again with the next document.
            }
            case GetNextResult::ReturnStatus::kPauseExecution: {
                MONGO_UNREACHABLE;  // Our input should be a random cursor, which should never
                                    // result in kPauseExecution.
            }
            case GetNextResult::ReturnStatus::kEOF: {
                return nextInput;
            }
        }
    }
    uasserted(28799,
              str::stream() << "$sample stage could not find a non-duplicate document after "
                            << kMaxAttempts
                            << " while using a random cursor. This is likely a "
                               "sporadic failure, please try again.");
}

Value DocumentSourceSampleFromRandomCursor::serialize(
    boost::optional<ExplainOptions::Verbosity> explain) const {
    return Value(DOC(getSourceName() << DOC("size" << _size)));
}

DocumentSource::GetDepsReturn DocumentSourceSampleFromRandomCursor::getDependencies(
    DepsTracker* deps) const {
    deps->fields.insert(_idField);
    return SEE_NEXT;
}

intrusive_ptr<DocumentSourceSampleFromRandomCursor> DocumentSourceSampleFromRandomCursor::create(
    const intrusive_ptr<ExpressionContext>& expCtx,
    long long size,
    std::string idField,
    long long nDocsInCollection) {
    intrusive_ptr<DocumentSourceSampleFromRandomCursor> source(
        new DocumentSourceSampleFromRandomCursor(expCtx, size, idField, nDocsInCollection));
    return source;
}
}  // mongo


/* Copyright © 2019 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at
 * https://opensource.org/licenses/BSD-3-Clause
 */

#define BOOST_TEST_MODULE

#include <boost/test/unit_test.hpp>
#include <core/util/test_macros.hpp>
#include <iostream>

#ifdef HAS_MACOS_10_15

struct style_transfer_test {
 public:
  /**
    Test Encoding

    This correctness test checks whether the Style Transfer Encoding layer
    present in TCMPS is performing as intended by comparing it to the golden
    set.
   */
  void test_encoding() {
    TS_ASSERT(true);  // TODO
  }
  /**
    Test Residual

    This correctness test checks whether the Style Transfer Residual layer
    present in TCMPS is performing as intended by comparing it to the golden
    set.
   */
  void test_residual() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Test Decoding

    This correctness test checks whether the Style Transfer Decoding layer
    present in TCMPS is performing as intended by comparing it to the golden
    set.
   */
  void test_decode() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Test Resnet

    This correctness test checks whether the Style Transfer Transformer network
    present in TCMPS is performing as intended by comparing it to the golden
    set.
   */
  void test_resnet() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Test Block 1

    This correctness test checks whether the VGG Block 1 Layer present in TCMPS
    is performing as intended by comparing it to the golden set.
   */
  void test_block_1() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Test Block 2

    This correctness test checks whether the VGG Block 2 Layer present in TCMPS
    is performing as intended by comparing it to the golden set.
   */
  void test_block_2() {
    TS_ASSERT(true);  // TODO
  }

  /**
    VGG 16

    This correctness test checks whether the VGG Model present in TCMPS is
    performing as intended by comparing it to the golden set.
   */
  void test_vgg16() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Loss

    This test checks whether the Loss out of the total graph defined in TCMPS is
    performing as intended by comparing it to the golden set.
   */
  void test_loss() {
    TS_ASSERT(true);  // TODO
  }

  /**
    Backwards

    This test checks whether the Weight Update in the total graph defined in
    TCMPS is performing as intended by comparing it to the golden set.
   */
  void test_backwards() {
    TS_ASSERT(true);  // TODO
  }
};

#endif

BOOST_FIXTURE_TEST_SUITE(_style_transfer_test, style_transfer_test)

#ifdef HAS_MACOS_10_15

BOOST_AUTO_TEST_CASE(test_encoding) { style_transfer_test::test_encoding(); }

BOOST_AUTO_TEST_CASE(test_residual) { style_transfer_test::test_residual(); }

BOOST_AUTO_TEST_CASE(test_decode) { style_transfer_test::test_decode(); }

BOOST_AUTO_TEST_CASE(test_resnet) { style_transfer_test::test_resnet(); }

BOOST_AUTO_TEST_CASE(test_block_1) { style_transfer_test::test_block_1(); }

BOOST_AUTO_TEST_CASE(test_block_2) { style_transfer_test::test_block_2(); }

BOOST_AUTO_TEST_CASE(test_vgg16) { style_transfer_test::test_vgg16(); }

BOOST_AUTO_TEST_CASE(test_loss) { style_transfer_test::test_loss(); }

BOOST_AUTO_TEST_CASE(test_backwards) { style_transfer_test::test_backwards(); }

#endif

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file descartes_robot_sampler.hpp
 * @brief Tesseract Descartes Kinematics Sampler Implementation
 *
 * @author Levi Armstrong
 * @date April 18, 2018
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2017, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * 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.
 */
#ifndef TESSERACT_MOTION_PLANNERS_DESCARTES_ROBOT_SAMPLER_HPP
#define TESSERACT_MOTION_PLANNERS_DESCARTES_ROBOT_SAMPLER_HPP

#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <console_bridge/console.h>
#include <Eigen/Geometry>
#include <vector>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

#include <tesseract_motion_planners/descartes/descartes_robot_sampler.h>
#include <tesseract_kinematics/core/utils.h>

namespace tesseract_planning
{
template <typename FloatType>
DescartesRobotSampler<FloatType>::DescartesRobotSampler(std::string target_working_frame,
                                                        const Eigen::Isometry3d& target_pose,
                                                        PoseSamplerFn target_pose_sampler,
                                                        tesseract_kinematics::KinematicGroup::ConstPtr manip,
                                                        DescartesCollision::Ptr collision,
                                                        std::string tcp_frame,
                                                        const Eigen::Isometry3d& tcp_offset,
                                                        bool allow_collision,
                                                        DescartesVertexEvaluator::Ptr is_valid,
                                                        bool use_redundant_joint_solutions)
  : target_working_frame_(std::move(target_working_frame))
  , target_pose_(target_pose)
  , target_pose_sampler_(std::move(target_pose_sampler))
  , manip_(std::move(manip))
  , collision_(std::move(collision))
  , tcp_frame_(std::move(tcp_frame))
  , tcp_offset_(tcp_offset)
  , allow_collision_(allow_collision)
  , dof_(static_cast<int>(manip_->numJoints()))
  , ik_seed_(Eigen::VectorXd::Zero(dof_))
  , is_valid_(std::move(is_valid))
  , use_redundant_joint_solutions_(use_redundant_joint_solutions)
{
  if (!allow_collision_ && !collision_)
    throw std::runtime_error("Collision checker must not be a nullptr if collisions are not allowed during planning");
}

template <typename FloatType>
std::vector<descartes_light::StateSample<FloatType>> DescartesRobotSampler<FloatType>::sample() const
{
  // Generate all possible Cartesian poses
  tesseract_common::VectorIsometry3d target_poses = target_pose_sampler_(target_pose_);

  // Generate the IK solutions for those poses
  std::vector<descartes_light::StateSample<FloatType>> samples;
  for (const auto& pose : target_poses)
  {
    // Get the transformation to the kinematic tip link
    Eigen::Isometry3d target_pose = pose * tcp_offset_.inverse();

    // Solve IK (TODO Should tcp_offset be stored in KinGroupIKInput?)
    tesseract_kinematics::KinGroupIKInput ik_input(target_pose, target_working_frame_, tcp_frame_);
    tesseract_kinematics::IKSolutions ik_solutions = manip_->calcInvKin({ ik_input }, ik_seed_);

    if (ik_solutions.empty())
      continue;

    // Check each individual joint solution
    for (const auto& sol : ik_solutions)
    {
      if ((is_valid_ != nullptr) && !(*is_valid_)(sol))
        continue;

      auto state = std::make_shared<descartes_light::State<FloatType>>(sol.cast<FloatType>());
      if (allow_collision_ && collision_ == nullptr)
      {
        samples.push_back(descartes_light::StateSample<FloatType>{ state, static_cast<FloatType>(0.0) });
      }
      else if (!allow_collision_)
      {
        if (collision_->validate(sol))
          samples.push_back(descartes_light::StateSample<FloatType>{ state, 0.0 });
      }
      else
      {
        const FloatType cost = static_cast<FloatType>(collision_->distance(sol));
        samples.push_back(descartes_light::StateSample<FloatType>{ state, cost });
      }
    }
  }

  if (samples.empty())
    return samples;

  if (allow_collision_)
  {
    // Sort state samples in descending order by distance from nearest collision (i.e. state cost)
    std::sort(samples.begin(), samples.end(), [](const auto& a, const auto& b) { return a.cost > b.cost; });

    // Convert the distance into a cost and normalize
    if (samples.size() > 1)
    {
      const FloatType max_dist = samples.front().cost;
      const FloatType min_dist = samples.back().cost;
      const FloatType range = max_dist - min_dist;
      if (range > std::numeric_limits<FloatType>::epsilon())
      {
        std::for_each(samples.begin(), samples.end(), [&min_dist, &range](auto& sample) {
          sample.cost = static_cast<FloatType>(1.0) - (sample.cost - min_dist) / range;
        });
      }
      else
      {
        std::for_each(samples.begin(), samples.end(), [](auto& sample) { sample.cost = 0.0; });
      }
    }
  }

  // Generate the redundant solutions
  if (use_redundant_joint_solutions_)
  {
    const Eigen::MatrixX2d& limits = manip_->getLimits().joint_limits;
    std::vector<Eigen::Index> redundancy_capable_joints = manip_->getRedundancyCapableJointIndices();
    std::vector<descartes_light::StateSample<FloatType>> redundant_samples;
    for (const descartes_light::StateSample<FloatType>& sample : samples)
    {
      const auto redundant_solutions =
          tesseract_kinematics::getRedundantSolutions<FloatType>(*(sample.state), limits, redundancy_capable_joints);

      // Add the redundant samples with the same cost as the nominal sample
      std::transform(redundant_solutions.begin(),
                     redundant_solutions.end(),
                     std::back_inserter(redundant_samples),
                     [&sample](const auto& sol) {
                       auto state = std::make_shared<descartes_light::State<FloatType>>(sol.template cast<FloatType>());
                       return descartes_light::StateSample<FloatType>{ state, sample.cost };
                     });
    }

    // Combine the nominal samples with the redundant samples
    samples.insert(samples.end(), redundant_samples.begin(), redundant_samples.end());
  }

  return samples;
}

}  // namespace tesseract_planning

#endif  // TESSERACT_MOTION_PLANNERS_DESCARTES_ROBOT_SAMPLER_HPP


/*

    Copyright (C) 2010-2011  Andreas Würl <blitzortung@tryb.de>

*/

#include "config.h"

#include <iostream>
#include <string>

#include <boost/program_options.hpp>

#include <json.h>

#ifdef HAVE_BOOST_ACCUMULATORS_ACCUMULATORS_HPP
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#endif

#include "namespaces.h"
#include "data/Events.h"
#include "data/EventsFile.h"
#include "hardware/parsing/Samples.h"
#include "exception/Base.h"
#include "Logger.h"

#ifdef HAVE_BOOST_ACCUMULATORS_ACCUMULATORS_HPP
typedef ac::accumulator_set<double, ac::features<ac::tag::mean, ac::tag::variance> > statistics_accumulator;
typedef ac::accumulator_set<double, ac::features<ac::tag::density> > accumulator;
typedef boost::iterator_range<std::vector<std::pair<double, double> >::iterator > histogram;
#endif

inline float sqr(float x) {
  return x*x;
}

class AbstractOutput {

  public:
    typedef std::unique_ptr<AbstractOutput> AP;

    virtual void add(const int index, const bo::data::Event& event) = 0;

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

class JsonOutput : public AbstractOutput {

  private:

    std::ostringstream secondsTimestamp_;

  protected:
    json_object* jsonArray_;

    std::string asSecondsTimestamp(const pt::ptime& timestamp) {
      secondsTimestamp_.clear();
      secondsTimestamp_ << timestamp;
      return secondsTimestamp_.str();
    }

  public:

    JsonOutput() {
      jsonArray_ = json_object_new_array();

      pt::time_facet *timefacet = new pt::time_facet();
      timefacet->format("%Y-%m-%d %H:%M:%S");
      secondsTimestamp_.imbue(std::locale(std::locale::classic(), timefacet));
    }

    virtual ~JsonOutput() {
      json_object_put(jsonArray_);
    }

    std::string getOutput() const {
      return std::string(json_object_to_json_string(jsonArray_)) + '\n';
    }

};

class StreamOutput : public AbstractOutput {

  protected:

    std::ostringstream stream_;

  public:

   StreamOutput() {
      pt::time_facet *timefacet = new pt::time_facet();
      timefacet->format("%Y-%m-%d %H:%M:%S.%f");
      stream_.imbue(std::locale(std::locale::classic(), timefacet));
   }

   std::string getOutput() const {
     return stream_.str();
   }

};

class DefaultStreamOutput : public StreamOutput {

  public:
    void add(const int index, const bo::data::Event& event) {
      stream_ << event << " " << index << std::endl;
    }
};

class DefaultJsonOutput : public JsonOutput {

  public:
    void add(const int index, const bo::data::Event& event) {
      json_object* jsonArray = event.asJson();
      json_object_array_add(jsonArray, json_object_new_int(index));
      json_object_array_add(jsonArray_, jsonArray);
    }
};

class LongStreamOutput : public StreamOutput {

  private:

    bool normalize_;

  public:

    LongStreamOutput(bool normalize) :
      normalize_(normalize)
    {}

    void add(const int index, const bo::data::Event& event) {
      stream_ << "# " << event << " " << index << std::endl;

      const bo::data::Waveform& waveform = event.getWaveform();

      float angle = -waveform.getPhase(waveform.getMaxIndexNoClip());
      float cos_angle = cos(angle);
      float sin_angle = sin(angle);

      float scaleFactor = 1 << (waveform.getElementSize() * 8 - 1);
      unsigned int numberOfChannels = waveform.getNumberOfChannels();
      unsigned int timeDeltaNanoseconds = waveform.getTimeDelta().total_nanoseconds();

      for (unsigned int sample = 0; sample < waveform.getNumberOfSamples(); sample++) {
	stream_ << waveform.getTimestamp(sample) << " " << timeDeltaNanoseconds * sample;
	double sum = 0.0;
	for (unsigned int channel = 0; channel < numberOfChannels; channel++) {
	  double value = waveform.getFloat(sample, channel) / scaleFactor;
	  sum += value * value;
	  stream_ << " " << value;
	}
	if (numberOfChannels > 1) {
	  stream_ << " " << sqrt(sum);

	  if (numberOfChannels == 2 && normalize_) {
	      float x = waveform.getFloat(sample, 0) / scaleFactor;
	      float y = waveform.getFloat(sample, 1) / scaleFactor;
	      stream_ << " " << x * cos_angle - y * sin_angle << " " << x * sin_angle + y * cos_angle;
	  }
	}
	stream_ << std::endl;
      }
      stream_ << std::endl;
    }
};

class LongJsonOutput : public JsonOutput {

  private:

    bool normalize_;

  public:

    LongJsonOutput(bool normalize) :
      normalize_(normalize)
    {}

    void add(const int index, const bo::data::Event& event) {
      json_object* jsonArray = event.asJson();
      json_object_array_add(jsonArray, event.getWaveform().asJson(normalize_));
      json_object_array_add(jsonArray, json_object_new_int(index));

      json_object_array_add(jsonArray_, jsonArray);
    }
};

class TimestampStreamOutput : public StreamOutput {
  public:
    void add(const int index, const bo::data::Event& event) {
      const blitzortung::data::Waveform& waveform = event.getWaveform();
      stream_ << waveform.getTimestamp(waveform.getMaxIndex()) << " " << index << std::endl;
    }
};

class TimestampJsonOutput : public JsonOutput {

  public:
    void add(const int index, const bo::data::Event& event) {
      const blitzortung::data::Waveform& wfm = event.getWaveform();

      json_object* jsonArray = json_object_new_array();

      json_object_array_add(jsonArray, json_object_new_string(asSecondsTimestamp(wfm.getTimestamp()).c_str()));
      json_object_array_add(jsonArray, json_object_new_int(wfm.getTimestamp().time_of_day().fractional_seconds()));
      json_object_array_add(jsonArray, json_object_new_int(index));

      json_object_array_add(jsonArray_, jsonArray);
    }
};

pt::time_duration parseTime(const std::string& inputString, bool isEnd=false) {
  std::istringstream iss(inputString);
  pt::time_duration endOffset(pt::seconds(0));

  std::string format;
  if (inputString.size() <= 4) {
    format = "%H%M";
    endOffset = pt::minutes(1);
  } else {
    format = "%H%M%S";
    endOffset = pt::seconds(1);
  }

  // create formatting facet and set format for time_duration type
  pt::time_input_facet *facet = new pt::time_input_facet();
  facet->time_duration_format(format.c_str());

  iss.imbue(std::locale(std::locale::classic(), facet));

  pt::time_duration time;
  iss >> time;

  if (isEnd)
    return time + endOffset;
  else
    return time;
}

void addStreamToInputFile(std::istream& istream, const std::string& outputFile) {

  pt::time_input_facet *timefacet = new pt::time_input_facet();
  timefacet->format("%Y-%m-%d %H:%M:%s");
  std::istringstream iss;
  iss.imbue(std::locale(std::locale::classic(), timefacet));

  blitzortung::data::Events events;

  while (!istream.eof()) {

    std::string line;
    getline(istream, line);

    if (line.length() > 0) {

      iss.clear();
      iss.str(line);

      pt::ptime timestamp;
      float longitude;
      float latitude;
      short altitude;
      unsigned short numberOfChannels;
      unsigned short numberOfSamples;
      unsigned short numberOfBitsPerSample;
      unsigned short sampleTime;
      std::string data;

      iss >> timestamp >> latitude >> longitude >> altitude >> numberOfChannels >> numberOfSamples >> numberOfBitsPerSample >> sampleTime >> data;

      blitzortung::data::Format format((numberOfBitsPerSample-1)/8+1, numberOfChannels, numberOfSamples);

      blitzortung::hardware::parsing::Samples sampleParser(format, timestamp, sampleTime, data);

      blitzortung::data::Waveform::AP waveform = sampleParser.getWaveform();

      blitzortung::data::GpsInfo::AP gpsInfo(new blitzortung::data::GpsInfo(longitude, latitude, altitude));

      blitzortung::data::Event::AP event(new blitzortung::data::Event(std::move(waveform), std::move(gpsInfo)));

      if (events.size() != 0 && events.getDate() != event->getWaveform().getTimestamp().date()) {
	events.appendToFile(outputFile);
	events.clear();
      }

      events.add(std::move(event));
    }
  }
  if (events.size() != 0) {
    events.appendToFile(outputFile);
  }
}

void cleanupFile(const std::string& cleanupFile) {
  blitzortung::data::Events events;

  events.readFromFile(cleanupFile);

  // sort events and remove duplicate elements with identical timestamp
  events.unique();

  events.writeToFile(cleanupFile);
}

void printFileInfo(const std::string& inputFile) {
  bo::data::EventsFile eventsFile(inputFile);

  const bo::data::EventsHeader& header = eventsFile.getHeader();

  bo::data::Events::AP start(eventsFile.read(0,1));
  bo::data::Events::AP end(eventsFile.read(-1,1));

  pt::time_facet *timefacet = new pt::time_facet();
  timefacet->format("%Y-%m-%d %H:%M:%S.%f");

  std::locale oldLocale = std::cout.imbue(std::locale(std::locale::classic(), timefacet));

  std::cout << start->front().getTimestamp() << " " << 0 << std::endl;
  std::cout << end->front().getTimestamp() << " " << header.getNumberOfEvents() - 1 << std::endl;

  std::cout.imbue(oldLocale);

  const bo::data::Format& format = header.getDataFormat();

  std::cout << header.getNumberOfEvents() << " events, ";
  std::cout << format.getNumberOfSamples() << " samples, ";
  std::cout << (short) format.getNumberOfChannels() << " channels, ";
  std::cout << format.getNumberOfBytesPerSample()*8 << " bits";
  std::cout << std::endl;
}

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

  std::string fileName = "";
  std::string mode = "default";
  std::string startTimeString, endTimeString;
  int startIndex, numberOfEvents;
  bool normalize = false;

  bo::Logger logger("");

  // programm arguments/options
  boost::program_options::options_description desc("program options");
  desc.add_options()
    ("help,h", "show program help")
    ("input-file,i", po::value<std::string>(&fileName), "input file name")
    ("output-file,o", po::value<std::string>(&fileName), "output file name")
    ("cleanup-file,c", po::value<std::string>(&fileName), "cleanup file name")
    ("info", "show file info")
    ("starttime,s", po::value<std::string>(&startTimeString), "start time in HHMM or HHMMSS format")
    ("endtime,e", po::value<std::string>(&endTimeString), "end time in HHMM or HHMMSS format")
    ("start", po::value<int>(&startIndex)->default_value(0), "start index of first event")
    ("number", po::value<int>(&numberOfEvents)->default_value(-1), "number of events")
    #ifdef HAVE_BOOST_ACCUMULATORS_ACCUMULATORS_HPP
    ("mode", po::value<std::string>(&mode)->default_value(mode), "data mode [default, statistics, histogram]")
    #endif
    ("verbose,v", "verbose mode")
    ("normalize", "reduce multiple channel to single best channel, works only in combinatino with long-data mode")
    ("json,j", "output JSON data")
    ("long-data,l", "output all samples")
    ("event-time", "output eventtime")
    ("debug", "debug mode")
    ;

  // parse command line options
  po::variables_map vm;
  bool showHelp = false;

  try {
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
    po::notify(vm);
  } catch (std::exception& e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    showHelp = true;
  }

  // help output or no 'sql-statement' given
  if (vm.count("help") || showHelp) {
    std::cout << argv[0] << " [options]" << std::endl << std::endl;
    std::cout << desc << std::endl;
    std::cout << std::endl;

    return 1;
  }

  // logging setup

  logger.setPriority(log4cpp::Priority::NOTICE);

  if (vm.count("output-file")) {
    addStreamToInputFile(std::cin, fileName);
    return 0;
  }

  if (vm.count("cleanup-file")) {
    cleanupFile(fileName);
    return 0;
  }

  if (! vm.count("input-file")) {
    std::cerr << "'input-file' missing\n";
    return 5;
  }

  if (vm.count("verbose")) {
    logger.setPriority(log4cpp::Priority::INFO);
  }

  if (vm.count("debug")) {
    logger.setPriority(log4cpp::Priority::DEBUG);
  }

  if (vm.count("info")) {
    printFileInfo(fileName);
    return 0;
  }

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

  bo::data::Events events;

  if (vm.count("starttime") || vm.count("endtime")) {
    pt::time_duration startTime(pt::not_a_date_time);
    if (vm.count("starttime")) {
      startTime = parseTime(startTimeString);
    }

    pt::time_duration endTime(pt::not_a_date_time);
    if (vm.count("endtime")) {
      endTime = parseTime(endTimeString, true);
    }

    events.readFromFile(fileName, startTime, endTime);
  } else {
    events.readFromFile(fileName, startIndex, numberOfEvents);
  }

  #ifdef HAVE_BOOST_ACCUMULATORS_ACCUMULATORS_HPP
  if (mode == "statistics") {
    statistics_accumulator acc;

    for (bo::data::Events::CI event = events.begin(); event != events.end(); event++) {
      const bo::data::Waveform& waveform = event->getWaveform();
      acc(waveform.getAmplitude(waveform.getMaxIndex()));
    }

    std::cout << events.size() << " " << ac::mean(acc) << " " << ac::variance(acc) << std::endl;
  } else if (mode == "histogram") {
    accumulator acc(ac::tag::density::num_bins = 20, ac::tag::density::cache_size = events.size());

    for (bo::data::Events::CI event = events.begin(); event != events.end(); event++) {
      const bo::data::Waveform& waveform = event->getWaveform();
      acc(waveform.getAmplitude(waveform.getMaxIndex()));
    }

    histogram hist = ac::density(acc);
    for (int i = 0; i < hist.size(); i++) {
      std::cout << hist[i].first << " " << hist[i].second * events.size() << std::endl;
    }
  } else
  #endif
    if (mode == "default") {
      AbstractOutput::AP output;
      if (vm.count("json")) {
	if (vm.count("long-data")) {
	  output = AbstractOutput::AP(new LongJsonOutput(normalize));
	} else if (vm.count("event-time")) {
	  output = AbstractOutput::AP(new TimestampJsonOutput());
	} else {
	  output = AbstractOutput::AP(new DefaultJsonOutput());
	}
      } else {
	if (vm.count("long-data")) {
	  output = AbstractOutput::AP(new LongStreamOutput(normalize));
	} else if (vm.count("event-time")) {
	  output = AbstractOutput::AP(new TimestampStreamOutput());
	} else {
	  output = AbstractOutput::AP(new DefaultStreamOutput());
	}
      }

      int index = events.getStartIndex();
      for (bo::data::Events::CI event = events.begin(); event != events.end(); event++) {
	output->add(index++, *event);
      }

      std::cout << output->getOutput();
  } else {
    std::cerr << "unknown mode '" << mode << "'";
    exit(1);
  }
}


// Copyright (C) 2010-2015 Internet Systems Consortium, Inc. ("ISC")
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <config.h>

#include <stdint.h>

#include <sstream>
#include <ostream>

#include <util/buffer.h>
#include <dns/messagerenderer.h>
#include <dns/rrttl.h>

#include <boost/lexical_cast.hpp>
#include <algorithm>
#include <cctype>

using namespace std;
using namespace isc::dns;
using namespace isc::util;

namespace {

// We wrap the C isalpha, because it seems to be overloaded with something.
// Then the find_if doesn't work.
bool
myIsalpha(char c) {
    return (isalpha(c) != 0);
}

// The conversion of units to their size
struct Unit {
    char unit;
    uint32_t multiply;
    uint32_t max_allowed;
};

Unit units[] = {
    { 'S', 1,                0xffffffff / 1 },
    { 'M', 60,               0xffffffff / 60 },
    { 'H', 60 * 60,          0xffffffff / (60 * 60) },
    { 'D', 24 * 60 * 60,     0xffffffff / (24 * 60 * 60) },
    { 'W', 7 * 24 * 60 * 60, 0xffffffff / (7 * 24 * 60 * 60) }
};

}

namespace isc {
namespace dns {

namespace {
bool
parseTTLString(const string& ttlstr, uint32_t& ttlval, string* error_txt) {
    if (ttlstr.empty()) {
        if (error_txt != NULL) {
            *error_txt = "Empty TTL string";
        }
        return (false);
    }

    // We use a larger data type to handle negative number cases.
    uint64_t val = 0;
    const string::const_iterator end = ttlstr.end();
    string::const_iterator pos = ttlstr.begin();

    try {
        // When we detect we have some units
        bool units_mode = false;

        while (pos != end) {
            // Find the first unit, if there's any.
            const string::const_iterator unit = find_if(pos, end, myIsalpha);
            // No unit
            if (unit == end) {
                if (units_mode) {
                    // We had some units before. The last one is missing unit.
                    if (error_txt != NULL) {
                        *error_txt = "Missing the last unit: " + ttlstr;
                    }
                    return (false);
                } else {
                    // Case without any units at all. Just convert and store
                    // it.
                    val = boost::lexical_cast<uint64_t>(ttlstr);
                    break;
                }
            }
            // There's a unit now.
            units_mode = true;
            // Find the unit and get the size.
            uint32_t multiply = 1;  // initialize to silence compiler warnings
            uint32_t max_allowed = 0xffffffff;
            bool found = false;
            for (size_t i = 0; i < sizeof(units) / sizeof(*units); ++i) {
                if (toupper(*unit) == units[i].unit) {
                    found = true;
                    multiply = units[i].multiply;
                    max_allowed = units[i].max_allowed;
                    break;
                }
            }
            if (!found) {
                if (error_txt != NULL) {
                    *error_txt = "Unknown unit used: " +
                        boost::lexical_cast<string>(*unit) + " in: " + ttlstr;
                }
                return (false);
            }
            // Now extract the number.
            if (unit == pos) {
                if (error_txt != NULL) {
                    *error_txt = "Missing number in TTL: " + ttlstr;
                }
                return (false);
            }
            const uint64_t value =
                boost::lexical_cast<uint64_t>(string(pos, unit));
            if (value > max_allowed) {
                if (error_txt != NULL) {
                    *error_txt = "Part of TTL out of range: "  + ttlstr;
                }
                return (false);
            }

            // seconds cannot be out of range at this point.
            const uint64_t seconds = value * multiply;
            assert(seconds <= 0xffffffff);

            // Add what we found
            val += seconds;
            // Check the partial value is still in range (the value can only
            // grow, so if we get out of range now, it won't get better, so
            // there's no need to continue).
            if (val < seconds || val > 0xffffffff) {
                if (error_txt != NULL) {
                    *error_txt = "Part of TTL out of range: "  + ttlstr;
                }
                return (false);
            }
            // Move to after the unit.
            pos = unit + 1;
        }
    } catch (const boost::bad_lexical_cast&) {
        if (error_txt != NULL) {
            *error_txt = "invalid TTL: " + ttlstr;
        }
        return (false);
    }

    if (val <= 0xffffffff) {
        ttlval = val;
    } else {
        // This could be due to negative numbers in input, etc.
        if (error_txt != NULL) {
            *error_txt = "TTL out of range: " + ttlstr;
        }
        return (false);
    }

    return (true);
}
}

RRTTL::RRTTL(const std::string& ttlstr) {
    string error_txt;
    if (!parseTTLString(ttlstr, ttlval_, &error_txt)) {
        isc_throw(InvalidRRTTL, error_txt);
    }
}

RRTTL*
RRTTL::createFromText(const string& ttlstr) {
    uint32_t ttlval;
    if (parseTTLString(ttlstr, ttlval, NULL)) {
        return (new RRTTL(ttlval));
    }
    return (NULL);
}

RRTTL::RRTTL(InputBuffer& buffer) {
    if (buffer.getLength() - buffer.getPosition() < sizeof(uint32_t)) {
        isc_throw(IncompleteRRTTL, "incomplete wire-format TTL value");
    }
    ttlval_ = buffer.readUint32();
}

const string
RRTTL::toText() const {
    ostringstream oss;
    oss << ttlval_;
    return (oss.str());
}

void
RRTTL::toWire(OutputBuffer& buffer) const {
    buffer.writeUint32(ttlval_);
}

void
RRTTL::toWire(AbstractMessageRenderer& renderer) const {
    renderer.writeUint32(ttlval_);
}

ostream&
operator<<(ostream& os, const RRTTL& rrttl) {
    os << rrttl.toText();
    return (os);
}
}
}


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python.hpp>
#include <boost/python/class.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <cctbx/geometry_restraints/nonbonded_sorted.h>

namespace cctbx { namespace geometry_restraints {
namespace {

  struct nonbonded_sorted_asu_proxies_base_wrappers : boost::python::pickle_suite
  {
    typedef nonbonded_sorted_asu_proxies_base w_t;

    static boost::python::tuple
      getstate(w_t const& self)
    {
      return boost::python::make_tuple(
        self.asu_mappings(),
        self.simple,
        self.asu
        );
    }

    static void
      setstate(w_t& self, boost::python::tuple state)
    {
      self.asu_mappings_owner_ = boost::python::extract< boost::shared_ptr<direct_space_asu::asu_mappings<> > >(state[0]);
      self.simple = boost::python::extract< af::shared<nonbonded_simple_proxy> >(state[1]);
      self.asu = boost::python::extract< af::shared<nonbonded_asu_proxy> >(state[2]);
      self.asu_mappings_ = self.asu_mappings_owner_.get();
    }

    static void
    wrap()
    {
      using namespace boost::python;
      typedef return_value_policy<copy_const_reference> ccr;
      class_<w_t>("nonbonded_sorted_asu_proxies_base", no_init)
        .def(init<
          boost::shared_ptr<asu_mappings> const&>(
            (arg("asu_mappings"))))
        .def("asu_mappings", &w_t::asu_mappings, ccr())
        .def("process",
          (void(w_t::*)(nonbonded_simple_proxy const&)) &w_t::process,
            (arg("proxy")))
        .def("process",
          (void(w_t::*)(af::const_ref<nonbonded_simple_proxy> const&))
            &w_t::process,
          (arg("proxies")))
        .def("process",
          (void(w_t::*)(nonbonded_asu_proxy const&, bool)) &w_t::process,
            (arg("proxy"), arg("sym_excl_flag")=false))
        .def("process",
          (void(w_t::*)(af::const_ref<nonbonded_asu_proxy> const&))
            &w_t::process,
          (arg("proxies")))
        .def("n_total", &w_t::n_total)
        .def_readonly("simple", &w_t::simple)
        .def_readonly("asu", &w_t::asu)
        .def_pickle(nonbonded_sorted_asu_proxies_base_wrappers())
        ;
    }
  };

  struct nonbonded_sorted_asu_proxies_wrappers : boost::python::pickle_suite
  {
    typedef nonbonded_sorted_asu_proxies w_t;

    static boost::python::tuple
      getinitargs(w_t const& self)
    {
        return boost::python::make_tuple(self.asu_mappings());
    }

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t, bases<nonbonded_sorted_asu_proxies_base> >(
        "nonbonded_sorted_asu_proxies", no_init)
        .def(init<
          boost::shared_ptr<asu_mappings> const&>(
            (arg("asu_mappings"))))
        .def(init<
          af::const_ref<std::size_t> const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<std::size_t> const&,
          nonbonded_params const&,
          af::const_ref<std::string> const&,
          af::const_ref<int> const&,
          double,
          double,
          std::vector<crystal::pair_asu_table<> > const&>((
            arg("model_indices"),
            arg("conformer_indices"),
            arg("sym_excl_indices"),
            arg("donor_acceptor_excl_groups"),
            arg("nonbonded_params"),
            arg("nonbonded_types"),
            arg("nonbonded_charges"),
            arg("nonbonded_distance_cutoff_plus_buffer"),
            arg("min_cubicle_edge"),
            arg("shell_asu_tables"))))
        .def_readonly("n_unknown_nonbonded_type_pairs",
          &w_t::n_unknown_nonbonded_type_pairs)
        .def_readonly("min_vdw_distance", &w_t::min_vdw_distance)
        .def_readonly("max_vdw_distance", &w_t::max_vdw_distance)
        .def_pickle(nonbonded_sorted_asu_proxies_wrappers())
        ;
    }
  };

  void
  wrap_all()
  {
    nonbonded_sorted_asu_proxies_base_wrappers::wrap();
    nonbonded_sorted_asu_proxies_wrappers::wrap();
  }

} // namespace <anonymous>

namespace boost_python {

  void
  wrap_nonbonded_sorted() { wrap_all(); }

}}} // namespace cctbx::geometry_restraints::boost_python


#ifndef FAST_TF_FAST_TF_HPP__
#define FAST_TF_FAST_TF_HPP__

// eigen seems to confuse IWYU a lot...
#include <Eigen/Core>
#include <Eigen/Geometry>  // IWYU pragma: keep
// IWYU pragma: no_include "Eigen/src/Geometry/Quaternion.h"
// IWYU pragma: no_include "Eigen/src/Geometry/Transform.h"
// IWYU pragma: no_include "Eigen/src/Geometry/Translation.h"

// std-lib
#include <chrono>
#include <condition_variable>
#include <list>
#include <map>
#include <mutex>
#include <stddef.h>
#include <string>
#include <unordered_map>
#include <utility>
#include <variant>

namespace fast_tf {
namespace detail {

/// @brief Class storing translation and rotation separately.
///
/// The benchmarking showed that we would spend a lot of time at retrieving the
/// rotation from Eigen::Isometry3d. Since we get these informations already
/// separated, we will store them also seperated.
struct transform {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Translation3d translation;
  Eigen::Quaterniond rotation;
};

// below the definition of two transform types: static and dynamic: static
// transforms are time-independent; dynamic transforms store a sequence of
// transforms. both classes correspont to the tf2::StaticCache and
// tf2::TimedCache, respectively.

/// @brief Storage for a static transform.
///
/// Class stores a single transform (our version of tf2::StaticCache).
struct static_transform {
  static_transform() = default;
  explicit static_transform(const Eigen::Isometry3d& _data) noexcept;
  explicit static_transform(const transform& _data) noexcept;

  void
  set(const Eigen::Isometry3d& _data) noexcept;

  void
  set(const transform& _data) noexcept;

  const Eigen::Isometry3d&
  get() const noexcept;

protected:
  Eigen::Isometry3d data_;  ///< the data...
};

// this is the most used method of our class - so we inline it.
inline const Eigen::Isometry3d&
static_transform::get() const noexcept {
  return data_;
}

// replacing the ros::Time with std::chrono::time_point yields in ~100%
// speedup (!) of the dynamic_transform::set function.
// todo clock_t collides with other definitions
using clock_t = std::chrono::system_clock;
using time_t = std::chrono::time_point<clock_t>;
using duration_t = std::chrono::nanoseconds;

/**
 * @brief Class with a scoped counter. The user can count up as long as the
 * scoped_signal remains in the scope. The scoped_signal will count down as soon
 * as it goes out of scope. The class is not thread-safe.
 *
 * Example
 * @code
 * // create an instance of the counter
 * counter c;
 * assert(c && "counter is empty");
 *
 * // create a scoped counter. counter is now at 1
 * scoped_handle sc1{c};
 * assert(!c && "counter is now incremented");
 *
 * {
 *  scoped_handle sc2{c}; // counter is now at 2
 *  scoped_handle sc3{c}; // counter is now at 3
 * }
 * // counter is now back to 1
 * @endcode
 */
struct counter {
  /// @brief When constructed, the class will increment the count and decrement
  /// when destructed.
  struct scoped_signal {
    scoped_signal(const counter& _c);
    ~scoped_signal();

  private:
    size_t& counter_;
  };

  friend scoped_signal;

  explicit operator bool() const noexcept { return counter_ != 0; }

private:
  mutable size_t counter_ = 0;
};

/// @brief Storage for a sequence of dynamic transforms.
///
/// Class stores a timed sequence of a dynamically changing transforms. The
/// implementation matches the implementation of the tf2::TimedCache.
struct dynamic_transform : public counter {
  /// @brief c'tor with the storage duration
  /// @param _dur positive duration
  explicit dynamic_transform(const duration_t& _dur) noexcept;
  dynamic_transform() noexcept;

  /// @brief Inserts new data and prunes stale data.
  /// @param _time The time-stamp of the data.
  /// @param _data The new data to add.
  ///
  /// Function will prune data older then the newest data - storage duration.
  /// The passed _data does not replace the old data if.
  void
  set(const time_t& _time, const Eigen::Isometry3d& _data) noexcept;

  /// @brief Inserts new data and prunes stale data.
  ///
  /// Function identical with its overload - the only difference is the data
  /// format.
  void
  set(const time_t& _time, const transform& _data) noexcept;

  /// @brief Returns the closest element to the query time.
  /// @param _time The query time.
  /// @throw std::runtime_error if no data can be retrieved.
  Eigen::Isometry3d
  get(const time_t& _time) const;

private:
  duration_t dur_;  ///< the duration how long to keep the data

  // remarks: the benchmarking (see perf/perf_dynamic_transform.cpp) showed that
  // the std::map performs better than boost::container::flat_map or std::deque
  // for our usecase.
  using impl_t = std::map<time_t, transform>;
  impl_t map_;  ///< impl holding the data
};

// our transforms may be either static or dynamic.
using variable_transform = std::variant<static_transform, dynamic_transform>;

/// @brief Structure holding a transformation tree.
///
/// The tree is a cycle-tree, connected graph, where every node has one parent
/// (but the root), and an arbitrary number of children.
struct transform_tree {
  /// @brief Structure holding the transform.
  struct node {
    // constructors which will set depth and parent automatically
    explicit node(variable_transform&& _data) noexcept;
    node(variable_transform&& _data, const node& _parent) noexcept;

    unsigned int depth = 0;        ///< the current depth (increasing from root)
    const node* parent = nullptr;  ///< link to the parent; we don't own it
    variable_transform data;       ///< the actual data; we own it
  };

  // the data structure mapping frame-id to the node
  using impl_t = std::unordered_map<std::string, node>;

  // the iterators
  using iterator = impl_t::iterator;
  using const_iterator = impl_t::const_iterator;
  using pair_type = std::pair<iterator, bool>;

  // key-value types
  using value_type = impl_t::value_type;  // node
  using key_type = impl_t::key_type;      // std::string

  /// @brief Returns true, if the tree holds no data.
  bool
  empty() const noexcept {
    return data_.empty();
  }

  /// @brief Initializes the new root node.
  ///
  /// A new root node can be created if the current tree is empty or if the
  /// current root node is the direct child of the new root node: Adding the
  /// link {X->A} to the tree {A->B->C} would then generate the tree
  /// {X->A->B->C}.
  ///
  /// @return Pair of an iterator to the root node and boolean if the new node
  /// has been created (same signature as std::unordered_map::emplace).
  ///
  /// @throw std::runtime_error if new root node can't be inserted in the tree.
  [[nodiscard]] pair_type
  emplace_parent(const std::string& _parent_frame,
                 const std::string& _child_frame, bool _is_static);

  /// @brief Returns the iterator to the node identified by _child_frame.
  ///
  /// The function will create a new node, if the node does not exist. It may
  /// also create a new root node, if the parent node does not exist (see
  /// emplace_parent).
  ///
  /// @return Pair of an iterator to the child node and a boolean if a new node
  /// (child or parent) has been created.
  ///
  /// @throw std::runtime_error if a circle would be created, or if the tree
  /// would be disconnected.
  [[nodiscard]] pair_type
  emplace(const std::string& _parent_frame, const std::string& _child_frame,
          bool _is_static);

  /// @brief Merges the given tree into the current.
  ///
  /// The trees can only be merged if for at least one tree the common node is
  /// the root node: {A->B->C} and {B->D} have the common node B and B is the
  /// root node of the second tree; the resulting tree would be {A->B->{C, D}}.
  /// The function is a noop, if one of the trees is empty.
  ///
  /// @throw std::runtime_error if the trees cannot be merged.
  void
  merge(transform_tree& _other);

  impl_t data_;
  impl_t::iterator root_ = data_.end();
};

/// @brief Thread-safe structure allowing to store and query transforms.
struct transform_buffer {
  transform_buffer();

  /// @brief Inserts a transform into the buffer.
  ///
  /// @param _parent_frame frame id of the parent link.
  /// @param _child_frame frame id of the child link.
  /// @param _stamp_time time of the transform (only for dynamic links).
  /// @param _tf the transform.
  /// @param _is_static flag, if the link is dynamic or static.
  ///
  /// @throw std::runtime_error if the tree would become ill formed.
  /// @throw std::bad_variant_access if a link would change its type
  /// (dynamic/static).
  void
  set(const std::string& _parent_frame, const std::string& _child_frame,
      const time_t& _stamp_time, const Eigen::Isometry3d& _tf, bool _is_static);

  /// @brief Inserts a transform into the buffer.
  ///
  /// Function identical with its overload - the only difference is the data
  /// format.
  void
  set(const std::string& _parent_frame, const std::string& _child_frame,
      const time_t& _stamp_time, const transform& _tf, bool _is_static);

  /// @brief Retrieves a transform.
  ///
  /// @param _target_frame frame id of the target link.
  /// @param _source_frame frame id of the source link.
  /// @param _query_time time of the query (only for dynamic links).
  /// @param _tolerance timeout how long to wait for the query.
  ///
  /// @throw std::runtime_error if no transformation can be retrieved.
  Eigen::Isometry3d
  get(const std::string& _target_frame, const std::string& _source_frame,
      const time_t& _query_time, const duration_t& _tolerance) const;

protected:
  // we may have multiple trees.
  using trees_t = std::list<transform_tree>;

  [[nodiscard]] bool
  same_tree(trees_t::const_iterator _l,
            trees_t::const_iterator _r) const noexcept {
    return _l != trees_.end() && _r != trees_.end() && _l == _r;
  }

  [[nodiscard]] transform_tree::pair_type
  emplace(const std::string& _parent_frame, const std::string& _child_frame,
          bool _is_static);

  [[nodiscard]] std::pair<trees_t::const_iterator,
                          transform_tree::const_iterator>
  find(const std::string& _frame) const noexcept;

  void
  merge(const std::string& _parent_frame, const std::string& _child_frame);

  bool
  merge(const std::string& _common_frame);

  Eigen::Isometry3d
  get_dynamic_transform(const time_t& _query_time, const time_t& _end_time,
                        const dynamic_transform& _dyn_tr,
                        std::unique_lock<std::mutex>& _lock) const;

  counter c_;
  trees_t trees_;

  mutable std::mutex m_;
  mutable std::condition_variable cv_;
};

}  // namespace detail
}  // namespace fast_tf

#endif  // FAST_TF_FAST_TF_HPP__


/**
 * Copyright (c) 2019 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef semantic_roof_hpp_included_
#define semantic_roof_hpp_included_

#include <vector>
#include <array>

#include <boost/variant.hpp>

#include "utility/enum-io.hpp"
#include "math/geometry_core.hpp"

namespace semantic { namespace roof {

struct Rectangular {
    using Curb = std::array<double, 4>;
    using Hip = std::array<double, 2>;
    using EaveHeight = std::array<double, 4>;

    constexpr static Curb defaultCurb = { 1.0, 1.0, 0.0, 0.0 };
    constexpr static Hip defaultHip = { 1.0, 1.0 };
    constexpr static EaveHeight defaultEaveHeight = { 0.0, 0.0, 0.0, 0.0 };

    math::Size2f size;
    math::Point2d skew;
    double azimuth = 0.0;
    Curb curb = defaultCurb;
    Hip hip = defaultHip;
    double ridgeHeight = 0.0;
    double curbHeight = 0.0;
    EaveHeight eaveHeight = defaultEaveHeight;

    enum class Key { top = 0, bottom = 1, left = 2, right = 3 };
};

/** Casts Key to integer.
 */
int operator+(Rectangular::Key key);

struct Circular {
    double radius = 0.0;
    double curb = 0.0;
    double ridgeHeight = 0.0;
    double curbHeight = 0.0;
    double eaveHeight = 0.0;
};

typedef boost::variant<Circular, Rectangular> Instance;
enum class Type { circular = 0, rectangular };

struct Roof {
    typedef std::vector<Roof> list;

    math::Point3 center;
    Instance instance;

    Type type() const { return static_cast<Type>(instance.which()); }

    // needed by python bindings
    bool operator==(const Roof &r) const;
};

// inline stuff

UTILITY_GENERATE_ENUM_IO(Type,
                         ((rectangular))
                         ((circular))
                         )

UTILITY_GENERATE_ENUM_IO(Rectangular::Key,
                         ((top))
                         ((bottom))
                         ((left))
                         ((right))
                         )

inline int operator+(Rectangular::Key key) {
    return static_cast<int>(key);
}

} } // namespace roof::semantic

#endif // semantic_roof_hpp_included_


#include <boost/test/unit_test.hpp>

#include <boost/date_time/gregorian/gregorian.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include "TestHelper.h"
#include "MultiPatternScanner.h"
#include "PatternMatchFilter.h"
#include "InvertedVScanner.h"
#include "VScanner.h"
#include "DoubleBottomScanner.h"
#include "CupScanner.h"
#include "CupWithoutHandleScanner.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( CupWithoutHandle_GBX )
{

    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/GBX_Daily_2014.csv");

    PatternScannerPtr cupWithoutHandleScanner(new CupWithoutHandleScanner());
    MultiPatternScanner multiCupWithoutHandleScanner(cupWithoutHandleScanner);
    PatternMatchListPtr cupWithoutHandleMatches = multiCupWithoutHandleScanner.scanUniquePatternMatches(chartData);

    // This data includes a well-formed cup without handle, but also one which is more V-like and
    // should be disqualified using the CupScanners constraints on the ratio for the length of the
    // flat bottom versus the LHS downtrend.

    verifyMatchList("CupWithoutHandle_GBX",cupWithoutHandleMatches,1);

    verifyPatternMatch("CupWithoutHandle_GBX match",
            ptime(date(2014,4,2)),ptime(date(2014,4,22)),3,cupWithoutHandleMatches,0);

    BOOST_TEST_MESSAGE("CupWithoutHandle_GBX");
}

BOOST_AUTO_TEST_CASE( CupWithoutHandle_AMBA )
{

    PeriodValSegmentPtr chartData = PeriodValSegment::readFromFile("./patternScan/AMBA_Daily_2014.csv");

    PatternScannerPtr cupWithoutHandleScanner(new CupWithoutHandleScanner());
    MultiPatternScanner multiCupWithoutHandleScanner(cupWithoutHandleScanner);
    PatternMatchListPtr cupWithoutHandleMatches = multiCupWithoutHandleScanner.scanUniquePatternMatches(chartData);

    // This data includes a mal-formed cup without handle, one which is  V-like and
    // should be disqualified using the CupScanners constraints on the ratio for the length of the
    // flat bottom versus the LHS downtrend.
    //
    // In particular: Scanning was identifying the following as a valid cup without handle, when it shouldn't:
    //            CupWithoutHandle_AMBA: pattern match: start = 2014-01-30, end = 2014-02-27,
    //            num segments = 3, depth % = 27.5741, last close = 33.91

    verifyMatchList("CupWithoutHandle_AMBA",cupWithoutHandleMatches,0);

    BOOST_TEST_MESSAGE("CupWithoutHandle_AMBA");
}


/*
 * vi:ts=4:tw=78:shiftwidth=4:expandtab
 * vim600:fdm=marker
 *
 * trainer.hpp  -  abstract Trainer interface for conditional ME trainers
 *
 * Copyright (C) 2003 by Zhang Le <ejoy@users.sourceforge.net>
 * Begin       : 01-Jun-2003
 * Last Change : 24-Dec-2004.
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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 Lesser General Public
 * License along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */

#ifndef TRAINER_H
#define TRAINER_H

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <vector>
#include <string>
#include <boost/utility.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/shared_array.hpp>
#include "meevent.hpp"

namespace maxent{
using namespace std;
using boost::shared_ptr;
using boost::shared_array;
using namespace me;

// utility function
void load_events_txt(const string& filename, MEEventSpace& events);
void save_events_txt(const string& filename, const MEEventSpace& events);

/**
 * Trainer class provides an abstract interface to various training
 * algorithms.
 *
 * Usually you need not use this class explicitly. \ref MaxentModel::train()
 * provides a wrapper for the underline Trainer instances.
 */
class Trainer /*: boost::noncopyable*/{
    public:
        virtual ~Trainer() {}
        virtual void train(size_t iter = 15, double tol = 1E-05) = 0;

        // void save_param(const string& model, bool binary) const;

        void load_training_data(const string& events, const string& model);

        void set_training_data(shared_ptr<MEEventSpace> es,
                shared_ptr<ParamsType> params,
                size_t n_theta,
                shared_array<double> theta,
                shared_array<double> sigma2,
                size_t n_outcomes,
                shared_ptr<MEEventSpace> heldout_es =
                shared_ptr<MEEventSpace>()
                );

    protected:
        size_t m_n_outcomes;
        size_t m_N;          // total number of examples
        size_t m_n_theta;

        shared_ptr<MEEventSpace> m_es;
        shared_ptr<MEEventSpace> m_heldout_es;
        shared_ptr<ParamsType>   m_params;
        shared_array<double>     m_theta;
        shared_array<double>     m_sigma2;

        size_t eval(const Event::context_type* context, size_t len,
                vector<double>& probs) const;
    private:
        // void save_param_txt(const string& model) const;
};

} // namespace maxent

#endif /* ifndef TRAINER_H */