// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#include <memory>
#include <numeric>
#include <popart/graph.hpp>
#include <popart/ir.hpp>
#include <popart/op/concat.hpp>
#include <popart/op/gather.hpp>
#include <popart/op/reshape.hpp>
#include <popart/op/slice.hpp>
#include <popart/op/transpose.hpp>
#include <popart/patterns/pattern.hpp>
#include <popart/patterns/splitgather.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensorindex.hpp>
#include <popart/tensors.hpp>

#include <boost/integer/common_factor.hpp>

namespace popart {

// Private gather op class
namespace {
class SplitGatherOp : public GatherOp {
public:
  using GatherOp::GatherOp;
};
} // namespace

bool SplitGatherPattern::matches(Op *op) const {
  // Isn't a gather op
  if (!op->isConvertibleTo<GatherOp>()) {
    return false;
  }

  // Is an already split gather op
  if (op->isConvertibleTo<SplitGatherOp>()) {
    return false;
  }

  // The op doesn't have a vgraph id
  if (!op->hasVirtualGraphId()) {
    return false;
  }

  if (op->input->tensor(GatherOp::dataInIndex())->tensorType() !=
      TensorType::Const) {
    return false;
  }

  auto gather = dynamic_cast<GatherOp *>(op);

  const auto inputShape = gather->inShape(GatherOp::dataInIndex());
  const auto axis       = gather->getAxis();

  const int64_t virtualGraphCount = op->getIr().getDeviceInfo()->getNumIpus();

  // We aren't using vgraphs
  if (!op->getIr().virtualGraphsEnabled()) {
    return false;
  }

  // We don't have any vgraphs to split on
  if (virtualGraphCount < 2) {
    return false;
  }

  const auto numElements = std::accumulate(
      inputShape.begin(), inputShape.end(), 1, std::multiplies<int64_t>());

  const auto split =
      boost::integer::gcd(virtualGraphCount, numElements / inputShape[axis]);

  // We won't split the gather
  if (split < 2) {
    return false;
  }

  return true;
}

std::vector<const Tensor *> SplitGatherPattern::touches(Op *) const {
  return {};
}

static std::unique_ptr<TransposeOp>
canonicalizeTranspose(const int64_t axis,
                      const Shape &inputShape,
                      const Op::Settings &settings) {
  std::vector<int64_t> permutation(inputShape.size());
  std::iota(permutation.begin(), permutation.end(), 0);
  std::swap(permutation.front(), permutation[axis]);

  return std::make_unique<TransposeOp>(
      Onnx::Operators::Transpose_1, permutation, settings);
}

static std::unique_ptr<ReshapeOp>
canonicalizeShape(const int64_t axis,
                  const Shape &inputShape,
                  const Op::Settings &settings) {
  const auto numElements = std::accumulate(
      inputShape.begin(), inputShape.end(), 1, std::multiplies<int64_t>());

  const Shape canonShape = {inputShape[axis], numElements / inputShape[axis]};
  return std::make_unique<ReshapeOp>(
      Onnx::Operators::Reshape_5, canonShape, settings);
}

static std::vector<std::unique_ptr<SliceOp>>
createSlices(const int64_t count, const int64_t stride, Op::Settings settings) {
  std::vector<std::unique_ptr<SliceOp>> slices;
  slices.reserve(count);

  for (int i = 0; i < count; ++i) {
    settings.vgraphId = i;

    const std::vector<int64_t> starts_ = {i * stride};
    const std::vector<int64_t> ends_   = {(i + 1) * stride};
    const std::vector<int64_t> axes_   = {1};
    const std::vector<int64_t> steps_  = {};

    slices.push_back(std::make_unique<SliceOp>(
        Onnx::Operators::Slice_1, starts_, ends_, axes_, steps_, settings));
  }

  return slices;
}

static std::vector<std::unique_ptr<SplitGatherOp>>
createGathers(const int64_t count,
              Op::Settings settings,
              const nonstd::optional<float> &availMemProp) {
  std::vector<std::unique_ptr<SplitGatherOp>> gathers;
  gathers.reserve(count);

  for (int i = 0; i < count; ++i) {
    settings.vgraphId = i;
    gathers.push_back(std::make_unique<SplitGatherOp>(
        Onnx::Operators::Gather_1, 0, settings, availMemProp));
  }

  return gathers;
}

static std::unique_ptr<ReshapeOp>
decanonicalizeShape(const int64_t axis,
                    Shape origShape,
                    Shape indicesShape,
                    const Op::Settings &settings) {
  std::swap(origShape.front(), origShape[axis]);
  origShape.erase(origShape.begin());
  origShape.insert(origShape.begin(), indicesShape.begin(), indicesShape.end());

  return std::make_unique<ReshapeOp>(
      Onnx::Operators::Reshape_5, origShape, settings);
}

static std::unique_ptr<TransposeOp>
decanonicalizeTranspose(const int64_t axis,
                        Shape inputShape,
                        Shape indicesShape,
                        const Op::Settings &settings) {
  std::vector<int64_t> permutation(inputShape.size() + indicesShape.size() - 1);
  std::iota(permutation.begin(), permutation.end(), 0);
  std::rotate(permutation.begin(),
              permutation.begin() + indicesShape.size(),
              permutation.begin() + indicesShape.size() + axis);

  return std::make_unique<TransposeOp>(
      Onnx::Operators::Transpose_1, permutation, settings);
}

bool SplitGatherPattern::apply(Op *op) const {
  auto gather = dynamic_cast<GatherOp *>(op);

  const auto inputShape           = gather->inShape(GatherOp::dataInIndex());
  const auto axis                 = gather->getAxis();
  const auto availMemProp         = gather->getAvailableMemoryProportion();
  const int64_t virtualGraphCount = op->getIr().getDeviceInfo()->getNumIpus();

  const auto numElements = std::accumulate(
      inputShape.begin(), inputShape.end(), 1, std::multiplies<int64_t>());

  const auto split =
      boost::integer::gcd(virtualGraphCount, numElements / inputShape[axis]);
  const auto stride      = (numElements / inputShape[axis]) / split;
  const auto inTensorId  = op->input->id(GatherOp::dataInIndex());
  const auto idxTensorId = op->input->id(GatherOp::indicesInIndex());
  auto output            = op->outTensor(GatherOp::outIndex());
  op->disconnectOutTensor(output);
  const auto outTensorId = output->id;

  logging::pattern::trace("Splitting {} into {} gathers of size [{}, {}]",
                          op->str(),
                          split,
                          inputShape[axis],
                          (numElements / inputShape[axis]) / split);

  auto t0        = inTensorId;
  auto transpose = canonicalizeTranspose(axis, inputShape, op->settings);
  auto t1        = output->getIr().createIntermediateTensorId(outTensorId);
  transpose->connectInTensor(0, t0);
  transpose->createAndConnectOutTensor(0, t1);
  transpose->setup();

  // Reshape the input tensor to a 2D tensor with the gather axis at the front
  auto reshape = canonicalizeShape(axis, inputShape, op->settings);
  auto t2      = transpose->getIr().createIntermediateTensorId(outTensorId);
  reshape->connectInTensor(0, t1);
  reshape->createAndConnectOutTensor(0, t2);
  reshape->setup();

  // Slice the input tensor along the sequence dimension for each IPU
  auto slices = createSlices(split, stride, op->settings);
  std::vector<TensorId> slicedts;
  slicedts.reserve(split);
  for (int i = 0; i < split; ++i) {
    auto tid = reshape->getIr().createIntermediateTensorId(outTensorId);
    slicedts.push_back(tid);
    slices[i]->connectInTensor(0, t2);
    slices[i]->createAndConnectOutTensor(0, tid);
    slices[i]->setup();
  }

  // Gather the slice fragments on each IPU
  auto gathers = createGathers(split, op->settings, availMemProp);
  std::vector<TensorId> gatheredts;
  gatheredts.reserve(split);
  for (int i = 0; i < split; ++i) {
    auto tid = gathers[i]->getIr().createIntermediateTensorId(outTensorId);
    gatheredts.push_back(tid);
    gathers[i]->connectInTensor(0, slicedts[i]);
    gathers[i]->connectInTensor(1, idxTensorId);
    gathers[i]->createAndConnectOutTensor(0, tid);
    gathers[i]->setup();
  }

  // Concatenate the gathered fragments
  const auto concatDim = gather->inRank(1);
  auto concat          = std::make_unique<ConcatOp>(
      Onnx::Operators::Concat_4, concatDim, op->settings);
  auto t3 = concat->getIr().createIntermediateTensorId(outTensorId);
  for (int i = 0; i < split; ++i) {
    concat->connectInTensor(i, gatheredts[i]);
  }
  concat->createAndConnectOutTensor(0, t3);
  concat->setup();

  const auto indicesShape = gather->inShape(1);

  // Reshape back to the original shape, with the gather axis at the front
  auto unreshape =
      decanonicalizeShape(axis, inputShape, indicesShape, op->settings);
  auto t4 = concat->getIr().createIntermediateTensorId(outTensorId);
  unreshape->connectInTensor(0, t3);
  unreshape->createAndConnectOutTensor(0, t4);
  unreshape->setup();

  // Put the gather axis back in original position
  auto untranspose =
      decanonicalizeTranspose(axis, inputShape, indicesShape, op->settings);
  auto t5 = outTensorId;
  untranspose->connectInTensor(0, t4);
  untranspose->connectOutTensor(0, t5);
  untranspose->setup();

  // Insert the ops into the IR
  auto &graph = op->getGraph();
  graph.moveIntoGraph(std::move(transpose));
  graph.moveIntoGraph(std::move(reshape));
  for (auto &s : slices) {
    graph.moveIntoGraph(std::move(s));
  }
  for (auto &g : gathers) {
    graph.moveIntoGraph(std::move(g));
  }
  graph.moveIntoGraph(std::move(concat));
  graph.moveIntoGraph(std::move(unreshape));
  graph.moveIntoGraph(std::move(untranspose));

  // Remove the old op
  op->disconnectAllInputs();
  op->getGraph().eraseOp(op->id);

  return true;
}

namespace {
static PatternCreator<SplitGatherPattern> splitGatherer("SplitGather", false);
}

} // namespace popart


#include <math.h>
#include <vector>
#include <ros/ros.h>
#include <iomanip>
#include <iostream>
#include <boost/thread.hpp>

#include "geometry_msgs/PoseStamped.h"
#include "sensor_msgs/Joy.h"
#include "std_srvs/Empty.h"

#include "wam_msgs/RTCartVel.h"
#include "wam_msgs/RTOrtnVel.h"
#include "wam_srvs/GravityComp.h"
#include "wam_srvs/Hold.h"
#include "wam_srvs/BHandGraspVel.h"
#include "wam_srvs/BHandSpreadVel.h"
#include "wam_srvs/ForceTorqueTool.h"
#include "wam_srvs/ForceTorqueBase.h"
#include "wam_srvs/ForceTorque.h"

#define MODE_DISCONNECT 0
#define MODE_FREE 1
#define MODE_BASE_CONNECT 2
#define MODE_BASE_UPDATE 3
#define MODE_TOOL_CONNECT 4
#define MODE_FORCE_UPDATE 5
#define MODE_TORQUE_UPDATE 6
#define BUTTON_A 0
#define BUTTON_B 1
#define BUTTON_X 2
#define BUTTON_Y 3
#define BUTTON_LB 4
#define BUTTON_RB 5
#define BUTTON_BACK 6
#define BUTTON_START 7
#define BUTTON_LEFT_STICK 9
#define BUTTON_RIGHT_STICK 10

using namespace std;

const int CNTRL_FREQ = 50; // Frequency at which we will publish our control messages.
const double MAX_FORCE = 6.0;
const double MAX_TORQUE = 0.2;

//WamTeleop Class
class WamTeleop {
  public:
    ros::NodeHandle n_, nh_wam, nh_bhand; // NodeHandles for publishing / subscribing on topics "/... & /wam/..." & "/bhand/..."
    // Boolean statuses for commanded states
    // bool cart_publish, ortn_publish, home_publish;
    // bool hold_publish, home_st, hold_st, ortn_mode;
    // bool open_grasp, close_grasp;
    // bool open_spread, close_spread;
    // bool open_grasp_state, close_grasp_state;
    // bool open_spread_state, close_spread_state;
    // bool stop;
    bool grasp;
    bool spread;
    bool lock_joints;
    bool new_command;
    // bool rt_control;
    // Integer status for BarrettHand commanded state
    // int bh_cmd_st;
    int mode;
    //variables to describe buttons on Joystick and their assignments
    // int deadman_btn, guardian_deadman_btn;
    // int gpr_open_btn, gpr_close_btn;
    // int sprd_open_btn, sprd_close_btn;
    // int ortn_btn, home_btn, hold_btn;
    // int axis_x, axis_y, axis_z;
    // int axis_r, axis_p, axis_yaw;
    int button_pressed;
    //variables to describe velocity commands
    // double max_grsp_vel, max_sprd_vel;
    // double cart_mag, ortn_mag;
    double req_xdir, req_ydir, req_zdir;
    double req_rdir, req_pdir, req_yawdir;
    //Subscribers
    ros::Subscriber joy_sub;
    //Services
    wam_srvs::BHandGraspVel grasp_vel;
    wam_srvs::BHandSpreadVel spread_vel;
    wam_srvs::Hold hold;
    std_srvs::Empty go_home;
    std_srvs::Empty empty;
    //Service Clients
    ros::ServiceClient bhand_init_srv;
    ros::ServiceClient grasp_open_srv;
    ros::ServiceClient grasp_close_srv;
    ros::ServiceClient grasp_vel_srv;
    ros::ServiceClient spread_open_srv;
    ros::ServiceClient spread_close_srv;
    ros::ServiceClient spread_vel_srv;
    ros::ServiceClient go_home_srv;
    ros::ServiceClient hold_srv;
    ros::ServiceClient force_torque_base_srv;
    ros::ServiceClient joy_force_torque_base_srv;
    ros::ServiceClient joy_force_torque_tool_srv;
    ros::ServiceClient disconnect_srv;
    //Messages
    // wam_msgs::RTCartVel cart_vel;
    // wam_msgs::RTOrtnVel ortn_vel;
    //Publishers
    // ros::Publisher cart_vel_pub, ortn_vel_pub;
    // Name our nodehandle "wam" to preceed our messages/services
    WamTeleop() : nh_wam("zeus/wam"), nh_bhand("zeus/bhand") {}

    void init();
    void joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg);
    void updateRT();
    void updateCommand();

    ~WamTeleop() {}
};

// WAM Teleoperation Initialization Function
void WamTeleop::init() {
    hold.request.hold = false; // Default Start for joint hold command is false
    mode = MODE_DISCONNECT;
    lock_joints = false;
    grasp = true;
    spread = false;
    new_command = false;

    joy_sub = n_.subscribe < sensor_msgs::Joy > ("joy", 1, &WamTeleop::joyCallback, this); // /joy

    bhand_init_srv = nh_bhand.serviceClient<std_srvs::Empty>("initialize");
    grasp_open_srv = nh_bhand.serviceClient<std_srvs::Empty>("open_grasp");
    grasp_close_srv = nh_bhand.serviceClient<std_srvs::Empty>("close_grasp");
    spread_open_srv = nh_bhand.serviceClient<std_srvs::Empty>("open_spread");
    spread_close_srv = nh_bhand.serviceClient<std_srvs::Empty>("close_spread");
    spread_vel_srv = nh_bhand.serviceClient<wam_srvs::BHandSpreadVel>("spread_vel");
    grasp_vel_srv = nh_bhand.serviceClient<wam_srvs::BHandGraspVel>("grasp_vel");
    go_home_srv = nh_wam.serviceClient<std_srvs::Empty>("go_home");                  // /wam/go_home
    hold_srv = nh_wam.serviceClient<wam_srvs::Hold>("hold_joint_pos");               // /wam/hold_joint_pos
    force_torque_base_srv = nh_wam.serviceClient<wam_srvs::ForceTorqueBase>("force_torque_base");
    joy_force_torque_base_srv = nh_wam.serviceClient<wam_srvs::ForceTorque>("joy_force_torque_base");
    joy_force_torque_tool_srv = nh_wam.serviceClient<wam_srvs::ForceTorque>("joy_force_torque_tool");
    disconnect_srv = nh_wam.serviceClient<std_srvs::Empty>("disconnect_systems");
    // cout << "MODE_FREE" << endl;
    // cart_vel_pub = nh_wam.advertise<wam_msgs::RTCartVel>("cart_vel_cmd", 1);         // /wam/cart_vel_cmd
    // ortn_vel_pub = nh_wam.advertise<wam_msgs::RTOrtnVel>("ortn_vel_cmd", 1);         // /wam/ortn_vel_cmd
}

void WamTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg) {
    //Set our publishing states back to false for new commands
    std::vector<int> input;
    input = joy_msg->buttons;
    for (int i = 0; i < joy_msg->buttons.size(); ++i) {
        if (!new_command && joy_msg->buttons[i]) {
            button_pressed = i;
            new_command = true;
        }
    }
    if (-0.25 < joy_msg->axes[1] && joy_msg->axes[1] < 0.25) {
        req_xdir = 0.0;
    } else {
        req_xdir = joy_msg->axes[1];
    }

    if (-0.25 < joy_msg->axes[0] && joy_msg->axes[0] < 0.25) {
        req_ydir = 0.0;
    } else {
        req_ydir = joy_msg->axes[0];
    }

    if (-0.25 < joy_msg->axes[4] && joy_msg->axes[4] < 0.25) {
        req_zdir = 0.0;
    } else {
        req_zdir = joy_msg->axes[4];
    }
    // //RPY Velocity Portion
    // if ((joy_msg->axes[axis_r] > 0.25 || joy_msg->axes[axis_r] < -0.25) && ortn_mode) {
    //     req_rdir = -joy_msg->axes[axis_r];
    //     ortn_publish = true;
    // } else {
    //     req_rdir = 0.0;
    // }

    // if ((joy_msg->axes[axis_y] > 0.25 || joy_msg->axes[axis_y] < -0.25) && ortn_mode) {
    //     req_pdir = -joy_msg->axes[axis_p];
    //     ortn_publish = true;
    // } else {
    //     req_pdir = 0.0;
    // }
    // if ((joy_msg->axes[axis_z] > 0.25 || joy_msg->axes[axis_z] < -0.25) && ortn_mode) {
    //     req_yawdir = joy_msg->axes[axis_yaw];
    //     ortn_publish = true;
    // } else {
    //     req_yawdir = 0.0;
    // }

}

void WamTeleop::updateRT() {
    ros::Rate r(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)
    while (ros::ok()) {
        wam_srvs::ForceTorque ft;
        // cout << "Mode: " << mode << endl;
        switch (mode) {
            case MODE_DISCONNECT:
                disconnect_srv.call(empty);
                mode = MODE_FREE;
                cout << "\tFREE" << endl;
                break;
            case MODE_FREE:
                break;
            case MODE_BASE_CONNECT:
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.force.push_back(req_xdir);
                ft.request.force.push_back(req_ydir);
                ft.request.force.push_back(req_zdir);
                ft.request.kp.push_back(4.0);
                ft.request.kp.push_back(4.0);
                ft.request.kp.push_back(4.0);
                ft.request.kd.push_back(0.2);
                ft.request.kd.push_back(0.2);
                ft.request.kd.push_back(0.2);
                ft.request.initialize = true;
                joy_force_torque_base_srv.call(ft);
                mode = MODE_BASE_UPDATE;
                cout << "\tBASE FORCE UPDATE" << endl;
                break;
            case MODE_BASE_UPDATE:
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.force.push_back(req_xdir);
                ft.request.force.push_back(req_ydir);
                ft.request.force.push_back(req_zdir);
                ft.request.kp.push_back(150.0);
                ft.request.kp.push_back(150.0);
                ft.request.kp.push_back(150.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.initialize = false;
                joy_force_torque_base_srv.call(ft);
                break;
            case MODE_TOOL_CONNECT:
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.force.push_back(0.0);
                ft.request.force.push_back(0.0);
                ft.request.force.push_back(0.0);
                ft.request.kp.push_back(0.0);
                ft.request.kp.push_back(0.0);
                ft.request.kp.push_back(0.0);
                ft.request.kd.push_back(0.0);
                ft.request.kd.push_back(0.0);
                ft.request.kd.push_back(0.0);
                ft.request.initialize = true;
                joy_force_torque_tool_srv.call(ft);
                mode = MODE_FORCE_UPDATE;
                cout << "\tTOOL FORCE UPDATE" << endl;
                break;
            case MODE_FORCE_UPDATE:
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.torque.push_back(0.0);
                ft.request.force.push_back(req_xdir);
                ft.request.force.push_back(req_ydir);
                ft.request.force.push_back(req_zdir);
                ft.request.kp.push_back(50.0);
                ft.request.kp.push_back(50.0);
                ft.request.kp.push_back(50.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.initialize = false;
                joy_force_torque_tool_srv.call(ft);
                break;
            case MODE_TORQUE_UPDATE:
                ft.request.torque.push_back(req_ydir);
                ft.request.torque.push_back(req_xdir);
                ft.request.torque.push_back(req_zdir);
                ft.request.force.push_back(0.0);
                ft.request.force.push_back(0.0);
                ft.request.force.push_back(0.0);
                ft.request.kp.push_back(50.0);
                ft.request.kp.push_back(50.0);
                ft.request.kp.push_back(50.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.kd.push_back(5.0);
                ft.request.initialize = false;
                joy_force_torque_tool_srv.call(ft);
                break;
            }
        r.sleep();
    }
}

// Function for updating the commands and publishing
void WamTeleop::updateCommand() {
    if (new_command) {
        switch (button_pressed) {
            case BUTTON_A:
                grasp = !grasp;
                if (grasp) {
                    cout << "\t\tcalling grasp open" << endl;
                    grasp_open_srv.call(empty);
                } else {
                    cout << "\t\tcalling grasp close" << endl;
                    grasp_close_srv.call(empty);
                }
                break;
            case BUTTON_B:
                spread = !spread;
                if (spread) {
                    cout << "\t\tcalling spread open" << endl;
                    spread_open_srv.call(empty);
                } else {
                    cout << "\t\tcalling spread close" << endl;
                    spread_close_srv.call(empty);
                }
                break;
            case BUTTON_X:
                break;
            case BUTTON_Y:
                break;
            case BUTTON_LB:
                mode = (mode + 1) % 7;
                switch (mode) {
                    case MODE_DISCONNECT:
                        // cout << "MODE_DISCONNECT" << endl;
                        break;
                    case MODE_FREE:
                        cout << "\tFREE" << endl;
                        break;
                    case MODE_BASE_CONNECT:
                        // cout << "MODE_BASE_CONNECT" << endl;
                        break;
                    case MODE_BASE_UPDATE:
                        cout << "\tBASE FORCE UPDATE" << endl;
                        break;
                    case MODE_TOOL_CONNECT:
                        // cout << "MODE_TOOL_CONNECT" << endl;
                        break;
                    case MODE_FORCE_UPDATE:
                        cout << "\tTOOL FORCE UPDATE" << endl;
                        break;
                    case MODE_TORQUE_UPDATE:
                        cout << "\tTOOL TORQUE UPDATE" << endl;
                        break;
                }
                break;
            case BUTTON_RB:
                lock_joints = !lock_joints;
                cout << "\t\tjoints locked: " << boolalpha << lock_joints << endl;
                hold.request.hold = lock_joints;
                hold_srv.call(hold);
                break;
            case BUTTON_BACK:
                disconnect_srv.call(empty);
                mode = MODE_FREE;
                cout << "\t\tcalling go home" << endl;
                go_home_srv.call(empty);
                lock_joints = true;
                break;
            case BUTTON_START:
                cout << "\t\tinitializing hand and setting velocities to 16" << endl;
                bhand_init_srv.call(empty);
                spread_vel.request.velocity = 16;
                spread_vel_srv.call(spread_vel);
                grasp_vel.request.velocity = 16;
                grasp_vel_srv.call(grasp_vel);
                spread_close_srv.call(empty);
                break;
            case BUTTON_LEFT_STICK:
                break;
            case BUTTON_RIGHT_STICK:
                break;
        }
        new_command = false;
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "wam_teleop"); // Start our wam_node and name it "wam_teleop"
    // don't remove spinner lines or arm won't subscribe to wam topics
    ros::AsyncSpinner spinner(0);
    spinner.start();

    WamTeleop wam_teleop; // Declare a member of WamTeleop "wam_teleop"
    wam_teleop.init(); // Initialize our teleoperation
    boost::thread feedback_thread(&WamTeleop::updateRT, &wam_teleop);

    ros::Rate pub_rate(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)
    // Looping at the specified frequency while our ros node is ok
    cout << "******************************************************************" << endl;
    cout << "* WAM Teleoperation Node with the Logitech Controller            *" << endl;
    cout << "******************************************************************" << endl;
    cout << "* LB BUTTON:     Mode Switch                                     *" << endl;
    cout << "* RB BUTTON:     Toggle Hold Joints                              *" << endl;
    cout << "* START BUTTON:  Initialize Hand                                 *" << endl;
    cout << "* BACK BUTTON:   Go Home                                         *" << endl;
    cout << "* A BUTTON:      Toggle Grasp                                    *" << endl;
    cout << "* B BUTTON:      Toggle Spread                                   *" << endl;
    cout << "* LEFT STICK:    X & Y Axis Motion                               *" << endl;
    cout << "* RIGHT STICK:   Z Axis Motion                                   *" << endl;
    cout << "******************************************************************" << endl;
    cout << "Current Mode:" << endl;
    while (wam_teleop.n_.ok()) {
        ros::spinOnce();
        wam_teleop.updateCommand();
        pub_rate.sleep();
    }
    return 0;
}


//          Copyright Camille Gillot 2012 - 2015.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef RTTI_MMETHOD_DECLARE_TRAITS_HPP
#define RTTI_MMETHOD_DECLARE_TRAITS_HPP

#include "mmethod/config.hpp"
#include "mmethod/traits/tags.hpp"

#include <boost/mpl/next.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/reverse_fold.hpp>

namespace rtti {
namespace mmethod {
namespace detail {

template<typename Ret, typename Policy, typename Args>
struct make_declare_traits
{
  typedef typename boost::mpl::transform<Args, tags::unwrap>::type unwrapped_args;
  typedef typename boost::mpl::transform<Args, tags::is_virtual>::type type_tags;

  BOOST_STATIC_CONSTANT(std::size_t, vsize = tags::virtual_size<Args>::value);
  BOOST_STATIC_CONSTANT(std::size_t, type_bitset = (boost::mpl::reverse_fold<
      type_tags,
      boost::mpl::size_t<0>,
      boost::mpl::if_<mpl_::_2
      , boost::mpl::next<boost::mpl::times<mpl_::_1, boost::mpl::size_t<2> > >
      , boost::mpl::times<mpl_::_1, boost::mpl::size_t<2> >
      >
    >::type::value
  ));

  typedef Policy policy;

  BOOST_STATIC_ASSERT_MSG( (vsize > 0), "At least one virtual parameter must be provided." );
};

} // namespace detail
} // namespace mmethod
} // namespace rtti

#endif


/* Search within a matrix of neighbours for similar segments
 *
 * author: Bjoern Krueger (kruegerb@cs.uni-bonn.de)
 *
 * example (Matlab) call:
 * res = C_findPathsTrellisGlobalOptimization(ind,dist,max(ind(:)));
 */

#include "mex.h"
#include <float.h>
#include <cmath>
#include <limits>

#include "path.h"
// #include "TrellisTree.h"
#include "SimpleHashUnsafe.h"

/******* included for boost library *******/
#include <boost/config.hpp>
#include <iostream>
#include <fstream>

#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

using namespace boost;
/******* included for boost library *******/

#define getSub(i,j)   (iFrames*(j)+(i))
#define vuiInd(i,j)   (*(puiInd + iNeigh*(j) + (i)))
#define  vfDis(i,j)   (*( pfDis + iNeigh*(j) + (i)))

class BKNode{
    public:
    unsigned int	 frame;
    unsigned int	 neighbour;
    unsigned int     value;
    double           distance;
    std::vector<unsigned int> childs;

    BKNode(unsigned int f,unsigned int n,unsigned int v, double d)
    {
        frame     = f;
        neighbour = n;
        value     = v;
        distance  = d;
    }

    ~BKNode(){}

    void addChild(int childNode){
        childs.push_back(childNode);
//         mexPrintf("\naddChild: %i -> %i\n", value, childNode);
    }

    void writeNode(){
        mexPrintf("\nNode: frame=%i neigh=%i value=%i dist=%f", frame,neighbour,value,distance);
    }
};

// Bin Seek search:
int binsuch(unsigned int* feld,unsigned int l,unsigned int zusuchen )
{
//     mexPrintf("\n");
//   for(int b=0;b<l;b++)
//       mexPrintf(" %i",feld[b]);

  if( l<1 )
  {
    // leeres Feld? Nichts gefunden!
//        mexPrintf("leer");
    return -1000000000;
  }
  else if( l==1 )
  {
    // nur ein Element; vergleichen und ggf. zurückgeben:
//      mexPrintf("eins");
    return ( feld[0]==zusuchen ? 0 : -100000000 );
  }
  else
  {
    // Mehr als ein Element
    // Mitte suchen, und ggf. links oder rechts suchen:
    int index_Mitte = l/2;
    if( feld[index_Mitte]==zusuchen )
    {
      // Zufällig getroffen!
      return index_Mitte;
    }
    else if( feld[index_Mitte]>zusuchen )
    {
//          mexPrintf(" >%i ",index_Mitte);
//          mexPrintf("\n l=%i",l);
      // Mitte ist zu groß, also links weitersuchen
      return binsuch( feld, index_Mitte, zusuchen );
    }
    else
    {
      // Mitte zu klein, also in rechter Hälfte suchen:
//         mexPrintf(" <%i ",index_Mitte);
//         mexPrintf("\n l=%i",l);
      return binsuch( &feld[index_Mitte+1], l - index_Mitte - 1, zusuchen )+index_Mitte+1;
    }
  }

} // int *binsuch( int feld[], size_t l, int zusuchen )

/* Path finding algorithm */
void getPaths(mxArray* pInd, mxArray* pDis, mxArray* pPaths, int iNeigh, int iFrames, int iDBSize, mxArray* pathDists){

    // Some usefull variables
    double        dLengthFac = 0.75;
    unsigned int  uiGap      = 2;
    int           iNumPaths  = 0;
    int           iTreeCount = 1;

    // Frame list: stores where a frame occurs
//     int          *pFrameList = new int[iDBSize];

    unsigned int* puiInd     = (unsigned int*)mxGetPr(pInd);
    double*       pfDis      = (double*)      mxGetPr(pDis);
    double*       pfPathDist = (double*)      mxGetPr(pathDists);

    // Lookup table: Which neighbours correspond to which node?
    unsigned int* lookup=new unsigned int[iNeigh*iFrames];

    for(int j =0;j<iFrames;j++)
        for(int i=0;i<iNeigh;i++)
            lookup[j*iNeigh + i]=100000000;

    typedef adjacency_list < listS, vecS, directedS, no_property, property < edge_weight_t, int > > graph_t;
    typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
    typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
    typedef std::pair<int, int> Edge;

    // Find out number of nodes and create them.
    int numNodes=0;
    int numEdges=0;

    std::vector<BKNode> Nodes;

    mexPrintf("\nInit done.\n");

    for(int j=0;j<iFrames;j++){
         //mexPrintf("Neigh=%i",j);
        for(int i=0;i<iNeigh;i++){
             //mexPrintf("Frame=%i",i);
            if(vuiInd(i,j)!=0){
                numNodes++;
                BKNode newNode(j,i,vuiInd(i,j),vfDis(i,j));
                Nodes.push_back(newNode);

                lookup[j*iNeigh + i]=Nodes.size()-1;
                //mexPrintf("%i ",numNodes);
            }
        }
    }
    /*
	 mexPrintf("\nDebug Lookup:\n");
     for(int i=0;i<iNeigh;i++){
         mexPrintf("\n");
         for(int j=0;j<iFrames;j++)
             mexPrintf(" %i",lookup[j*iNeigh + i]);
     }*/

    mexPrintf("\nNodes built!");
//     mexPrintf("\nSearch Edges:");

    for(int curNode=0;curNode<Nodes.size();curNode++){
         //mexPrintf("\nNode %i: Frame = %i, Neigh = %i, Value = %i",curNode,Nodes[curNode].frame,Nodes[curNode].neighbour,Nodes[curNode].value);
        int position = -1;

      /*  mexPrintf("\n");
        for(int i=0;i<iNeigh;i++)
            mexPrintf(" %i ",*(puiInd + iNeigh*(Nodes[curNode].frame+1) + i));
      */

        position = binsuch(puiInd + iNeigh*(Nodes[curNode].frame+1),iNeigh,Nodes[curNode].value+1);
        if(position>-1){
            if(lookup[position+iNeigh*(Nodes[curNode].frame+1)]!=100000000){
                Nodes[curNode].addChild(lookup[position+iNeigh*(Nodes[curNode].frame+1)]);
                numEdges++;
                //mexPrintf("\n   \\ %i %i Value=%i Node=%i",position,Nodes[curNode].frame+1,Nodes[curNode].value+1,lookup[position+iNeigh*(Nodes[curNode].frame+1)]);
            }
        }

        // Vert
        position = -1;
        position = binsuch(puiInd + iNeigh*(Nodes[curNode].frame+1),iNeigh,Nodes[curNode].value);
        if(position>-1){
            if(lookup[position+iNeigh*(Nodes[curNode].frame+1)]!=100000000){
                Nodes[curNode].addChild(lookup[position+iNeigh*(Nodes[curNode].frame+1)]);
                numEdges++;
                 //mexPrintf("\n   - %i %i Value=%i Node=%i",position,Nodes[curNode].frame+1,Nodes[curNode].value,lookup[position+iNeigh*(Nodes[curNode].frame+1)]);
            }
        }
        // Horz
        position = -1;
        position = binsuch(puiInd + iNeigh*(Nodes[curNode].frame),iNeigh,Nodes[curNode].value+1);
        if(position>-1){
            if(lookup[position+iNeigh*Nodes[curNode].frame]!=100000000){
                Nodes[curNode].addChild(lookup[position+iNeigh*Nodes[curNode].frame]);
                numEdges++;
                 //mexPrintf("\n   | %i %i Value=%i Node=%i",position,Nodes[curNode].frame,Nodes[curNode].value+1,lookup[position+iNeigh*Nodes[curNode].frame]);
            }
        }
    }
    /*
    mexPrintf("\nNeighbours found!");
    */
    mexPrintf("\nnumEdges = %i\n",numEdges);

    Edge*   edge_array=new Edge[numEdges+iNeigh];//
//     double     weights[numEdges+iNeigh];
    int*       weights=new int[numEdges+iNeigh];

    int curEdge=0;

    for(int node=0;node<Nodes.size();node++){

//          mexPrintf(".");

        for(int edge=0;edge<Nodes[node].childs.size();edge++){
             //mexPrintf("\n Edge = %i", curEdge);
            edge_array[curEdge] = Edge(node,Nodes[node].childs[edge]);
//             mexPrintf(" : %i --> %i",      node       ,      Nodes[node].childs[edge]       );
//             mexPrintf(" : %i,%i --> %i,%i = %i",Nodes[node].value,Nodes[node].frame,Nodes[Nodes[node].childs[edge]].value, Nodes[Nodes[node].childs[edge]].frame,(int) (Nodes[Nodes[node].childs[edge]].distance*1000+Nodes[node].distance*1000));
//             mexPrintf("\nDist Check: Node.dist=%f Node->child.dist=%f",Nodes[node].distance,Nodes[Nodes[node].childs[edge]].distance);
            weights   [curEdge] = (int) (Nodes[node].distance*1000);
            curEdge++;
        }
        if(Nodes[node].frame==iFrames-1){
             //mexPrintf("\n Edge = %i", curEdge);
            edge_array[curEdge] = Edge(node,numNodes);
             //mexPrintf(" : %i --> TARGET",Nodes[node].value);
            weights   [curEdge] = (int) 0;//(Nodes[node].distance*1000);
            curEdge++;
        }
    }
//     mexPrintf("\nEdege Array constructed");
//     for(int ew=0;ew<curEdge;ew++) mexPrintf("\nw(%i)=%i",ew,weights[ew]);

//     mexPrintf("\nNumNodes = %i\n",numNodes);

//     mexPrintf("\n\nNumNodes = %i\n",numNodes);

     mexPrintf("Edges built\n");
    // Create edges
//     Edge edge_array[] = {Edge(1,2),Edge(2,3),Edge(3,4),Edge(1,5),Edge(3,6),Edge(6,4),Edge(4,9)};
    //int weih[] = { 1, 2, 1, 2, 7, 3, 1, 1, 2, 1, 2, 7, 3, 1};
    int num_arcs = curEdge;//sizeof(edge_array) / sizeof(Edge);

    graph_t g(edge_array, edge_array + num_arcs, weights, numNodes+1);
    property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);

// //     graph_t g(numNodes+1);
// //     property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
// //     for (std::size_t j = 0; j < num_arcs; ++j) {
// //         edge_descriptor e;
// //         bool inserted;
// //
// //         mexPrintf("\nedge_array.first=%i edge_array.second%i",edge_array[j].first,edge_array[j].second);
// //
// //         tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
// //         weightmap[e] = weights[j];
// //     }

    std::vector<vertex_descriptor>  p(num_vertices(g));
    std::vector<int>                d(num_vertices(g));

// 	mexPrintf("size(d) = %i\n",d.size());

    for(int startNode=0;startNode<iNeigh;startNode++){

//         mexPrintf(".");

        vertex_descriptor s = vertex(startNode, g);

        dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

//          mexPrintf("distances and parents:\n");
        graph_traits < graph_t >::vertex_iterator vi, vend;
        int* parents=new int[numNodes+1];
        int* dists  =new int[numNodes+1];

        for (tie(vi, vend) = vertices(g); vi != vend; ++vi) {
//              mexPrintf("distance(%i) = %i Parent = %i \n",*vi,d[*vi],p[*vi]);
            parents[*vi]=p[*vi];
              dists[*vi]=d[*vi];
        }

        int depth=1;
        int curNode=numNodes;

        *pfPathDist =(double) dists[curNode];
         pfPathDist++;

        while(parents[curNode]!=startNode && parents[curNode] < numNodes && parents[curNode] > 0){
            depth++;
            curNode=parents[curNode];
        }

//          mexPrintf("PathDepth = %i",depth);

        mwSize dims[2];
        dims[0] = (mwSize)2;
        dims[1] = (mwSize)depth;

        mxArray *pSteps=mxCreateNumericArray(2,(const mwSize*)&dims,mxUINT32_CLASS,mxREAL);
        unsigned int* puiSteps=(unsigned int*)mxGetPr(pSteps);

        curNode=parents[numNodes];
        while(parents[curNode]!=startNode && parents[curNode] < numNodes && parents[curNode] > 0){

            *puiSteps = Nodes[curNode].frame+1; puiSteps++;
            *puiSteps = Nodes[curNode].value;   puiSteps++;

            curNode=parents[curNode];
        }

        *puiSteps = Nodes[curNode].frame+1; puiSteps++;
        *puiSteps = Nodes[curNode].value;   puiSteps++;
        curNode=parents[curNode];
//          mexPrintf("\nPING\n");
        *puiSteps = Nodes[curNode].frame+1; puiSteps++;
        *puiSteps = Nodes[curNode].value;   puiSteps++;
        curNode=parents[curNode];

//         mexPrintf("C");

        mxSetCell(pPaths, (mwIndex)getSub(startNode,0), pSteps);

        delete parents;
        delete dists  ;
// 		mexPrintf("D");
    }
    mexPrintf("Search Done");

	delete lookup;
	delete edge_array;
	delete weights;

//     mexPrintf("\nDone!");
}

/* The gateway routine */
void mexFunction(int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[])
{

    double    *pDis;

    int   	  *pInd,
              *pDBSize;

    unsigned int iNeigh,
                 iFrames;

    /*  Check for proper number of arguments. */
    if (nrhs != 3)
    mexErrMsgTxt("Three inputs required.");

    if (nlhs != 2)
    mexErrMsgTxt("Two outputs required.");

    /* Get Pointers to input arguments */
    pInd     = (int*)    mxGetPr(prhs[0]);
    pDis     = (double*) mxGetPr(prhs[1]);
    pDBSize  = (int*)    mxGetPr(prhs[2]);

    /* Get the dimensions of the input array. */
    iNeigh   = mxGetM(prhs[0]);
    iFrames  = mxGetN(prhs[0]);

    /* Create output cell array */
    mwSize pDims[2];
    pDims[0] = (mwSize)1;
    pDims[1] = (mwSize)iNeigh; // max Number of possible paths

    plhs[0]  = mxCreateCellArray(2, (const mwSize*)pDims);
    plhs[1]  = mxCreateDoubleMatrix(1, iNeigh, mxREAL);

    /* Call Path finding algorithm */
    getPaths((mxArray*)prhs[0], (mxArray*)prhs[1], plhs[0], iNeigh, iFrames, *pDBSize, plhs[1]);

}


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

// This is a modified version of the Kitti "object development kit"
// available from http://www.cvlibs.net/datasets/kitti/eval_object.php

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <common/filesystem.hpp>
#include <common/types.hpp>

#include <cstdio>
#include <cmath>
#include <cstring>
#include <cassert>

#include <iostream>
#include <algorithm>
#include <vector>
#include <numeric>
#include <string>
#include <functional>

#include "kittiobjevalmodule.hpp"

#ifdef BOOST_GEOMETRY_REGISTER_C_ARRAY_CS
BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
#endif

using autoware::common::types::bool8_t;
using autoware::common::types::char8_t;
using autoware::common::types::float64_t;
using std::size_t;

typedef boost::geometry::model::polygon<boost::geometry::model::d2::point_xy<float64_t>> Polygon;

/*=======================================================================
STATIC EVALUATION PARAMETERS
=======================================================================*/

// easy, moderate and hard evaluation level
enum class DIFFICULTY {EASY = 0, MODERATE = 1, HARD = 2};

// evaluation metrics: image, ground or 3D
enum class METRIC {IMAGE = 0, GROUND = 1, BOX3D = 2};

// evaluation parameter
// minimum height for evaluated groundtruth/detections
constexpr int32_t MIN_HEIGHT[3] = {40, 25, 25};
// maximum occlusion level of the groundtruth used for evaluation
constexpr int32_t MAX_OCCLUSION[3] = {0, 1, 2};
// maximum truncation level of the groundtruth used for evaluation
constexpr float64_t MAX_TRUNCATION[3] = {0.15, 0.3, 0.5};

// evaluated object classes
enum class CLASSES {CAR = 0, PEDESTRIAN = 1, CYCLIST = 2};
constexpr int NUM_CLASS = 3;

// parameters varying per class
std::vector<std::basic_string<char8_t>> CLASS_NAMES;
std::vector<std::basic_string<char8_t>> CLASS_NAMES_CAP;
// the minimum overlap required for 2D evaluation on the image/ground plane and 3D evaluation
constexpr float64_t MIN_OVERLAP[3][3] = {{0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}, {0.7, 0.5, 0.5}};

// no. of recall steps that should be evaluated (discretized)
constexpr float64_t N_SAMPLE_PTS = 41;

// initialize class names
void initGlobals()
{
  CLASS_NAMES.push_back("car");
  CLASS_NAMES.push_back("pedestrian");
  CLASS_NAMES.push_back("cyclist");
  CLASS_NAMES_CAP.push_back("Car");
  CLASS_NAMES_CAP.push_back("Pedestrian");
  CLASS_NAMES_CAP.push_back("Cyclist");
}

/*=======================================================================
DATA TYPES FOR EVALUATION
=======================================================================*/

// holding data needed for precision-recall and precision-aos
struct tPrData
{
  std::vector<float64_t> v;           // detection score for computing score thresholds
  float64_t similarity;          // orientation similarity
  int32_t tp;                 // true positives
  int32_t fp;                 // false positives
  int32_t fn;                 // false negatives
  tPrData()
  : similarity(0), tp(0), fp(0), fn(0) {}
};

// holding bounding boxes for ground truth and detections
struct tBox
{
  std::basic_string<char8_t> type;      // object type as car, pedestrian or cyclist,...
  float64_t x1;        // left corner
  float64_t y1;        // top corner
  float64_t x2;        // right corner
  float64_t y2;        // bottom corner
  float64_t alpha;     // image orientation
  tBox(
    std::basic_string<char8_t> type, float64_t x1, float64_t y1, float64_t x2, float64_t y2,
    float64_t alpha)
  : type(type), x1(x1), y1(y1), x2(x2), y2(y2), alpha(alpha) {}
};

// holding ground truth data
struct tGroundtruth
{
  tBox box;           // object type, box, orientation
  float64_t truncation;  // truncation 0..1
  int32_t occlusion;  // occlusion 0,1,2 (non, partly, fully)
  float64_t ry;
  float64_t t1, t2, t3;
  float64_t h, w, l;
  tGroundtruth()
  : box(tBox("invalild", -1, -1, -1, -1, -10)), truncation(-1), occlusion(-1) {}
  tGroundtruth(tBox box, float64_t truncation, int32_t occlusion)
  : box(box), truncation(truncation), occlusion(occlusion) {}
  tGroundtruth(
    std::basic_string<char8_t> type, float64_t x1, float64_t y1, float64_t x2,
    float64_t y2, float64_t alpha, float64_t truncation, int32_t occlusion)
  : box(tBox(type, x1, y1, x2, y2, alpha)), truncation(truncation), occlusion(occlusion) {}
};

// holding detection data
struct tDetection
{
  tBox box;       // object type, box, orientation
  float64_t thresh;  // detection score
  float64_t ry;
  float64_t t1, t2, t3;
  float64_t h, w, l;
  tDetection()
  : box(tBox("invalid", -1, -1, -1, -1, -10)), thresh(-1000) {}
  tDetection(tBox box, float64_t thresh)
  : box(box), thresh(thresh) {}
  tDetection(
    std::basic_string<char8_t> type, float64_t x1, float64_t y1, float64_t x2,
    float64_t y2, float64_t alpha, float64_t thresh)
  : box(tBox(type, x1, y1, x2, y2, alpha)), thresh(thresh) {}
};

/*=======================================================================
FUNCTIONS TO LOAD DETECTION AND GROUND TRUTH DATA ONCE, SAVE RESULTS
=======================================================================*/
std::vector<tDetection> loadDetections(
  std::basic_string<char8_t> file_name, bool8_t & compute_aos,
  std::vector<bool8_t> & eval_image, std::vector<bool8_t> & eval_ground,
  std::vector<bool8_t> & eval_3d, bool8_t & success)
{
  // holds all detections (ignored detections are indicated by an index vector
  std::vector<tDetection> detections;
  FILE * fp = fopen(file_name.c_str(), "r");
  if (!fp) {
    success = false;
    return detections;
  }
  while (!feof(fp)) {
    tDetection d;
    float64_t trash;
    char8_t str[255];
    if (fscanf(
        fp, "%s %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
        str, &trash, &trash, &d.box.alpha, &d.box.x1, &d.box.y1,
        &d.box.x2, &d.box.y2, &d.h, &d.w, &d.l, &d.t1, &d.t2, &d.t3,
        &d.ry, &d.thresh) == 16)
    {
      d.box.type = str;
      detections.push_back(d);

      // orientation=-10 is invalid, AOS is not evaluated if at least one orientation is invalid
      if (d.box.alpha == -10) {
        compute_aos = false;
      }

      // a class is only evaluated if it is detected at least once
      for (size_t c = 0; c < NUM_CLASS; c++) {
        if (!strcasecmp(
            d.box.type.c_str(),
            CLASS_NAMES[c].c_str()) || !strcasecmp(d.box.type.c_str(), CLASS_NAMES_CAP[c].c_str()))
        {
          if (!eval_image[c] && d.box.x1 >= 0) {
            eval_image[c] = true;
          }
          if (!eval_ground[c] && d.t1 != -1000 && d.t3 != -1000 && d.w > 0 && d.l > 0) {
            eval_ground[c] = true;
          }
          if (!eval_3d[c] && d.t1 != -1000 && d.t2 != -1000 && d.t3 != -1000 && d.h > 0 &&
            d.w > 0 && d.l > 0)
          {
            eval_3d[c] = true;
          }
          break;
        }
      }
    }
  }

  fclose(fp);
  success = true;
  return detections;
}

std::vector<tGroundtruth> loadGroundtruth(std::basic_string<char8_t> file_name, bool8_t & success)
{
  // holds all ground truth (ignored ground truth is indicated by an index vector
  std::vector<tGroundtruth> groundtruth;
  FILE * fp = fopen(file_name.c_str(), "r");
  if (!fp) {
    success = false;
    return groundtruth;
  }
  while (!feof(fp)) {
    tGroundtruth g;
    char8_t str[255];
    if (fscanf(
        fp, "%s %lf %d %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
        str, &g.truncation, &g.occlusion, &g.box.alpha,
        &g.box.x1, &g.box.y1, &g.box.x2, &g.box.y2,
        &g.h, &g.w, &g.l, &g.t1,
        &g.t2, &g.t3, &g.ry) == 15)
    {
      g.box.type = str;
      groundtruth.push_back(g);
    }
  }
  fclose(fp);
  success = true;
  return groundtruth;
}

void saveStats(
  const std::vector<float64_t> & precision, const std::vector<float64_t> & aos, FILE * fp_det,
  FILE * fp_ori)
{
  // save precision to file
  if (precision.empty()) {
    return;
  }
  for (uint32_t i = 0; i < precision.size(); i++) {
    fprintf(fp_det, "%f ", precision[i]);
  }
  fprintf(fp_det, "\n");

  // save orientation similarity, only if there were no invalid orientation entries in submission
  // (alpha=-10)
  if (aos.empty()) {
    return;
  }
  for (uint32_t i = 0; i < aos.size(); i++) {
    fprintf(fp_ori, "%f ", aos[i]);
  }
  fprintf(fp_ori, "\n");
}

/*=======================================================================
EVALUATION HELPER FUNCTIONS
=======================================================================*/

// criterion defines whether the overlap is computed with respect to both areas (ground truth and
// detection) or with respect to box a or b (detection and "dontcare" areas)
inline float64_t imageBoxOverlap(tBox a, tBox b, int32_t criterion = -1)
{
  // overlap is invalid in the beginning
  float64_t o = -1;

  // get overlapping area
  float64_t x1 = std::max(a.x1, b.x1);
  float64_t y1 = std::max(a.y1, b.y1);
  float64_t x2 = std::min(a.x2, b.x2);
  float64_t y2 = std::min(a.y2, b.y2);

  // compute width and height of overlapping area
  float64_t w = x2 - x1;
  float64_t h = y2 - y1;

  // set invalid entries to 0 overlap
  if (w <= 0 || h <= 0) {
    return 0;
  }

  // get overlapping areas
  float64_t inter = w * h;
  float64_t a_area = (a.x2 - a.x1) * (a.y2 - a.y1);
  float64_t b_area = (b.x2 - b.x1) * (b.y2 - b.y1);

  // intersection over union overlap depending on users choice
  if (criterion == -1) {   // union
    o = inter / (a_area + b_area - inter);
  } else if (criterion == 0) {  // bbox_a
    o = inter / a_area;
  } else if (criterion == 1) {  // bbox_b
    o = inter / b_area;
  }

  // overlap
  return o;
}

inline float64_t imageBoxOverlap(tDetection a, tGroundtruth b, int32_t criterion = -1)
{
  return imageBoxOverlap(a.box, b.box, criterion);
}

// compute polygon of an oriented bounding box
template<typename T>
Polygon toPolygon(const T & g)
{
  boost::numeric::ublas::matrix<float64_t> mref(2, 2);
  mref(0, 0) = cos(g.ry); mref(0, 1) = sin(g.ry);
  mref(1, 0) = -sin(g.ry); mref(1, 1) = cos(g.ry);

  boost::numeric::ublas::matrix<float64_t> corners(2, 4);
  float64_t data[] = {g.l / 2, g.l / 2, -g.l / 2, -g.l / 2,
    g.w / 2, -g.w / 2, -g.w / 2, g.w / 2};
  std::copy(data, data + 8, corners.data().begin());
  boost::numeric::ublas::matrix<float64_t> gc = prod(mref, corners);
  for (size_t i = 0; i < 4; ++i) {
    gc(0, i) += g.t1;
    gc(1, i) += g.t3;
  }

  float64_t points[][2] =
  {{gc(0, 0), gc(1, 0)}, {gc(0, 1), gc(1, 1)}, {gc(0, 2), gc(1, 2)}, {gc(0, 3), gc(1, 3)}, {gc(
        0,
        0),
      gc(1, 0)}};
  Polygon poly;
  boost::geometry::append(poly, points);
  return poly;
}

// measure overlap between bird's eye view bounding boxes, parametrized by (ry, l, w, tx, tz)
inline float64_t groundBoxOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1)
{
  Polygon gp = toPolygon(g);
  Polygon dp = toPolygon(d);

  std::vector<Polygon> in, un;
  boost::geometry::intersection(gp, dp, in);
  boost::geometry::union_(gp, dp, un);

  float64_t inter_area = in.empty() ? 0 : boost::geometry::area(in.front());
  float64_t union_area = boost::geometry::area(un.front());
  float64_t o = 0;
  if (criterion == -1) {  // union
    o = inter_area / union_area;
  } else if (criterion == 0) {  // bbox_a
    o = inter_area / boost::geometry::area(dp);
  } else if (criterion == 1) {  // bbox_b
    o = inter_area / boost::geometry::area(gp);
  }

  return o;
}

// measure overlap between 3D bounding boxes, parametrized by (ry, h, w, l, tx, ty, tz)
inline float64_t box3DOverlap(tDetection d, tGroundtruth g, int32_t criterion = -1)
{
  Polygon gp = toPolygon(g);
  Polygon dp = toPolygon(d);

  std::vector<Polygon> in, un;
  boost::geometry::intersection(gp, dp, in);
  boost::geometry::union_(gp, dp, un);

  float64_t ymax = std::min(d.t2, g.t2);
  float64_t ymin = std::max(d.t2 - d.h, g.t2 - g.h);

  float64_t inter_area = in.empty() ? 0 : boost::geometry::area(in.front());
  float64_t inter_vol = inter_area * std::max(0.0, ymax - ymin);

  float64_t det_vol = d.h * d.l * d.w;
  float64_t gt_vol = g.h * g.l * g.w;

  float64_t o = 0;
  if (criterion == -1) {  // union
    o = inter_vol / (det_vol + gt_vol - inter_vol);
  } else if (criterion == 0) {  // bbox_a
    o = inter_vol / det_vol;
  } else if (criterion == 1) {  // bbox_b
    o = inter_vol / gt_vol;
  }

  return o;
}

std::vector<float64_t> getThresholds(std::vector<float64_t> & v, float64_t n_groundtruth)
{
  // holds scores needed to compute N_SAMPLE_PTS recall values
  std::vector<float64_t> t;

  // sort scores in descending order
  // (highest score is assumed to give best/most confident detections)
  sort(v.begin(), v.end(), std::greater<float64_t>());

  // get scores for linearly spaced recall
  float64_t current_recall = 0;
  for (uint32_t i = 0; i < v.size(); i++) {
    // check if right-hand-side recall with respect to current recall is close than left-hand-side
    // one. In this case, skip the current detection score
    float64_t l_recall, r_recall /*, recall*/;
    l_recall = static_cast<float64_t>(i + 1) / n_groundtruth;
    if (i < (v.size() - 1)) {
      r_recall = static_cast<float64_t>(i + 2) / n_groundtruth;
    } else {
      r_recall = l_recall;
    }

    if ( (r_recall - current_recall) < (current_recall - l_recall) && i < (v.size() - 1)) {
      continue;
    }

    // left recall is the best approximation, so use this and goto next recall step for
    // approximation recall = l_recall;

    // the next recall step was reached
    t.push_back(v[i]);
    current_recall += 1.0 / (N_SAMPLE_PTS - 1.0);
  }
  return t;
}

void cleanData(
  CLASSES current_class, const std::vector<tGroundtruth> & gt,
  const std::vector<tDetection> & det, std::vector<int32_t> & ignored_gt,
  std::vector<tGroundtruth> & dc, std::vector<int32_t> & ignored_det, int32_t & n_gt,
  DIFFICULTY difficulty)
{
  // extract ground truth bounding boxes for current evaluation class
  for (uint32_t i = 0; i < gt.size(); i++) {
    // only bounding boxes with a minimum height are used for evaluation
    float64_t height = gt[i].box.y2 - gt[i].box.y1;

    // neighboring classes are ignored ("van" for "car" and "person_sitting" for "pedestrian")
    // (lower/upper cases are ignored)
    int32_t valid_class;
    const char8_t * class_name = CLASS_NAMES[static_cast<size_t>(current_class)].c_str();
    const char8_t * box_type = gt[i].box.type.c_str();

    // all classes without a neighboring class
    if (!strcasecmp(box_type, class_name)) {
      valid_class = 1;
    } else if (!strcasecmp(class_name, "Pedestrian") && !strcasecmp("Person_sitting", box_type)) {
      // classes with a neighboring class
      valid_class = 0;
    } else if (!strcasecmp(class_name, "Car") && !strcasecmp("Van", box_type)) {
      valid_class = 0;
    } else {
      // classes not used for evaluation
      valid_class = -1;
    }

    // ground truth is ignored, if occlusion, truncation exceeds the difficulty or ground truth is
    // too small (doesn't count as FN nor TP, although detections may be assigned)
    bool8_t ignore = false;
    if (gt[i].occlusion > MAX_OCCLUSION[static_cast<size_t>(difficulty)] ||
      gt[i].truncation > MAX_TRUNCATION[static_cast<size_t>(difficulty)] ||
      height <= MIN_HEIGHT[static_cast<size_t>(difficulty)])
    {
      ignore = true;
    }

    // set ignored vector for ground truth
    // current class and not ignored (total no. of ground truth is detected for recall denominator)
    if (valid_class == 1 && !ignore) {
      ignored_gt.push_back(0);
      n_gt++;
    } else if (valid_class == 0 || (ignore && valid_class == 1)) {
      // neighboring class, or current class but ignored
      ignored_gt.push_back(1);
    } else {
      // all other classes which are FN in the evaluation
      ignored_gt.push_back(-1);
    }
  }

  // extract dontcare areas
  for (uint32_t i = 0; i < gt.size(); i++) {
    if (!strcasecmp("DontCare", gt[i].box.type.c_str())) {
      dc.push_back(gt[i]);
    }
  }

  // extract detections bounding boxes of the current class
  for (uint32_t i = 0; i < det.size(); i++) {
    // neighboring classes are not evaluated
    int32_t valid_class;
    if (!strcasecmp(
        det[i].box.type.c_str(),
        CLASS_NAMES[static_cast<size_t>(current_class)].c_str()))
    {
      valid_class = 1;
    } else {
      valid_class = -1;
    }

    auto height = fabs(det[i].box.y1 - det[i].box.y2);

    // set ignored vector for detections
    if (height < MIN_HEIGHT[static_cast<size_t>(difficulty)]) {
      ignored_det.push_back(1);
    } else if (valid_class == 1) {
      ignored_det.push_back(0);
    } else {
      ignored_det.push_back(-1);
    }
  }
}

tPrData computeStatistics(
  CLASSES current_class, const std::vector<tGroundtruth> & gt,
  const std::vector<tDetection> & det, const std::vector<tGroundtruth> & dc,
  const std::vector<int32_t> & ignored_gt, const std::vector<int32_t> & ignored_det,
  bool8_t compute_fp, float64_t (* boxoverlap)(tDetection, tGroundtruth, int32_t),
  METRIC metric, bool8_t compute_aos = false, float64_t thresh = 0)
{
  tPrData stat = tPrData();
  const float64_t NO_DETECTION = -10000000;
  // holds angular difference for TPs (needed for AOS evaluation)
  std::vector<float64_t> delta;
  // holds wether a detection was assigned to a valid or ignored ground truth
  std::vector<bool8_t> assigned_detection;
  assigned_detection.assign(det.size(), false);
  std::vector<bool8_t> ignored_threshold;
  // holds detections with a threshold lower than thresh if FP are computed
  ignored_threshold.assign(det.size(), false);

  // detections with a low score are ignored for computing precision (needs FP)
  if (compute_fp) {
    for (uint32_t i = 0; i < det.size(); i++) {
      if (det[i].thresh < thresh) {
        ignored_threshold[i] = true;
      }
    }
  }

  // evaluate all ground truth boxes
  for (uint32_t i = 0; i < gt.size(); i++) {
    // this ground truth is not of the current or a neighboring class and therefore ignored
    if (ignored_gt[i] == -1) {
      continue;
    }

    /*=======================================================================
    find candidates (overlap with ground truth > 0.5) (logical len(det))
    =======================================================================*/
    uint32_t det_idx = 0;
    float64_t valid_detection = NO_DETECTION;
    float64_t max_overlap = 0;

    // search for a possible detection
    bool8_t assigned_ignored_det = false;
    for (uint32_t j = 0; j < det.size(); j++) {
      // detections not of the current class, already assigned or with a low threshold are ignored
      if (ignored_det[j] == -1) {
        continue;
      }
      if (assigned_detection[j]) {
        continue;
      }
      if (ignored_threshold[j]) {
        continue;
      }

      // find the maximum score for the candidates and get idx of respective detection
      float64_t overlap = boxoverlap(det[j], gt[i], -1);

      // for computing recall thresholds, the candidate with highest score is considered
      if (!compute_fp &&
        overlap > MIN_OVERLAP[static_cast<size_t>(metric)][static_cast<size_t>(current_class)] &&
        det[j].thresh > valid_detection)
      {
        det_idx = j;
        valid_detection = det[j].thresh;
      } else if (compute_fp &&  // NOLINT
        overlap > MIN_OVERLAP[static_cast<size_t>(metric)][static_cast<size_t>(current_class)] &&
        (overlap > max_overlap || assigned_ignored_det) && ignored_det[j] == 0)
      {
        // for computing pr curve values, the candidate with the greatest overlap is considered
        // if the greatest overlap is an ignored detection (min_height), the overlapping detection
        // is used
        max_overlap = overlap;
        det_idx = j;
        valid_detection = 1;
        assigned_ignored_det = false;
      } else if (compute_fp &&  // NOLINT
        overlap > MIN_OVERLAP[static_cast<size_t>(metric)][static_cast<size_t>(current_class)] &&
        valid_detection == NO_DETECTION && ignored_det[j] == 1)
      {
        det_idx = j;
        valid_detection = 1;
        assigned_ignored_det = true;
      }
    }

    /*=======================================================================
    compute TP, FP and FN
    =======================================================================*/

    // nothing was assigned to this valid ground truth
    if (valid_detection == NO_DETECTION && ignored_gt[i] == 0) {
      stat.fn++;
    } else if (valid_detection != NO_DETECTION &&  // NOLINT
      (ignored_gt[i] == 1 || ignored_det[det_idx] == 1))
    {
      // only evaluate valid ground truth <=> detection assignments (considering difficulty level)
      assigned_detection[det_idx] = true;
    } else if (valid_detection != NO_DETECTION) {
      // found a valid true positive
      // write highest score to threshold vector
      stat.tp++;
      stat.v.push_back(det[det_idx].thresh);

      // compute angular difference of detection and ground truth if valid detection orientation was
      // provided
      if (compute_aos) {
        delta.push_back(gt[i].box.alpha - det[det_idx].box.alpha);
      }

      // clean up
      assigned_detection[det_idx] = true;
    }
  }

  // if FP are requested, consider stuff area
  if (compute_fp) {
    // count fp
    for (uint32_t i = 0; i < det.size(); i++) {
      // count false positives if required (height smaller than required is ignored (ignored_det==1)
      if (!(assigned_detection[i] || ignored_det[i] == -1 || ignored_det[i] == 1 ||
        ignored_threshold[i]))
      {
        stat.fp++;
      }
    }

    // do not consider detections overlapping with stuff area
    int32_t nstuff = 0;
    for (uint32_t i = 0; i < dc.size(); i++) {
      for (uint32_t j = 0; j < det.size(); j++) {
        // detections not of the current class, already assigned, with a low threshold or a low
        // minimum height are ignored
        if (assigned_detection[j]) {
          continue;
        }
        if (ignored_det[j] == -1 || ignored_det[j] == 1) {
          continue;
        }
        if (ignored_threshold[j]) {
          continue;
        }

        // compute overlap and assign to stuff area, if overlap exceeds class specific value
        float64_t overlap = boxoverlap(det[j], dc[i], 0);
        auto metric_s = static_cast<size_t>(metric);
        auto current_class_s = static_cast<size_t>(current_class);
        if (overlap > MIN_OVERLAP[metric_s][current_class_s]) {
          assigned_detection[j] = true;
          nstuff++;
        }
      }
    }

    // FP = no. of all not to ground truth assigned detections - detections assigned to stuff areas
    stat.fp -= nstuff;

    // if all orientation values are valid, the AOS is computed
    if (compute_aos) {
      std::vector<float64_t> tmp;

      // FP have a similarity of 0, for all TP compute AOS
      tmp.assign(static_cast<size_t>(stat.fp), 0);
      for (uint32_t i = 0; i < delta.size(); i++) {
        tmp.push_back((1.0 + cos(delta[i])) / 2.0);
      }

      // be sure, that all orientation deltas are computed
      assert(tmp.size() == static_cast<size_t>(stat.fp + stat.tp));
      assert(delta.size() == static_cast<size_t>(stat.tp));

      // get the mean orientation similarity for this image
      if (stat.tp > 0 || stat.fp > 0) {
        stat.similarity = accumulate(tmp.begin(), tmp.end(), 0.0);
      } else {
        // there was neither a FP nor a TP, so the similarity is ignored in the evaluation
        stat.similarity = -1;
      }
    }
  }
  return stat;
}

/*=======================================================================
EVALUATE CLASS-WISE
=======================================================================*/

bool8_t eval_class(
  FILE * fp_det, FILE * fp_ori, CLASSES current_class,
  const std::vector<std::vector<tGroundtruth>> & groundtruth,
  const std::vector<std::vector<tDetection>> & detections, bool8_t compute_aos,
  float64_t (* boxoverlap)(tDetection, tGroundtruth, int32_t),
  std::vector<float64_t> & precision, std::vector<float64_t> & aos,
  DIFFICULTY difficulty, METRIC metric)
{
  assert(groundtruth.size() == detections.size());

  // init
  // total no. of gt (denominator of recall)
  int32_t n_gt = 0;
  // detection scores, evaluated for recall discretization
  std::vector<float64_t> v, thresholds;
  // index of ignored gt detection for current class/difficulty
  std::vector<std::vector<int32_t>> ignored_gt, ignored_det;
  // index of dontcare areas, included in ground truth
  std::vector<std::vector<tGroundtruth>> dontcare;

  // for all test images do
  for (uint32_t i = 0; i < groundtruth.size(); i++) {
    // holds ignored ground truth, ignored detections and dontcare areas for current frame
    std::vector<int32_t> i_gt, i_det;
    std::vector<tGroundtruth> dc;

    // only evaluate objects of current class and ignore occluded, truncated objects
    cleanData(current_class, groundtruth[i], detections[i], i_gt, dc, i_det, n_gt, difficulty);
    ignored_gt.push_back(i_gt);
    ignored_det.push_back(i_det);
    dontcare.push_back(dc);

    // compute statistics to get recall values
    tPrData pr_tmp = tPrData();
    pr_tmp = computeStatistics(
      current_class, groundtruth[i], detections[i], dc, i_gt, i_det, false,
      boxoverlap, metric);

    // add detection scores to vector over all images
    for (uint32_t j = 0; j < pr_tmp.v.size(); j++) {
      v.push_back(pr_tmp.v[j]);
    }
  }

  // get scores that must be evaluated for recall discretization
  thresholds = getThresholds(v, n_gt);

  // compute TP,FP,FN for relevant scores
  std::vector<tPrData> pr;
  pr.assign(thresholds.size(), tPrData());
  for (uint32_t i = 0; i < groundtruth.size(); i++) {
    // for all scores/recall thresholds do:
    for (uint32_t t = 0; t < thresholds.size(); t++) {
      tPrData tmp = tPrData();
      tmp = computeStatistics(
        current_class, groundtruth[i], detections[i], dontcare[i],
        ignored_gt[i], ignored_det[i], true, boxoverlap, metric,
        compute_aos, thresholds[t]);

      // add no. of TP, FP, FN, AOS for current frame to total evaluation for current threshold
      pr[t].tp += tmp.tp;
      pr[t].fp += tmp.fp;
      pr[t].fn += tmp.fn;
      if (tmp.similarity != -1) {
        pr[t].similarity += tmp.similarity;
      }
    }
  }

  // compute recall, precision and AOS
  std::vector<float64_t> recall;
  precision.assign(N_SAMPLE_PTS, 0);
  if (compute_aos) {
    aos.assign(N_SAMPLE_PTS, 0);
  }
  float64_t r = 0;
  for (uint32_t i = 0; i < thresholds.size(); i++) {
    r = pr[i].tp / static_cast<float64_t>(pr[i].tp + pr[i].fn);
    recall.push_back(r);
    precision[i] = pr[i].tp / static_cast<float64_t>(pr[i].tp + pr[i].fp);
    if (compute_aos) {
      aos[i] = pr[i].similarity / static_cast<float64_t>(pr[i].tp + pr[i].fp);
    }
  }

  // filter precision and AOS using max_{i..end}(precision)
  for (uint32_t i = 0; i < thresholds.size(); i++) {
    precision[i] = *max_element(precision.begin() + i, precision.end());
    if (compute_aos) {
      aos[i] = *max_element(aos.begin() + i, aos.end());
    }
  }

  // save statisics and finish with success
  saveStats(precision, aos, fp_det, fp_ori);
  return true;
}

void saveAndPlotPlots(
  std::basic_string<char8_t> dir_name, std::basic_string<char8_t> file_name,
  std::basic_string<char8_t> obj_type, std::vector<float64_t> vals[], bool8_t is_aos)
{
  // save plot data to file
  FILE * fp = fopen((dir_name + "/" + file_name + ".txt").c_str(), "w");
  printf("save %s\n", (dir_name + "/" + file_name + ".txt").c_str());
  for (size_t i = 0; i < static_cast<size_t>(N_SAMPLE_PTS); i++) {
    fprintf(
      fp, "%f %f %f %f\n", static_cast<float64_t>(i) / (N_SAMPLE_PTS - 1.0), vals[0][i], vals[1][i],
      vals[2][i]);
  }
  fclose(fp);

  // create png + eps
  for (int32_t j = 0; j < 2; j++) {
    std::basic_string<char8_t> type = "_png";

    if (j != 0) {
      type = "_pdf";
    }
    // open file
    FILE * fp = fopen((dir_name + "/" + file_name + type + ".gp").c_str(), "w");

    // save gnuplot instructions
    if (j == 0) {
      fprintf(fp, "set term png size 450,315 font \"Helvetica\" 11\n");
      fprintf(fp, "set output \"%s.png\"\n", file_name.c_str());
    } else {
      fprintf(fp, "set term postscript eps enhanced color font \"Helvetica\" 20\n");
      fprintf(fp, "set output \"%s.eps\"\n", file_name.c_str());
    }

    // set labels and ranges
    fprintf(fp, "set size ratio 0.7\n");
    fprintf(fp, "set xrange [0:1]\n");
    fprintf(fp, "set yrange [0:1]\n");
    fprintf(fp, "set xlabel \"Recall\"\n");
    if (!is_aos) {fprintf(fp, "set ylabel \"Precision\"\n");} else {
      fprintf(fp, "set ylabel \"Orientation Similarity\"\n");
    }
    obj_type[0] = static_cast<char8_t>(toupper(obj_type[0]));
    fprintf(fp, "set title \"%s\"\n", obj_type.c_str());

    // line width
    int32_t lw = 5;
    if (j == 0) {lw = 3;}

    // plot error curve
    fprintf(fp, "plot ");
    fprintf(fp, "\"%s.txt\" using 1:2 title 'Easy' with lines ls 1 lw %d,", file_name.c_str(), lw);
    fprintf(
      fp, "\"%s.txt\" using 1:3 title 'Moderate' with lines ls 2 lw %d,",
      file_name.c_str(), lw);
    fprintf(fp, "\"%s.txt\" using 1:4 title 'Hard' with lines ls 3 lw %d", file_name.c_str(), lw);

    // close file
    fclose(fp);
  }
}

namespace kittisdk
{

extern "C"
int32_t eval(
  std::basic_string<char8_t> ground_truth_path, std::basic_string<char8_t> detection_path,
  std::basic_string<char8_t> output_path, bool8_t eval_2d_res, bool8_t eval_ground_red,
  bool8_t eval_3d_res, bool8_t print_stdout, bool8_t create_plot, int32_t n_testimages)
{
  // set some global parameters
  initGlobals();

  // ground truth and result directories
  std::basic_string<char8_t> result_dir = output_path;
  std::basic_string<char8_t> plot_dir = result_dir + "/plot";

  // Check folder existance
  if (!ghc::filesystem::is_directory(ghc::filesystem::path(ground_truth_path)) ||
    !ghc::filesystem::is_directory(ghc::filesystem::path(detection_path)) ||
    !ghc::filesystem::is_directory(ghc::filesystem::path(result_dir)) )
  {
    if (print_stdout) {
      printf("Folder structure error, please check the existance of: \n");
      printf("%s\n", ground_truth_path.c_str());
      printf("%s\n", detection_path.c_str());
      printf("%s\n", result_dir.c_str());
    }
    return 0;
  }

  if (create_plot && !ghc::filesystem::is_directory(ghc::filesystem::path(plot_dir))) {
    printf("Folder structure error, please check the existance of: \n");
    printf("%s\n", plot_dir.c_str());
  }

  // hold detections and ground truth in memory
  std::vector<std::vector<tGroundtruth>> groundtruth;
  std::vector<std::vector<tDetection>> detections;

  // holds wether orientation similarity shall be computed (might be set to false while loading
  // detections) and which labels where provided by this submission
  bool8_t compute_aos = true;
  std::vector<bool8_t> eval_image(NUM_CLASS, false);
  std::vector<bool8_t> eval_ground(NUM_CLASS, false);
  std::vector<bool8_t> eval_3d(NUM_CLASS, false);

  // for all images read groundtruth and detections
  if (print_stdout) {
    printf("Loading detections...\n");
  }

  for (int32_t i = 0; i < n_testimages; i++) {
    // file name
    char8_t file_name[256];
    snprintf(file_name, sizeof(file_name), "%06d.txt", i);

    // read ground truth and result poses
    bool8_t gt_success, det_success;
    std::vector<tGroundtruth> gt = loadGroundtruth(ground_truth_path + "/" + file_name, gt_success);
    std::vector<tDetection> det = loadDetections(
      detection_path + "/" + file_name,
      compute_aos, eval_image, eval_ground, eval_3d, det_success);
    groundtruth.push_back(gt);
    detections.push_back(det);

    // check for errors
    if (!gt_success) {
      if (print_stdout) {
        printf("ERROR: Couldn't read: %s of ground truth.\n", file_name);
      }
      return 0;
    }
    if (!det_success) {
      if (print_stdout) {
        printf("ERROR: Couldn't read: %s\n", file_name);
      }
      return 0;
    }
  }
  if (print_stdout) {
    printf("  done.\n");
  }

  // holds pointers for result files
  FILE * fp_det = 0, * fp_ori = 0;

  if (eval_2d_res) {
    // eval image 2D bounding boxes
    for (size_t c = 0; c < static_cast<size_t>(NUM_CLASS); c++) {
      CLASSES cls = (CLASSES)c;

      if (eval_image[c]) {
        if (print_stdout) {
          printf("Starting 2D evaluation (%s) ...\n", CLASS_NAMES[c].c_str());
        }
        fp_det = fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection.txt").c_str(), "w");
        if (compute_aos) {
          fp_ori =
            fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_orientation.txt").c_str(), "w");
        }
        std::vector<float64_t> precision[3], aos[3];
        if (!eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, imageBoxOverlap,
            precision[0], aos[0], DIFFICULTY::EASY, METRIC::IMAGE) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, imageBoxOverlap,
            precision[1], aos[1], DIFFICULTY::MODERATE, METRIC::IMAGE) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, imageBoxOverlap,
            precision[2], aos[2], DIFFICULTY::HARD, METRIC::IMAGE))
        {
          if (print_stdout) {
            printf("%s evaluation failed.\n", CLASS_NAMES[c].c_str());
          }
          return 0;
        }
        fclose(fp_det);
        if (create_plot) {
          saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_detection", CLASS_NAMES[c], precision, 0);
        }
        if (compute_aos) {
          if (create_plot) {
            saveAndPlotPlots(plot_dir, CLASS_NAMES[c] + "_orientation", CLASS_NAMES[c], aos, 1);
          }
          fclose(fp_ori);
        }
        if (print_stdout) {
          printf("  done.\n");
        }
      }
    }
  }

  // don't evaluate AOS for birdview boxes and 3D boxes
  compute_aos = false;

  if (eval_ground_red) {
    // eval bird's eye view bounding boxes
    for (size_t c = 0; c < static_cast<size_t>(NUM_CLASS); c++) {
      CLASSES cls = (CLASSES)c;

      if (eval_ground[c]) {
        if (print_stdout) {
          printf("Starting bird's eye evaluation (%s) ...\n", CLASS_NAMES[c].c_str());
        }
        fp_det = fopen(
          (result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_ground.txt").c_str(), "w");
        std::vector<float64_t> precision[3], aos[3];
        if (!eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, groundBoxOverlap,
            precision[0], aos[0], DIFFICULTY::EASY, METRIC::GROUND) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, groundBoxOverlap,
            precision[1], aos[1], DIFFICULTY::MODERATE, METRIC::GROUND) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, groundBoxOverlap,
            precision[2], aos[2], DIFFICULTY::HARD, METRIC::GROUND))
        {
          if (print_stdout) {
            printf("%s evaluation failed.\n", CLASS_NAMES[c].c_str());
          }
          return 0;
        }
        fclose(fp_det);
        if (create_plot) {
          saveAndPlotPlots(
            plot_dir, CLASS_NAMES[c] + "_detection_ground", CLASS_NAMES[c],
            precision, 0);
        }
        if (print_stdout) {
          printf("  done.\n");
        }
      }
    }
  }

  if (eval_3d_res) {
    // eval 3D bounding boxes
    for (size_t c = 0; c < static_cast<size_t>(NUM_CLASS); c++) {
      CLASSES cls = (CLASSES)c;

      if (eval_3d[c]) {
        if (print_stdout) {
          printf("Starting 3D evaluation (%s) ...\n", CLASS_NAMES[c].c_str());
        }
        fp_det =
          fopen((result_dir + "/stats_" + CLASS_NAMES[c] + "_detection_3d.txt").c_str(), "w");
        std::vector<float64_t> precision[3], aos[3];
        if (!eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, box3DOverlap,
            precision[0], aos[0], DIFFICULTY::EASY, METRIC::BOX3D) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, box3DOverlap,
            precision[1], aos[1], DIFFICULTY::MODERATE, METRIC::BOX3D) ||
          !eval_class(
            fp_det, fp_ori, cls, groundtruth, detections, compute_aos, box3DOverlap,
            precision[2], aos[2], DIFFICULTY::HARD, METRIC::BOX3D))
        {
          if (print_stdout) {
            printf("%s evaluation failed.\n", CLASS_NAMES[c].c_str());
          }
          return 0;
        }
        fclose(fp_det);
        if (create_plot) {
          saveAndPlotPlots(
            plot_dir, CLASS_NAMES[c] + "_detection_3d", CLASS_NAMES[c], precision,
            0);
        }
        if (print_stdout) {
          printf("  done.\n");
        }
      }
    }
  }

  // success
  return 1;
}

}  // namespace kittisdk


#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <boost/foreach.hpp>
#include <map>
#include "enigma.h"
#include "main.h"
#include "key.h"
#include "net.h"
#include "init.h"
#include "util.h"
#include "wallet.h"
#include "ui_interface.h"

#include <openssl/ecdsa.h>
#include <openssl/obj_mac.h>
#include <openssl/rand.h>
#include <openssl/ecdh.h>
#include <openssl/hmac.h>
#include <openssl/aes.h>
#include "encryption.h"

extern bool lightCloakShieldIcon ; // should we light the cloak shield icon on the main gui status area?

void CCloakingSecret::SetNull()
{
    memset(&chKey, 0, sizeof chKey);
    memset(&chIV, 0, sizeof chIV);
    userPubKeyForHash = "";
    userPubKeyHashHex = "";
}

const std::string CCloakingSecret::GetKeyHash()
{
    // need to regen hash ?
    if (userPubKeyForHash.compare(userPubKey) != 0){
        userPubKeyHashHex = Hash160(vchFromString(userPubKey)).GetHex();
    }
    return userPubKeyHashHex;
}

// CCloakingEncryptionKey is our per-session pub/priv keypair for ECDH communication with peers
CCloakingEncryptionKey::CCloakingEncryptionKey() : CKey()
{
    try
    {
        MakeNewKey(true);
        pubkeyHex = HexStr(this->GetPubKey().Raw());
        pubkeyHexHash = Hash160(vchFromString(pubkeyHex)).GetHex();

    }catch (std::exception& e){
        error("CCloakingEncryptionKey::CCloakingEncryptionKey: %s.", e.what());
    }
}

// create (or return existing) hex encoded ECDH secret for a public key (using our current Enigma priv-key)
bool CCloakingEncryptionKey::GetSecret(std::string targetPubKey, CCloakingSecret& cloakSecretOut, bool addToMap)
{
    try
    {
        LOCK(pwalletMain->cs_CloakingSecrets);
        CCloakShield* cs = CCloakShield::GetShield();

        // return existing?
        if (cs->mapCloakSecrets.count(targetPubKey)){
            cloakSecretOut = cs->mapCloakSecrets[targetPubKey];
            return true;
        }

        EC_KEY *peerkey;
        int field_size;
        unsigned char *secret;
        size_t secret_len;

        std::vector<unsigned char> vchPubKey = ParseHex(targetPubKey.c_str());
        const unsigned char* pbegin = &vchPubKey[0];

        peerkey = EC_KEY_new_by_curve_name(NID_secp256k1);
        if (o2i_ECPublicKey(&peerkey, &pbegin, vchPubKey.size()))
        {
            // In testing, d2i_ECPrivateKey can return true
            // but fill in pkey with a key that fails
            // EC_KEY_check_key, so:
            if (!EC_KEY_check_key(peerkey))
            {
                printf("CreateSecretECDH: EC_KEY_check_key failed");
                return false;
            }
        }

        // Calculate the size of the buffer for the shared secret
        field_size = EC_GROUP_get_degree(EC_KEY_get0_group(this->pkey));
        secret_len = (field_size+7)/8;

        // Allocate the memory for the shared secret
        if(NULL == (secret = (unsigned char*)OPENSSL_malloc(secret_len)))
        {
            printf("CreateSecretECDH: OPENSSL_malloc failed");
        }

        // Derive the shared secret
        secret_len = ECDH_compute_key(secret, secret_len, EC_KEY_get0_public_key(peerkey),
                            this->pkey, NULL);

        // Clean up
        EC_KEY_free(peerkey);

        if(secret_len <= 0)
        {
            if(GetBoolArg("-printenigma", false))
                OutputDebugStringF("CreateSecretECDH: secret_len <= 0");
            OPENSSL_free(secret);
            return NULL;
        }

        vector<unsigned char> vchSecret;
        vchSecret.insert( vchSecret.end(), secret, secret+(secret_len));

        vector<unsigned char> vchSecretHash;
        uint256 secretHash = Hash(vchSecret.begin(), vchSecret.end());
        vchSecretHash.insert( vchSecretHash.end(), secretHash.begin(), secretHash.end());

        // create the key and IV that we will use to encrypt the shared Enigma encryption key for this user.
        EVP_BytesToKey(EVP_aes_256_cbc(), EVP_sha512(), NULL,
                          (unsigned char *)&vchSecretHash[0], vchSecretHash.size(), 25000, &cloakSecretOut.chKey[0], &cloakSecretOut.chIV[0]);

        cloakSecretOut.secretHex = HexStr(vchSecretHash.begin(), vchSecretHash.end());

        cloakSecretOut.keyHex = HexStr(&cloakSecretOut.chKey[0], &cloakSecretOut.chKey[32]);
        cloakSecretOut.ivHex = HexStr(&cloakSecretOut.chIV[0], &cloakSecretOut.chIV[16]);

        cloakSecretOut.userPubKey = targetPubKey;

        if (addToMap)
            cs->mapCloakSecrets[targetPubKey] = cloakSecretOut;

        return true;

    }catch (std::exception& e){
        error("CCloakingEncryptionKey::GetSecretECDH: %s.", e.what());
        return false;
    }
}


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Copyright (c) 2009-2013 Illumina, Inc.
//
// This software is provided under the terms and conditions of the
// Illumina Open Source Software License 1.
//
// You should have received a copy of the Illumina Open Source
// Software License 1 along with this program. If not, see
// <https://github.com/sequencing/licenses/>
//

/// \file

/// \author Chris Saunders
///
#ifndef __POSITION_SNP_CALL_PPROB_DIGT_HH
#define __POSITION_SNP_CALL_PPROB_DIGT_HH

#include "blt_common/blt_shared.hh"
#include "blt_common/snp_pos_info.hh"

#include "blt_util/digt.hh"
#include "blt_util/qscore.hh"

#include <boost/array.hpp>
#include <boost/utility.hpp>

#include <iosfwd>

struct diploid_genotype {

    diploid_genotype() { reset(); }

    void reset() {
        is_snp=false;
        ref_gt=0;
        genome.reset();
        poly.reset();
    }

    struct result_set {

        result_set() { reset(); }

        void
        reset() {
            max_gt=0;
            static const blt_float_t p(1./static_cast<blt_float_t>(DIGT::SIZE));
            static const int qp(error_prob_to_qphred((1.-p)));
            snp_qphred=qp;
            max_gt_qphred=qp;
            for (unsigned i(0); i<DIGT::SIZE; ++i) {
                pprob[i] = p;
            }
        }

        unsigned max_gt;
        int snp_qphred;
        int max_gt_qphred;
        boost::array<double,DIGT::SIZE> pprob; // note this is intentionally stored at higher float resolution than the rest of the computation
    };

    bool is_snp;
    unsigned ref_gt;
    result_set genome;
    result_set poly;
    double sb;
};

// debuging output -- produces labels
//
std::ostream& operator<<(std::ostream& os,const diploid_genotype& dgt);

// more debuging output:
void
debug_dump_digt_lhood(const blt_float_t* lhood,
                      std::ostream& os);

// allele call output:
//
void
write_diploid_genotype_allele(const blt_options& opt,
                              const snp_pos_info& pi,
                              const diploid_genotype& dgt,
                              std::ostream& os,
                              const unsigned hpol);

// snp call output:
//
inline
void
write_diploid_genotype_snp(const blt_options& opt,
                           const snp_pos_info& pi,
                           const diploid_genotype& dgt,
                           std::ostream& os,
                           const unsigned hpol) {

    write_diploid_genotype_allele(opt,pi,dgt,os,hpol);
}

// Use caller object to precalculate prior distributions based on
// theta value:
//
struct pprob_digt_caller : private boost::noncopyable {

    pprob_digt_caller(const blt_float_t theta);

    /// \brief call a snp @ pos by calculating the posterior probability
    /// of all possible genotypes for a diploid individual.
    ///
    /// When is_always_test is true, probabilities are calculated even
    /// when a snp could not exist at the site.
    ///
    void
    position_snp_call_pprob_digt(const blt_options& opt,
                                 const extended_pos_info& epi,
                                 diploid_genotype& dgt,
                                 const bool is_always_test = false) const;

    const blt_float_t*
    lnprior_genomic(const unsigned ref_id) const {
        return _lnprior[ref_id].genome;
    }

    const blt_float_t*
    lnprior_polymorphic(const unsigned ref_id) const {
        return _lnprior[ref_id].poly;
    }

    static
    void
    get_diploid_gt_lhood(const blt_options& opt,
                         const extended_pos_info& epi,
                         const bool is_het_bias,
                         const blt_float_t het_bias,
                         blt_float_t* const lhood,
                         const bool is_strand_specific = false,
                         const bool is_ss_fwd = false);

    static
    void
    calculate_result_set(const blt_float_t* lhood,
                         const blt_float_t* lnprior,
                         const unsigned ref_gt,
                         diploid_genotype::result_set& rs);

private:
    struct prior_set {
        blt_float_t genome[DIGT::SIZE];
        blt_float_t poly[DIGT::SIZE];
    };

    prior_set _lnprior[N_BASE+1];
};

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/rsr/rsr.h>
#include <scitbx/array_family/boost_python/shared_wrapper.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>

namespace mmtbx { namespace rsr {
namespace {

  void init_module()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef return_value_policy<return_by_value> rbv;
    class_<manager<> >("manager",
      init<int const&,
           int const&,
           int const&,
           cctbx::xray::scattering_type_registry const&,
           cctbx::uctbx::unit_cell const&,
           af::const_ref<cctbx::xray::scatterer<> > const&,
           optional<double const&,
                    double const&> >((
                                  arg("nx"),
                                  arg("ny"),
                                  arg("nz"),
                                  arg("scattering_type_registry"),
                                  arg("unit_cell"),
                                  arg("scatterers"),
                                  arg("exp_table_one_over_step_size")=-100,
                                  arg("wing_cutoff")=1.e-3)))
      .add_property("density_array", make_getter(&manager<>::density_array, rbv()))
    ;

    class_<manager_BCR<> >("manager_BCR",
      init<int const&,
           int const&,
           int const&,
           cctbx::xray::scattering_type_registry const&,
           cctbx::uctbx::unit_cell const&,
           boost::python::list const&, // BCRscatterers
           optional<double const&,
                    double const&> >((
                                  arg("nx"),
                                  arg("ny"),
                                  arg("nz"),
                                  arg("scattering_type_registry"),
                                  arg("unit_cell"),
                                  arg("BCRscatterers"),
                                  arg("exp_table_one_over_step_size")=-100,
                                  arg("wing_cutoff")=1.e-3)))
      .add_property("density_array", make_getter(&manager_BCR<>::density_array, rbv()))
    ;

  }

} // namespace <anonymous>
}} // namespace mmtbx::rsr

BOOST_PYTHON_MODULE(mmtbx_rsr_ext)
{
  mmtbx::rsr::init_module();
}


#include "../include/Dictionary.h"
#include "../include/kbhit.h"

#include <fstream>
#include <omp.h>

#include "../include/exceptions.h"
#include "../include/Words.h"
#include "../include/Timer.h"
#include "../include/utility.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/iostreams/filtering_streambuf.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/filter/zlib.hpp>

using namespace std;
using namespace boost;

typedef vector< wstring > splitVec;

string Dictionary::ending = "dictionary";

Dictionary::Dictionary(Corpus* co) : Model<Dictionary>(co)
{
    systemMessage("Creating dictionary");

    // create dictionary parts t and c: f -> e
    size2 = co->getEwords()->size();
    size1 = co->getFwords()->size();
    int esize = size2;
    int fsize = size1;
    model = new float*[fsize];
    float** c;
    c = new float*[fsize];
    for (int i = 0; i < fsize; i++)
    {
        model[i] = new float[esize];
        c[i] = new float[esize];
        for (int j = 0; j < esize; j++)
        {
            c[i][j] = SMOOTH;
            model[i][j] = 0.0005;
        }
    }

    int loops = 0;
    int** fit;
    int** eit;
    vector<wstring>::iterator ewit;
    vector<wstring>::iterator fwit;
    splitVec ew;
    splitVec fw;
    vector<int*>* flines = co->getFlines();
    vector<int*>* elines = co->getElines();
    float normalize[esize];
    float norm = SMOOTH*fsize;
    while (!kbhit())
    {
        for(int i = 0; i<esize; i++)
        {
            normalize[i] = norm;
        }
        loops++;
        stringstream s;
        s << loops << ". loop";
        systemMessage(s.str().c_str());

#pragma omp parallel for
        for(unsigned int x=0; x<elines->size(); x++)
        {
            fit = &(flines->at(x));
            eit = &(elines->at(x));
            for(int i=1; i<(*fit)[0]; i++)
            {
                float s = 0.0;
                float* tpointer = model[(*fit)[i]];
                float* cpointer = c[(*fit)[i]];
                for(int j=1; j<(*eit)[0]; j++)
                {
                    s += tpointer[(*eit)[j]];
                }

                for(int j=1; j<(*eit)[0]; j++)
                {
                    float tmp = tpointer[(*eit)[j]]/s;
                    cpointer[(*eit)[j]] += tmp;
                    normalize[(*eit)[j]] += tmp;
                }
            }
        }
#pragma omp parallel for
        for(int i = 0; i<fsize; i++)
        {
            for(int j = 0; j<esize; j++)
            {
                model[i][j] = c[i][j]/normalize[j];
                c[i][j] = SMOOTH;
            }

        }
    }
    for (int j = 0; j < fsize ; j++)
    {
        delete [] c[j];
    }
    delete [] c;
    saveModel();
}

map<float, int>* Dictionary::getNBestTranslations(int word, unsigned int n)
{
    float last = 0.0;
    map<float, int>* tr = new map<float,int>();
    for(int i=0; i<size1; i++)
    {
        float p = lookup(i,word);
        if(tr->size()<n)
        {
            tr->insert(pair<float,int>(p, i));
            if(tr->size()==n)
                last = tr->begin()->first;
        }
        else if(p > last)
        {
            tr->insert(pair<float,int>(p, i));
            if(tr->size()>n)
                tr->erase(last);
            last = tr->begin()->first;
        }
    }
    return tr;
}

int Dictionary::getBestTranslation(int f)
{
    float last = 0.0;
    int tr = 0;
    for(int i=0; i<size2; i++)
    {
        float p = lookup(f,i);
        if(p > last)
        {
            tr=i;
            last = p;
        }
    }
    return tr;
}