#ifndef DATE_TIME_GREGORIAN_CALENDAR_HPP__
#define DATE_TIME_GREGORIAN_CALENDAR_HPP__

/* Copyright (c) 2002,2003 CrystalClear Software, Inc.
 * Use, modification and distribution is subject to the
 * Boost Software License, Version 1.0. (See accompanying
 * file LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)
 * Author: Jeff Garland
 * $Date$
 */

#include <boost/date_time/compiler_config.hpp>

namespace boost
{
namespace date_time
{

//! An implementation of the Gregorian calendar
/*! This is a parameterized implementation of a proleptic Gregorian Calendar
  that can be used in the creation of date systems or just to perform
  calculations. All the methods of this class are static functions, so the
  intent is to never create instances of this class.
  @param ymd_type_ Struct type representing the year, month, day.  The ymd_type
  must define a of types for the year, month, and day.  These types need to be
         arithmetic types.
  @param date_int_type_ Underlying type for the date count.  Must be an
  arithmetic type.
*/
template <typename ymd_type_, typename date_int_type_>
class BOOST_SYMBOL_VISIBLE gregorian_calendar_base
{
  public:
    //! define a type a date split into components
    typedef ymd_type_ ymd_type;
    //! define a type for representing months
    typedef typename ymd_type::month_type month_type;
    //! define a type for representing days
    typedef typename ymd_type::day_type day_type;
    //! Type to hold a stand alone year value (eg: 2002)
    typedef typename ymd_type::year_type year_type;
    //! Define the integer type to use for internal calculations
    typedef date_int_type_ date_int_type;

    static unsigned short day_of_week(const ymd_type& ymd);
    static int week_number(const ymd_type& ymd);
    // static unsigned short day_of_year(date_int_type);
    static date_int_type day_number(const ymd_type& ymd);
    static date_int_type julian_day_number(const ymd_type& ymd);
    static date_int_type modjulian_day_number(const ymd_type& ymd);
    static ymd_type from_day_number(date_int_type);
    static ymd_type from_julian_day_number(date_int_type);
    static ymd_type from_modjulian_day_number(date_int_type);
    static bool is_leap_year(year_type);
    static unsigned short end_of_month_day(year_type y, month_type m);
    static ymd_type epoch();
    static unsigned short days_in_week();
};

} // namespace date_time
} // namespace boost

#ifndef NO_BOOST_DATE_TIME_INLINE
#include "boost/date_time/gregorian_calendar.ipp"
#endif

#endif


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

#include <boost/filesystem.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ir.hpp>
#include <popart/names.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

#include <chrono>
#include <complex>
#include <iostream>
#include <random>

using namespace popart;

BOOST_AUTO_TEST_CASE(Inplace_numericsIpNip0) {

  // A 2x2 Tensor of zeros, denoted as
  //
  // . .
  // . .
  //
  // gets padded on different sides, then the padded
  // Tensors are added together:
  //
  // The padding constants are 1, 2, 4, 8:
  //
  //  . . 1       4 4 4       2 . .       8 8 8
  //  . . 1       . . 4       2 . .       8 . .
  //  1 1 1       . . 4       2 2 2       8 . .
  //    \          /             \          /.
  //      \      /                 \      /.
  //        \  /                     \  /.
  //        add                      add
  //            .                 .
  //                 .       .
  //                    add
  //
  //
  //
  // Expected output :
  //
  // 14 10 11
  // 12 .  3
  // 13 5  7
  //
  // Tested with inplace on, inplacing off, and pad inplace taking higher or
  // lower priority than usual.
  //
  auto test = [](bool withInplace, bool padInplaceHighPriority) {
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();

    std::vector<int64_t> inShape{{2, 2}};
    auto nInElms = 2 * 2;
    TensorInfo inInfo("FLOAT", inShape);

    std::vector<int64_t> outShape{{3, 3}};
    assert(outShape.size() == 2);
    auto nOutElms = 3 * 3;
    TensorInfo outInfo("FLOAT", outShape);

    auto inId = builder->addInputTensor(inInfo);

    // add minimal padding in each dimension, in all 4 possible directions
    auto pad0 = aiOnnx.pad({inId}, {1, 1, 0, 0}, "constant", 8.f);
    auto pad1 = aiOnnx.pad({inId}, {1, 0, 0, 1}, "constant", 2.f);
    auto pad2 = aiOnnx.pad({inId}, {0, 1, 1, 0}, "constant", 4.f);
    auto pad3 = aiOnnx.pad({inId}, {0, 0, 1, 1}, "constant", 1.f);

    float padPriority = padInplaceHighPriority ? 1000.f : 0.1;
    for (auto padId : {pad0, pad1, pad2, pad3}) {
      builder->setInplacePreferences(padId, {{"PadInplace", padPriority}});
    }

    auto add0   = aiOnnx.add({pad0, pad1});
    auto add1   = aiOnnx.add({pad2, pad3});
    auto summed = aiOnnx.add({add0, add1});

    builder->addOutputTensor(summed);

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);

    // Create the IR, adding outId as an anchor
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{summed, art}});

    auto opts   = SessionOptions();
    auto device = popart::createTestDevice(TEST_TARGET);

    auto session = popart::InferenceSession::createFromOnnxModel(
        proto,
        dataFlow,
        device,
        popart::InputShapeInfo(),
        opts,
        popart::Patterns(PatternsLevel::Minimal).enableInPlace(withInplace));

    // prepare the anchors
    std::vector<float> outputData(nOutElms, -777.);
    auto outputDataPtr = outputData.data();
    popart::NDArrayWrapper<float> outData(outputDataPtr, outShape);

    std::map<popart::TensorId, popart::IArray &> anchors = {
        {summed, outData},
    };

    session->prepareDevice();

    std::vector<float> vInData(nInElms, 0.0f);

    popart::NDArrayWrapper<float> inData(vInData.data(), inShape);
    std::map<popart::TensorId, popart::IArray &> inputs = {{inId, inData}};

    popart::StepIO stepio(inputs, anchors);

    session->run(stepio);

    std::vector<float> expected{14, 10, 11, 12, 0, 3, 13, 5, 7};

    for (uint64_t i = 0; i < nOutElms; ++i) {
      if (outputData[i] != expected[i]) {
        std::ostringstream oss;
        oss << "Failure in pad_outplace_0 test. "
            << "At index i = " << i
            << ", the expected value of the jitter-pad-add output is "
            << expected[i] << ", but the observed value was " << outputData[i]
            << '.' << " In this test, inplace = " << withInplace
            << ", and padInplaceHighPriority = " << padInplaceHighPriority;
        throw error(oss.str());
      }
    }
  };

  //  (inplace, pad-high-priority)
  test(true, true);
  test(true, false);
  test(false, true);

  // Note that the Ir does not take into account that Pad outputs cannot be
  // modified, as they contain constants. This is opaque to the Ir, and is
  // resolved at the last moment when the codelet is generated for the IPU,
  // where the Op calls poplar::Tensor::isModifiable().
  //
  // In the future, we will hopefully have more accurate information available
  // in the Ir w.r.t. constants and self-aliasing, and we will be able to
  // resolve these cases earlier and thereby have more effective inplacing.
}


// Listen to features, images and registeration triggers
// - when features received, find corresponding image from buffer of images and pair
// - when trigger is received, these two become the reference
// - for subsequent feature message, match the features and image with the reference and publish.

/*
The head is moving around - swaying over and back
- a trigger requests a sweep of scans @ utimeA - the last 100 scans, you bundle them off to the user.
- the user fits affordances to determine the transform from head at utimeA and afforance (utimeA->affordance)

Thereafter I register the head back to the image at utimeA (i.e. determine a current-to-previous transform).
- This is by aligning directly back to the frame at utimeA - not VO
- when the affordances are fitted and returned they are projected onto this pose (at utimeB)
- the transform to an affordance is: utimeB->utimeA->affordance

If at any time an alignment to utimeA is not possible, we do VO until it is.
- in that time, utimeC, the transform to an affordance then:
- utimeC -> utimeB->utimeA->affordance
- where utimeC->utimeB is openloop VO
*/

#include <iostream>
#include <Eigen/Dense>
#include <deque>

#include <boost/shared_ptr.hpp>
#include <boost/assign/std/vector.hpp>

#include <dirent.h>

#include "registeration.hpp"

#include <pronto_utils/pronto_vis.hpp> // visualize pt clds

#include <lcm/lcm-cpp.hpp>
#include <bot_frames/bot_frames.h>
#include <bot_param/param_client.h>

#include "lcmtypes/drc/map_params_t.hpp"
#include <lcmtypes/bot_core.hpp>
#include <lcmtypes/motion_estimate.hpp>

#define DO_TIMING_PROFILE TRUE
#include <ConciseArgs>

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

class RegApp{
  public:
    RegApp(boost::shared_ptr<lcm::LCM> &publish_lcm, std::string camera_);

    ~RegApp(){
    }

    void doReg();

  private:
    boost::shared_ptr<lcm::LCM> lcm_;
    void featuresCurrentHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  reg::features_t* msg);
    void featuresReferenceHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  reg::features_t* msg);
    void imageHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::image_t* msg);
    void registerCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::map_params_t* msg);
    void lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg);

    std::vector<ImageFeature> getFeaturesFromLCM(const  reg::features_t* msg, Eigen::Isometry3d &pose);
    std::string camera_;

    pronto_vis* pc_vis_;

    Reg::Ptr reg;
    std::vector<ImageFeature> cur_features_;
    bot_core::image_t cur_image_;
    Eigen::Isometry3d cur_pose_;

    std::vector<ImageFeature> ref_features_;
    bot_core::image_t ref_image_;
    Eigen::Isometry3d ref_pose_;

    int registeration_mode_ ;
    // mode: 0 listening for trigger, 1 listening for refer feats, 2 listening for current features

    BotParam* botparam_;
    BotFrames* botframes_;

    deque< bot_core::image_t  > * image_queue_;
    deque< bot_core::planar_lidar_t  > * laser_queue_;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr accum_cloud;
};

RegApp::RegApp(boost::shared_ptr<lcm::LCM> &lcm_, std::string camera_):
    lcm_(lcm_),camera_(camera_), registeration_mode_(0){

  botparam_ = bot_param_new_from_server( lcm_->getUnderlyingLCM() , 0);
  botframes_= bot_frames_get_global( lcm_->getUnderlyingLCM() , botparam_);

  lcm_->subscribe("FEATURES_REF",&RegApp::featuresReferenceHandler,this);
  lcm_->subscribe("FEATURES_CUR",&RegApp::featuresCurrentHandler,this);
  lcm_->subscribe(camera_ ,&RegApp::imageHandler,this);
  lcm_->subscribe("ROTATING_SCAN",&RegApp::lidarHandler,this);

  // In progress:
  lcm_->subscribe("MAP_CREATE",&RegApp::registerCommandHandler,this);

  reg = Reg::Ptr (new Reg (lcm_));

  image_queue_ = new deque< bot_core::image_t > ();
  laser_queue_ = new deque< bot_core::planar_lidar_t > ();

  // Vis Config:
  pc_vis_ = new pronto_vis( lcm_->getUnderlyingLCM() );
  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(70000,"Reg Pose",4,1) );

  // Cloud attached to Reg Pose
  float colors_0f[] ={1.0,0.0,0.0};
  vector <float> colors_0;
  colors_0.assign(colors_0f,colors_0f+4*sizeof(float));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(70001,"Null Pose",5,1) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(70002,"Reg Pt Cloud"     ,1,1, 70001,1,colors_0));
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(70003,"Reg Pt Cloud - Sensor"     ,1,1, 70000,1,colors_0));

  float colors_1f[] ={0.6,0.0,0.6};
  vector <float> colors_1;
  colors_1.assign(colors_1f,colors_1f+4*sizeof(float));
  pc_vis_->obj_cfg_list.push_back( obj_cfg(70100,"Revised Pose",4,1) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(70101,"Reg Pt Cloud - Sensor Revised"     ,1,1, 70100,0,colors_1));

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr accum_cloud_clf (new pcl::PointCloud<pcl::PointXYZRGB> ());
  accum_cloud = accum_cloud_clf;
}

// Classless method to convert queue to cloud on demand:
void deque_to_cloud(deque<bot_core::planar_lidar_t > * laser_queue,
                               BotFrames* botframes,
                               pcl::PointCloud<pcl::PointXYZRGB>::Ptr &accum_cloud){
  cout <<"deque of size " << laser_queue->size() <<"to cloud\n";
  accum_cloud->points.clear();

  for (int i=0; i <laser_queue->size() ; i++){
    bot_core::planar_lidar_t curr_msg =  laser_queue->at(i);

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr lidar_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    double maxRange = 2.0;//just the area right in front of the user    29.7;
    double minRange =0.0;
    double validBeamAngles[] ={-2.5,2.5}; // all of it both sides
    convertLidar(curr_msg.ranges, curr_msg.nranges, curr_msg.rad0,
      curr_msg.radstep, lidar_cloud, minRange, maxRange,
      validBeamAngles[0], validBeamAngles[1]);

    for (size_t j=0; j< lidar_cloud->points.size() ; j++){
       pcl::PointXYZRGB pt = lidar_cloud->points[j];
       float range = sqrt( pow(pt.x,2) + pow(pt.y,2) + pow(pt.z,2) );
       if (range< 2.0){
         float rgb[3];
         jet_rgb( (range - 0.5)/1.5, rgb); // map 0:2 to range -1/3:1   [i.e. 0.5->2 becomes 0->1]
         pt.r = rgb[0]*255.0; pt.g = rgb[1]*255.0; pt.b = rgb[2]*255.0;
       }else{
         pt.r = 255.0; pt.g = 0.0; pt.b = 0.0;
       }
       lidar_cloud->points[j] = pt;
    }

    BotTrans transform;
    bot_frames_get_trans_with_utime(botframes, "ROTATING_SCAN", "local",
                                          curr_msg.utime, &transform);

    // Fixed rigid transform
    Eigen::Isometry3d local_to_lidar;

    Eigen::Quaterniond quat = Eigen::Quaterniond(transform.rot_quat[0], transform.rot_quat[1],transform.rot_quat[2],transform.rot_quat[3]);
    local_to_lidar.setIdentity();
    local_to_lidar.translation()  << transform.trans_vec[0], transform.trans_vec[1], transform.trans_vec[2];
    local_to_lidar.rotate(quat);

    Eigen::Isometry3f pose_f = local_to_lidar.cast<float>();
    Eigen::Quaternionf pose_quat(pose_f.rotation());
    pcl::transformPointCloud (*lidar_cloud, *lidar_cloud,
        pose_f.translation(), pose_quat); // !! modifies lidar_cloud
    (*accum_cloud) += (*lidar_cloud);
  }
}

void RegApp::doReg(){
  // grey assumed:
  Mat ref_img = Mat::zeros( ref_image_.height , ref_image_.width ,CV_8UC1); // h,w
  ref_img.data = ref_image_.data.data();
  Mat cur_img = Mat::zeros( cur_image_.height , cur_image_.width ,CV_8UC1); // h,w
  cur_img.data = cur_image_.data.data();

  //imwrite("ref.png",ref_img);
  //imwrite("cur.png",cur_img);

  FrameMatchPtr match(new FrameMatch());
  reg->align_images(ref_img, cur_img, ref_features_, cur_features_, ref_image_.utime, cur_image_.utime , match);

  if (match->status == pose_estimator::SUCCESS){
    Eigen::Quaterniond delta_quat = Eigen::Quaterniond(match->delta.rotation());
    cout << "\nsuccess: matched ref at "
       <<ref_image_.utime <<" (" << ref_features_.size() << "f) to cur at "
       <<cur_image_.utime <<" " << cur_features_.size() << "f) | inliers: "<< match->n_inliers <<"\n";

  Eigen::Isometry3d rev_pose_revised = cur_pose_* match->delta.inverse();

  std::stringstream ss2;
  ss2 << "Ref: "; print_Isometry3d(ref_pose_, ss2); ss2 << "\n";
  ss2 << "Cur: "; print_Isometry3d(cur_pose_, ss2); ss2 << "\n";
  ss2 << "DLTA "; print_Isometry3d(match->delta, ss2); ss2 << "\n";
  ss2 << "New: "; print_Isometry3d(rev_pose_revised, ss2); ss2 << "\n";
  std::cout << ss2.str() << "\n";

  Isometry3dTime revised_poseT = Isometry3dTime(ref_image_.utime, rev_pose_revised );
  pc_vis_->pose_to_lcm_from_list(70100, revised_poseT);
  pc_vis_->ptcld_to_lcm_from_list(70101, *accum_cloud, ref_image_.utime, ref_image_.utime);

  }else{
    cout << "f";
    //cout << "failure: no match old frame at "
    //   <<ref_image_.utime <<" (" << ref_features_.size() << "f) to "
    //   <<cur_image_.utime <<" " << cur_features_.size() << "f)\n";
  }
}

std::vector<ImageFeature> RegApp::getFeaturesFromLCM(const  reg::features_t* msg, Eigen::Isometry3d &pose){
  std::vector<ImageFeature> features;

  for (int i =0; i < msg->nfeatures; i++){
    ImageFeature f;

    f.track_id = msg->features[i].track_id;
    f.uv[0] = msg->features[i].uv[0];
    f.uv[1] = msg->features[i].uv[1];
    f.base_uv[0] = msg->features[i].base_uv[0];
    f.base_uv[1] = msg->features[i].base_uv[1];
    f.uvd[0] = msg->features[i].uvd[0];
    f.uvd[1] = msg->features[i].uvd[1];
    f.uvd[2] = msg->features[i].uvd[2];
    f.xyz[0] = msg->features[i].xyz[0];
    f.xyz[1] = msg->features[i].xyz[1];
    f.xyz[2] = msg->features[i].xyz[2];
    f.xyzw[0] = msg->features[i].xyzw[0];
    f.xyzw[1] = msg->features[i].xyzw[1];
    f.xyzw[2] = msg->features[i].xyzw[2];
    f.xyzw[3] = msg->features[i].xyzw[3];
    // color left out for now

    pose.setIdentity();
    pose.translation() << msg->pos[0], msg->pos[1], msg->pos[2];
    Eigen::Quaterniond m(msg->quat[0],msg->quat[1],msg->quat[2],msg->quat[3]);
    pose.rotate(m);
    /*
    cout << line << " is line\n";
    cout << "i: " << i <<"\n";
    cout << "f.track_id: " << f.track_id <<"\n";
    cout << "f.uv: " << f.uv[0] << " "<< f.uv[1] <<"\n";
    cout << "f.base_uv: " << f.base_uv[0] << " "<< f.base_uv[1] <<"\n";
    cout << "f.uvd: " << f.uvd[0] << " "<< f.uvd[1]<< " "<< f.uvd[2]<<"\n";
    cout << "f.xyz: " << f.xyz[0] << " "<< f.xyz[1]<< " "<< f.xyz[2]<<"\n";
    cout << "f.xyzw: " << f.xyzw[0] << " "<< f.xyzw[1]<< " "<< f.xyzw[2]<< " "<< f.xyzw[3]<<"\n";
    cout << "f.color: " << (int)f.color[0] << " "<< (int)f.color[1] << " "<< (int)f.color[2] <<"\n";
      */

    features.push_back(f);
  }

  //cout << "in: " << msg->nfeatures << " | extracted: "<< features.size() << "\n";
  return features;
}

void RegApp::featuresCurrentHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  reg::features_t* msg){
  if (registeration_mode_!= 2){
    return;
  }
  //cout << "\n\ngot current features @ "<< msg->utime<<"\n";

  // now find the corresponding image and pair it
  bool feat_image_matched_ = false;
  for(int i=0; i < image_queue_->size(); i++){
    //cout << image_queue_->at(i).utime << " " << i << "\n";
    if (msg->utime == image_queue_->at(i).utime){
      cur_features_ = getFeaturesFromLCM(msg,cur_pose_);
      cur_image_ = image_queue_->at(i);
      //cout << "image and features matched: " << i << " " << image_queue_->at(i).utime << " ("<< cur_features_.size()<<"f)\n";
      feat_image_matched_ = true;

      break;
    }
  }

  // If the registartation have been activated, do it live:
  if (feat_image_matched_ ){
    doReg();
  }else{
   cout << "didn't find a matching current image - make image deque bigger\n";  // not fatal... just continue
  }
}

void RegApp::featuresReferenceHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  reg::features_t* msg){

  if (registeration_mode_ != 1){ // ignore features if we havent just been asked to track to them
    return;
  }

  cout << "\n\ngot reference features @ "<< msg->utime<<"\n";

  // now find the corresponding image and pair it
  bool feat_image_matched_ = false;
  for(int i=0; i < image_queue_->size(); i++){
    //cout << image_queue_->at(i).utime << " " << i << "\n";
    if (msg->utime == image_queue_->at(i).utime){
      cur_features_ = getFeaturesFromLCM(msg,cur_pose_);
      cur_image_ = image_queue_->at(i);
      cout << "image and features matched: " << i << " " << image_queue_->at(i).utime << " ("<< cur_features_.size()<<"f)\n";
      feat_image_matched_ = true;

      break;
    }
  }

  if (!feat_image_matched_){
     cout <<"didn't find a matching reference image - make image deque bigger\n";
     exit(-1);
  }

  ref_features_ = cur_features_;
  ref_image_ = cur_image_;
  ref_pose_ = cur_pose_;

  // set the reference image
  cout << "Will now register to " << ref_features_.size() << " features\n";

  // Send the pose we will try to register back to:
  Isometry3dTime ref_poseT = Isometry3dTime(ref_image_.utime, ref_pose_);
  pc_vis_->pose_to_lcm_from_list(70000, ref_poseT);

  deque_to_cloud(laser_queue_, botframes_, accum_cloud);

  Isometry3dTime null_poseT = Isometry3dTime(ref_image_.utime, Eigen::Isometry3d::Identity()  );
  pc_vis_->pose_to_lcm_from_list(70001, null_poseT);
  pc_vis_->ptcld_to_lcm_from_list(70002, *accum_cloud, ref_image_.utime, ref_image_.utime);

  std::stringstream ss2;
  print_Isometry3d(ref_pose_, ss2);
  std::cout << "Received refpose: " << ss2.str() << "\n";

  Eigen::Isometry3f pose_f = ref_pose_.inverse().cast<float>();
  Eigen::Quaternionf pose_quat(pose_f.rotation());
  pcl::transformPointCloud (*accum_cloud, *accum_cloud,
      pose_f.translation(), pose_quat); // !! modifies accum_cloud
  pc_vis_->ptcld_to_lcm_from_list(70003, *accum_cloud, ref_image_.utime, ref_image_.utime);

  registeration_mode_ = 2;
}

void RegApp::imageHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::image_t* msg){
  //cout << "got image @ "<< msg->utime <<"\n";

  // Keep a buffer/deque of left images - should only require a few images
  bot_core::image_t msg_cpy = *msg;
  image_queue_->push_back(  msg_cpy );

  //cout << "image_queue_ size: " << image_queue_->size() << "\n";
  if( image_queue_->size() > 10){
    image_queue_->pop_front();
  }
}

void RegApp::registerCommandHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  drc::map_params_t* msg){
  // TODO: add check that we've seen last_features
  cout << "got command\n";

  registeration_mode_ = 1;
}

void RegApp::lidarHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const  bot_core::planar_lidar_t* msg){
  bot_core::planar_lidar_t msg_cpy = *msg;
  laser_queue_->push_back(  msg_cpy );

  while (laser_queue_->size() > 100){
    laser_queue_->pop_front();
  }

  //if (newmap_requested){
  //  deque_to_cloud();
  //  newmap_requested=0;
  //}
}

int main( int argc, char** argv ){
  ConciseArgs parser(argc, argv, "registeration-app");
  string camera="CAMLCM_IMAGE_GRAY_LEFT";
  parser.add(camera, "c", "camera", "Camera channel");
  parser.parse();
  cout << camera << " is camera\n";

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

  RegApp app(lcm, camera);
  cout << "registeration is ready" << endl << endl;
  while(0 == lcm->handle());
  return 0;
}


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

#include "clotho/data_spaces/analysis/pairwise_difference/pairwise_difference_def.hpp"
#include "clotho/data_spaces/analysis/pairwise_difference/pairwise_difference_evaluator.hpp"

#include "clotho/data_spaces/association_matrix.hpp"

#include <boost/dynamic_bitset.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/variance.hpp>

namespace clotho {
namespace genetics {

template < class BlockType, class AlignmentType >
class pairwise_difference< association_matrix< BlockType, AlignmentType > > {
public:

    typedef association_matrix< BlockType, AlignmentType >              space_type;
    typedef typename space_type::block_type                             block_type;

    typedef ac::accumulator_set< size_t, ac::stats< ac::tag::min, ac::tag::mean, ac::tag::max, ac::tag::variance, ac::tag::median > >                                                           result_type;
    typedef boost::dynamic_bitset<>                                     analyzed_set_type;

    typedef pairwise_difference_evaluator< BlockType, AlignmentType >   evaluator_type;
    pairwise_difference( ) { }

    void evaluate( space_type & ss ) {
        m_indices.resize( ss.row_count() );

        m_indices.reset();
        m_indices.flip();

        eval( ss );
    }

    template < class Iterator >
    void evaluate( space_type & ss, Iterator first, Iterator last ) {
        m_indices.resize( ss.row_count() );
        m_indices.reset();

        size_t N = 0;
        while( first != last ) {
            size_t i = *first++;
            m_indices[i] = true;
            ++N;
        }

        eval( ss );
    }

    result_type & getResults() {
        return m_results;
    }

    void recordResults( boost::property_tree::ptree & log ) {
        log.put( "min", ac::min( m_results ) );
        log.put( "max", ac::max( m_results ) );
        log.put( "mean", ac::mean( m_results ) );
        log.put( "median", ac::median( m_results ) );
        log.put( "variance", ac::variance( m_results ) );
    }

    virtual ~pairwise_difference() {}

protected:

    void eval( space_type & ss ) {
        typedef typename space_type::raw_block_pointer iterator;

        evaluator_type ee( ss.block_column_count() );

        size_t i = m_indices.find_first();
        while( i != analyzed_set_type::npos ) {
            size_t j = m_indices.find_next( i );
            while( j != analyzed_set_type::npos ) {

                iterator i_start = ss.begin_row( i );
                iterator i_end = ss.end_row(i);

                iterator j_start = ss.begin_row( j );
                iterator j_end = ss.end_row(j);

                size_t diff = ee( i_start, i_end, j_start, j_end );

                m_results( diff );

                j = m_indices.find_next( j );
            }

            i = m_indices.find_next(i);
        }
    }

    result_type         m_results;
    analyzed_set_type   m_indices;
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_ASSOCIATION_MATRIX_PAIRWISE_DIFFERENCE_HPP_


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/config.hpp>
#include <boost/simd/detail/nsm.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/detail/constant_traits.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/simd/detail/dispatch/as.hpp>

/*

    @ingroup group-constant

    Constant used in modular computation involving \f$\pi\f$

    @par Semantic:

    For type T:

    @code
    T r = Pio2_2t<T>();
    @endcode

    @return a value of type T

*/

namespace boost { namespace simd
{
  namespace tag
  {
    struct pio2_2t_ : boost::dispatch::constant_value_<pio2_2t_>
    {
      BOOST_DISPATCH_MAKE_CALLABLE(ext,pio2_2t_,boost::dispatch::constant_value_<pio2_2t_>);
      BOOST_SIMD_REGISTER_CONSTANT(0, 0X2E85A308, 0X3BA3198A2E037073LL);
    };
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, pio2_2t_)
  }

  namespace detail
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::pio2_2t_,pio2_2t);
  }

  template<typename T> BOOST_FORCEINLINE auto Pio2_2t()
  BOOST_NOEXCEPT_DECLTYPE(detail::pio2_2t( boost::dispatch::as_<T>{}))
  {
    return detail::pio2_2t( boost::dispatch::as_<T>{} );
  }
} }

#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


#include "core/itf/ray.h"
#include "core/itf/triangle.h"

#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

#include <iomanip>
#include <iostream>
#include <chrono>

namespace po = boost::program_options;

void runIntersectionTests(unsigned int width, unsigned int height, unsigned int frames, const lb::Triangle &t,
        bool (lb::Ray::*intersectionMethod)(const lb::Triangle &) const)
{
    lb::Point3d origin(0.0, 0.0, 800.0);

    for (unsigned int frame = 0; frame < frames; ++frame)
    {
        for (unsigned int j = 0; j < height; ++j)
        {
            lb::Ray ray(origin, lb::Vector3d(-0.5 * width, 0.5 * height - j, -800.0));

            for (unsigned int i = 0; i < width; ++i)
            {
                ray.d.x += 1;

                (ray.*intersectionMethod)(t);
            }
        }
    }
}

enum IntersectionAlgorithm
{
    IA_DIDIER_BADOUEL,
    IA_MOLLER_TRUMBORE,
    IA_GEOMETRICALLY,
};

std::istream &operator>>(std::istream &in, IntersectionAlgorithm &ia)
{
    std::string token;
    in >> token;

    if (token == "didier_badouel")
    {
        ia = IA_DIDIER_BADOUEL;
    }
    else if (token == "geometrically")
    {
        ia = IA_GEOMETRICALLY;
    }
    else if (token == "moller_trumbore")
    {
        ia = IA_MOLLER_TRUMBORE;
    }

    return in;
}

std::ostream &operator<<(std::ostream &out, const IntersectionAlgorithm &ia)
{
    switch (ia)
    {
        case IA_DIDIER_BADOUEL:
            out << "didier_badouel";
            break;
        case IA_MOLLER_TRUMBORE:
            out << "moller_trumbore";
            break;
        case IA_GEOMETRICALLY:
            out << "geometrically";
            break;
    }

    return out;
}

int main(int argc, char **argv)
{
    unsigned int frames = 120;
    unsigned int iterations = 10;
    IntersectionAlgorithm algorithm = IA_MOLLER_TRUMBORE;

    po::options_description description("Available options");
    description.add_options()
        ("help,h", "Shows this help message")
        ("iterations,i", po::value<unsigned int>(&iterations), "Number of times to run the intersection tests")
        ("frames,f", po::value<unsigned int>(&frames), "Number of frames to render")
        ("type,t", po::value<IntersectionAlgorithm>(&algorithm), "Type of intersection test ['didier_badouel', 'moller_trumbore', 'geometrically']");

    po::variables_map vm;

    try
    {
        po::store(po::parse_command_line(argc, argv, description), vm);
        po::notify(vm);

        if (vm.count("help"))
        {
            std::cout << "Ray/triangle intersection benchmark application." << std::endl << std::endl << description << std::endl;
            return 1;
        }
    }
    catch (const po::error &e)
    {
        std::cerr << "Error parsing command line: " << e.what() << "." << std::endl;
        return 1;
    }

    // Depending on the selected algorithm, use the correct intersection
    // algorithm.
    bool (lb::Ray::*intersectionMethod)(const lb::Triangle &) const;
    switch (algorithm)
    {
        case IA_DIDIER_BADOUEL:
            intersectionMethod = &lb::Ray::intersectsDidierBadouel;
            break;
        case IA_MOLLER_TRUMBORE:
            intersectionMethod = &lb::Ray::intersectsMollerTrumbore;
            break;
        case IA_GEOMETRICALLY:
            intersectionMethod = &lb::Ray::intersectsGeometrically;
            break;
    }

    // Determine the width and height in pixels of the frame to render
    // repeatedly.
    const unsigned int width = 640;
    const unsigned int height = 480;

    // For now, just intersect against a single triangle.
    lb::Triangle triangle(lb::Point3d(0.0, 0.0, 0.0), lb::Point3d(200.0, 0.0, 0.0), lb::Point3d(100.0, 200.0, 0.0));

    // Run the intersection test once for a single frame, to prevent cluttered
    // overall benchmark results due to cache warmup effects.
    runIntersectionTests(width, height, 1, triangle, intersectionMethod);

    // Total number of intersections per iteration to benchmark.
    const unsigned int intersections = width * height * frames;

    std::cout << "Starting benchmark performing " << intersections << " ray/triangle intersections for " << iterations << " iteration(s), using '" <<
        algorithm << "'..." << std::endl << std::endl;

    // Rasterize the triangle, multiple times.
    double totalSeconds = 0;
    for (unsigned int i = 0; i < iterations; ++i)
    {
        std::chrono::time_point<std::chrono::steady_clock> start = std::chrono::steady_clock::now();

        runIntersectionTests(width, height, frames, triangle, intersectionMethod);

        double seconds = std::chrono::duration<double>(std::chrono::steady_clock::now() - start).count();
        long intersectionsPerSecond = (width * height * frames) / seconds;

        std::cout << std::setprecision(4) << std::fixed;
        std::cout << "Finished in " << seconds << "s, " << intersectionsPerSecond << " intersections per second." << std::endl;

        totalSeconds += seconds;
    }

    std::cout << std::endl << "Overall tested " << (intersections * iterations) << " rays in " << totalSeconds << "s, " <<
        (unsigned int)((intersections * iterations) / totalSeconds) << " intersections per second." << std::endl;

    return 0;
}


#pragma once

#include <Eigen/Dense>
#include <type_traits>
#include <vector>

#include "ft_eval.hpp"
#include "pl_radial_eval.hpp"
#include "spectral/basis/spectral_basis_factory_ks.hpp"

namespace boltzmann {

template <typename NUMERIC>
class PolarLaguerreEvaluator
{
 public:
  typedef NUMERIC numeric_t;
  typedef SpectralBasisFactoryKS::basis_type basis_t;

 public:
  PolarLaguerreEvaluator(const basis_t& basis)
      : L_(basis)
  { /* empty */
  }

  template <typename D>
  void init(const Eigen::DenseBase<D>& phi, const Eigen::DenseBase<D>& r);

 private:
  enum class storage
  {
    rowMajor,
    colMajor
  };

 private:
  LaguerreNKS<numeric_t> L_;
  FT<NUMERIC> A_;
  const basis_t& basis_;
  enum storage storage_;
};

template <typename NUMERIC>
template <typename D>
PolarLaguerreEvaluator<NUMERIC>(const Eigen::DenseBase<D>& phi, const Eigen::DenseBase<D>& r)
{
  if (D::IsRowMajor)
    storage_ = storage::rowMajor;
  else if (D::IsColMajor)
    storage_ = storage::colMajor;

  int n = phi.rows();
  int m = phi.cols();
}

}  // boltzmann


//
//  TDF SDK
//
//  Created by Sujan Reddy on 2019/04/16
//  Copyright 2019 Virtru Corporation
//

#define BOOST_TEST_MODULE test_gcm_encoding_suite

#include "gcm_encryption.h"
#include "bytes.h"
#include "tdf_exception.h"
#include "crypto_utils.h"

#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(test_gcm_encoding_suite)

    constexpr size_t kChunkSize = 25u;
    constexpr size_t kBufferSize = 1024u;

    using namespace std::string_literals;
    using namespace virtru::crypto;

    const static auto kPlainText =
            "Ehrsam, Meyer, Smith and Tuchman invented the Cipher Block Chaining (CBC) mode of operation in 1976. "
            "In CBC mode, each block of plaintext is XORed with the previous ciphertext block before being encrypted. "
            "This way, each ciphertext block depends on all plaintext blocks processed up to that point. "
            "To make each message unique, an initialization vector must be used in the first block."s;

    const static auto kEncryptedAes256GCMBase64 =
            "RFo0e8Q+dFtuBtMh0VGZk5u7BlXwUZvSbtPEZNJlrm5b893voEVhuqnQNF7HoPAN/Un0OL72cUJiLc8h0ufHhA6L3VtXFqSXTr+ebsphfTW2HI"
            "kV7s3p4MD2eg/XwjsAt4FxkYV/FoeQti8TSlkCfytnq6ClqQnYYw00UKNHoAb7uromnTz/IBeRRvBxQgg26arS5AvIHD9UdN4LfAvTXcsn+OfK"
            "9mWhiM9A6WAJSr/D/LXgvn6YLmtzd5txDpYrpR/2H6AI1yJDuv5CXjrsxbK1kk2gORWSCqozACQXnMT1v3nnSwr+DT6rXZ/ZdS5jl3hOdqQnCU"
            "477M2pJ1mWh87pmb6w9TyGqvwpuvc2Y36Z03LbrXPJw/sQWhRDIUtWTnPJsHKch/H+Dbw6Tdj7HnJb4o/7+IT9GpvgnDgNJ55yfZsAU655+Cag"
            "X5tCvCA+hYixxsD+/J5e+frs+SBacKQzNeq01IxA0vsVkoSryG+4sFu3nZI/gtgtu1vL693B"s;

    const static std::array<std :: uint8_t, 32u> kSymmetricKey = {
            0xf0, 0x5b, 0x5f, 0xab, 0x91, 0x60, 0xf3, 0xc6,
            0x51, 0x6a, 0x83, 0x3e, 0x82, 0xa3, 0x56, 0x62,
            0x65, 0xb1, 0x68, 0x01, 0xf2, 0x9f, 0x9a, 0x55,
            0xe2, 0x01, 0xba, 0xc6, 0x8a, 0xb8, 0x91, 0xf6
    };

    const static std::array<std :: uint8_t, 12u> kIV = {
            0x1a, 0x5f, 0x26, 0xf9, 0x5f, 0x81,
            0xf4, 0x42, 0x94, 0x60, 0x3b, 0x73
    };

    /// For testing message data authenticity (integrity).
    const static std::array<std :: uint8_t, 16> kAuthTagNoAad = {0x6c, 0x61, 0xc4, 0x6b, 0x8f, 0x80, 0x8e, 0x27, 0x67, 0x76, 0xaa, 0x24, 0xbb, 0x1d, 0x10, 0x77};
    const static std::vector<std :: uint8_t> kAuthAad = {0xeb, 0xfe, 0xa8, 0x74, 0x64, 0x28, 0x80, 0x96, 0x7b, 0x8a, 0x2a, 0x5f};
    const static std::array<std :: uint8_t, 16> kAuthTagForDefinedAad = {0xf3, 0xc1, 0x92, 0x81, 0x89, 0xe8, 0x9, 0x4, 0x61, 0x5f, 0xc1, 0x38, 0x5, 0x44, 0x7a, 0xb8};

    BOOST_AUTO_TEST_CASE(test_gcm_encoding_with_no_aad) {

        ByteArray<16> tag;
        auto encoder = GCMEncryption::create(toBytes(kSymmetricKey), toBytes(kIV));

        ByteArray<kBufferSize> buffer1;
        const auto bufferSpan = WriteableBytes{buffer1};

        auto outBufferSpan = bufferSpan;
        encoder->encrypt(toBytes(kPlainText), outBufferSpan);

        auto authTag = WriteableBytes{tag};
        encoder->finish(authTag);

        auto base64EncryptedData = base64Encode(bufferSpan.first(outBufferSpan.size()));
        BOOST_TEST(kEncryptedAes256GCMBase64 == base64EncryptedData);
        BOOST_TEST(toBytes(kAuthTagNoAad) == toBytes(tag));

        /// Test with wrapped key by copying the orginal key.
        WrappedKey key;
        for (size_t i = 0 ; i < kSymmetricKey.size(); i++) {
            key[i] = static_cast<gsl::byte>(kSymmetricKey[i]);
        }

        std::vector<gsl::byte> buffer(kBufferSize);
        auto writeableBytes = toWriteableBytes(buffer);

        encoder = GCMEncryption::create(toBytes(key), toBytes(kIV));
        encoder->encrypt(toBytes(kPlainText), writeableBytes);

        authTag = WriteableBytes{tag};
        encoder->finish (authTag);

        std::string decryptedMessage(reinterpret_cast<const char *>(&writeableBytes[0]), writeableBytes.size());
        base64EncryptedData = base64Encode(decryptedMessage);
        BOOST_TEST(kEncryptedAes256GCMBase64 == base64EncryptedData);
        BOOST_TEST(toBytes(kAuthTagNoAad) == toBytes(tag));
    }

    BOOST_AUTO_TEST_CASE(test_gcm_encoding_with_same_io_buffer) {

        ByteArray<16> tag;

        std::vector<gsl::byte> buffer(kPlainText.size());
        std::transform(kPlainText.begin(), kPlainText.end(), buffer.begin(),
                       [] (char c) { return gsl::byte(c); });

        auto writeableBytes = toWriteableBytes(buffer);
        auto encoder = GCMEncryption::create(toBytes(kSymmetricKey), toBytes(kIV));

        /// Passing the same buffer as input and output
        encoder->encrypt(writeableBytes, writeableBytes);

        auto authTag = WriteableBytes{tag};
        encoder->finish (authTag);

        auto base64EncryptedData = base64Encode(writeableBytes);
        BOOST_TEST(kEncryptedAes256GCMBase64 == base64EncryptedData);
        BOOST_TEST(toBytes(kAuthTagNoAad) == toBytes(tag));
    }

    BOOST_AUTO_TEST_CASE (test_gcm_encoding_with_stream_and_no_aad) {
        ByteArray<16> authTag;

        auto encoder = GCMEncryption::create(toBytes(kSymmetricKey), toBytes(kIV));
        auto inputBuffer = toBytes(kPlainText);

        ByteArray<kBufferSize> buffer;
        const auto bufferSpan = WriteableBytes{buffer};

        auto numberOfEncryptedBytes = 0u;
        size_t numberOfBytesToEncrypt = inputBuffer.size();
        while (numberOfBytesToEncrypt) {

            auto inputBufferSpan = inputBuffer.subspan(inputBuffer.size() - numberOfBytesToEncrypt,
                                                       (std::min)(kChunkSize, numberOfBytesToEncrypt));
            auto outputBufferSpan = bufferSpan.subspan(numberOfEncryptedBytes);

            encoder->encrypt(inputBufferSpan, outputBufferSpan);

            numberOfBytesToEncrypt -= inputBufferSpan.size();
            numberOfEncryptedBytes += outputBufferSpan.size();
        }

        auto tag = WriteableBytes{authTag};
        encoder->finish(tag);

        const std::string base64Representation = base64Encode(bufferSpan.first(numberOfEncryptedBytes));
        BOOST_TEST(kEncryptedAes256GCMBase64 == base64Representation);
        BOOST_TEST(toBytes(kAuthTagNoAad) == toBytes(authTag));
    }

    BOOST_AUTO_TEST_CASE(test_gcm_encoding_with_small_out_buffer) {

        auto encoder = GCMEncryption::create(toBytes(kSymmetricKey), toBytes(kIV));

        std::vector<gsl::byte> outBuffer(kPlainText.size() - 1);
        auto outputBufferSpan = WriteableBytes {outBuffer};

        BOOST_CHECK_THROW(encoder->encrypt(toBytes(kPlainText), outputBufferSpan), virtru::Exception);
    }

    BOOST_AUTO_TEST_CASE(test_gcm_encoding_with_defined_aad) {

        ByteArray<16> authTag;
        auto encoder = GCMEncryption::create(toBytes(kSymmetricKey),
                                          toBytes(kIV),
                                          toBytes(kAuthAad));

        ByteArray<kBufferSize> buffer;
        auto outputBufferSpan = WriteableBytes{buffer};

        encoder->encrypt(toBytes(kPlainText), outputBufferSpan);

        auto tag = WriteableBytes{authTag};
        encoder->finish(tag);

        const std::string base64Representation = base64Encode(outputBufferSpan);
        BOOST_TEST(kEncryptedAes256GCMBase64 == base64Representation);
        BOOST_TEST(toBytes(kAuthTagForDefinedAad) == toBytes(authTag));
    }

BOOST_AUTO_TEST_SUITE_END()


#include "json_schedule.h"
#include "events.h"
#include "progress.h"
#include "utils.h"
#include <boost/iostreams/device/file_descriptor.hpp>
#include <boost/iostreams/stream.hpp>
#include <gsl/gsl-lite.hpp>
#include <range/v3/view/transform.hpp>
#include <vector>

namespace {
using namespace angonoka;

/**
    How long it will take for a given agent to complete a given
    task.

    Factors in agent's performace.

    @param task     Task
    @param agent    Agent who performs the task

    @return Time in seconds
*/
[[nodiscard]] float
task_duration(const Task& task, const Agent& agent)
{
    Expects(task.duration.average().count() > 0);
    Expects(agent.performance.average() > 0.F);

    return static_cast<float>(task.duration.average().count())
        * agent.performance.average();
}

/**
    The time when the last dependency will be completed.

    @param task_done    Array of task completion times
    @param deps         Dependency ids

    @return Time in seconds
*/
[[nodiscard]] float dependencies_done(
    const std::vector<float>& task_done,
    const TaskIndices& deps)
{
    Expects(!task_done.empty());

    using ranges::max;
    using ranges::views::transform;
    if (deps.empty()) return 0.F;
    return max(deps | transform([&](auto dep_id) {
                   return task_done[dep_id];
               }));
}

/**
    Opaque types.
*/
enum class AgentIndex : gsl::index {};
enum class TaskIndex : gsl::index {};

constexpr auto operator*(AgentIndex i)
{
    return static_cast<gsl::index>(i);
}
constexpr auto operator*(TaskIndex i)
{
    return static_cast<gsl::index>(i);
}

/**
    Utility class for calculating task durations.
*/
class TaskDurations {
public:
    /**
        Constructor.

        @param config Tasks and agents
    */
    TaskDurations(const Configuration& config)
        : agent_work_end(config.agents.size())
        , task_done(config.tasks.size())
        , config{&config}
    {
        Expects(!config.tasks.empty());
        Expects(!config.agents.empty());
    }

    /**
        Calculate the duration and starting time for
        a given task and agent combo.

        @param agent_id Agent id
        @param task_id  Task id

        @return Task duration and expected start
        time in seconds
    */
    [[nodiscard]] auto
    operator()(AgentIndex agent_id, TaskIndex task_id)
    {
        Expects(task_done[*task_id] == 0.F);
        Expects(*agent_id >= 0 && *agent_id < config->agents.size());
        Expects(*task_id >= 0 && *task_id < config->tasks.size());

        const auto duration = task_duration(
            config->tasks[*task_id],
            config->agents[*agent_id]);
        const auto expected_start = std::max(
            agent_work_end[*agent_id],
            dependencies_done(
                task_done,
                config->tasks[*task_id].dependencies));
        agent_work_end[*agent_id] = task_done[*task_id]
            = expected_start + duration;

        Ensures(task_done[*task_id] > 0.F);
        Ensures(agent_work_end[*agent_id] > 0.F);

        return std::make_tuple(
            static_cast<int>(duration),
            expected_start);
    }

private:
    std::vector<float> agent_work_end;
    std::vector<float> task_done;
    gsl::not_null<const Configuration*> config;
};
} // namespace

namespace angonoka::cli {
namespace detail {
    nlohmann::json to_json(
        const Configuration& config,
        const OptimizedSchedule& schedule)
    {
        Expects(!config.tasks.empty());
        Expects(!config.agents.empty());

        using nlohmann::json;
        json tasks;
        std::vector<int> agent_priority(config.agents.size());
        TaskDurations durations{config};

        for (const auto& t : schedule.schedule) {
            const auto t_id = static_cast<gsl::index>(t.task_id);
            const auto a_id = static_cast<gsl::index>(t.agent_id);
            const auto [duration, expected_start]
                = durations(::AgentIndex{a_id}, ::TaskIndex{t_id});

            tasks.emplace_back<json>(
                {{"task", config.tasks[t_id].name},
                 {"agent", config.agents[a_id].name},
                 {"priority", agent_priority[a_id]++},
                 {"expected_duration", duration},
                 {"expected_start", expected_start}});
        }

        Ensures(!tasks.empty());
        Ensures(schedule.makespan.count() > 0);

        return {
            {"makespan", schedule.makespan.count()},
            {"tasks", std::move(tasks)}};
    }
} // namespace detail

nlohmann::json
json_schedule(const Configuration& config, const Options& options)
{
    Expects(!config.tasks.empty());
    Expects(!config.agents.empty());

    auto [schedule_future, event_queue] = schedule(config);

    if (!options.quiet) {
        Progress progress;
        if (options.color) progress.emplace<ProgressBar>();
        consume_events(
            *event_queue,
            schedule_future,
            EventHandler{&progress, &options});
    }
    const auto schedule = schedule_future.get();
    return detail::to_json(config, schedule);
}

void save_json(const nlohmann::json& json, const Options& options)
{
    using boost::iostreams::file_descriptor_sink;
    using boost::iostreams::stream;

    Expects(!options.output.empty());
    Expects(!json.empty());

    try {
        stream<file_descriptor_sink> output{options.output};
        output << std::setw(4) << json;
    } catch (const std::runtime_error& e) {
        print_error(
            options,
            stderr,
            "Error saving the schedule:\n{}",
            e.what());
        throw UserError{};
    }
}
} // namespace angonoka::cli


#include "segmatch/descriptors/descriptors.hpp"

#include <Eigen/Dense>
#include <glog/logging.h>
#include <laser_slam/common.hpp>

#include "segmatch/descriptors/autoencoder.hpp"
#include "segmatch/descriptors/eigenvalue_based.hpp"
#include "segmatch/descriptors/ensemble_shape_functions.hpp"

namespace segmatch {

// Descriptors methods definition
Descriptors::Descriptors() {
  //TODO: Implement default construction from default parameters.
  CHECK(false) << "Construction of descriptors without parameters is not implemented.";
}

Descriptors::~Descriptors() {}

Descriptors::Descriptors(const DescriptorsParameters& parameters) {
  CHECK_GT(parameters.descriptor_types.size(), 0) << "Description impossible without a descriptor.";

  // Create the descriptors.
  for (size_t i = 0u; i < parameters.descriptor_types.size(); ++i) {
    if (parameters.descriptor_types[i] == "EigenvalueBased") {
      descriptors_.push_back(std::unique_ptr<Descriptor>(
          new EigenvalueBasedDescriptor(parameters)));
    } else if (parameters.descriptor_types[i] == "EnsembleShapeFunctions") {
      descriptors_.push_back(std::unique_ptr<Descriptor>(new EnsembleShapeFunctions(parameters)));
    } else if (parameters.descriptor_types[i] == "Autoencoder") {
      descriptors_.push_back(std::unique_ptr<Descriptor>(new AutoencoderDescriptor(parameters)));
    } else {
      CHECK(false) << "The descriptor '" << parameters.descriptor_types[i] <<
          "' was not implemented.";
    }
  }
}

void Descriptors::describe(const Segment& segment, Features* features) {
  CHECK_NOTNULL(features)->clear();
  CHECK_GT(descriptors_.size(), 0) << "Description impossible without a descriptor.";
  for (size_t i = 0u; i < descriptors_.size(); ++i) {
    descriptors_[i]->describe(segment, features);
  }
}

void Descriptors::describe(SegmentedCloud* segmented_cloud_ptr,
                           std::vector<double>* timings) {
  if (timings != NULL) {
    timings->clear();
  }
  CHECK_NOTNULL(segmented_cloud_ptr);
  CHECK_GT(descriptors_.size(), 0) << "Description impossible without a descriptor.";
  for (size_t i = 0u; i < descriptors_.size(); ++i) {
    laser_slam::Clock clock_descriptor;
    descriptors_[i]->describe(segmented_cloud_ptr);
    if (timings != NULL) {
      clock_descriptor.takeTime();
      timings->push_back(clock_descriptor.getRealTime());
      LOG(INFO) << "Decriptor " << i << " took " << clock_descriptor.getRealTime() << " ms.";
      clock_descriptor.start();
    }
  }
}

unsigned int Descriptors::dimension() const {
  CHECK_GT(descriptors_.size(), 0) << "Description impossible without a descriptor.";
  unsigned int dimension = 0;
  for (size_t i = 0u; i < descriptors_.size(); ++i) {
    dimension += descriptors_[i]->dimension();
  }
  return dimension;
}

} // namespace segmatch


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <ff_common/init.h>
#include <ff_common/utils.h>
#include <config_reader/config_reader.h>
#include <ekf/ekf.h>
#include <sparse_mapping/sparse_map.h>
#include <localization_node/localization.h>
#include <lk_optical_flow/lk_optical_flow.h>
#include <image_transport/image_transport.h>
#include <ff_util/ff_names.h>

#include <Eigen/Core>
#include <gflags/gflags.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

Eigen::Vector3f QuatToEuler(const geometry_msgs::Quaternion & q) {
  Eigen::Vector3f euler;
  float q2q2 = q.y * q.y;
  euler.x() = atan2(2 * (q.x * q.w + q.y * q.z), 1 - 2 * (q.x * q.x + q2q2));
  float arg = std::max(-1.0, std::min(1.0, 2 * (q.y * q.w - q.x * q.z)));
  euler.y() = asin(arg);
  euler.z() = atan2(2 * (q.x * q.y + q.z * q.w), 1 - 2 * (q2q2 + q.z * q.z));
  return euler;
}

Eigen::Vector3f QuatToEuler(const Eigen::Quaternionf & q) {
  Eigen::Vector3f euler;
  float q2q2 = q.y() * q.y();
  euler.x() = atan2(2 * (q.x() * q.w() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q2q2));
  float arg = std::max(-1.0f, std::min(1.0f, 2 * (q.y() * q.w() - q.x() * q.z())));
  euler.y() = asin(arg);
  euler.z() = atan2(2 * (q.x() * q.y() + q.z() * q.w()), 1 - 2 * (q2q2 + q.z() * q.z()));
  return euler;
}

void ReadParams(localization_node::Localizer* loc) {
  config_reader::ConfigReader config;
  config.AddFile("cameras.config");
  config.AddFile("geometry.config");
  config.AddFile("localization.config");
  if (!config.ReadFiles()) {
    ROS_ERROR("Failed to read config files.");
    return;
  }

  loc->ReadParams(&config);
}

int main(int argc, char ** argv) {
  ff_common::InitFreeFlyerApplication(&argc, &argv);

  struct timespec tstart={0, 0}, tend={0, 0};
  struct timespec tstart2={0, 0}, tend2={0, 0};
  clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tstart);
  clock_gettime(CLOCK_MONOTONIC, &tstart2);

  if (argc < 3) {
    LOG(INFO) << "Usage: " << argv[0] << " map.map bag.bag";
    exit(0);
  }

  rosbag::Bag bag;
  bag.open(argv[2], rosbag::bagmode::Read);

  sparse_mapping::SparseMap map(argv[1], true);
  localization_node::Localizer loc(&map);

  ReadParams(&loc);

  std::vector<std::string> topics;
  topics.push_back(std::string("/") + std::string(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_RECORD));
  topics.push_back(std::string(TOPIC_MANAGEMENT_IMG_SAMPLER_NAV_CAM_RECORD));
  topics.push_back(std::string("/") + std::string(TOPIC_HARDWARE_NAV_CAM));
  topics.push_back(std::string(TOPIC_HARDWARE_NAV_CAM));
  topics.push_back(std::string("/") + std::string(TOPIC_LOCALIZATION_TRUTH));
  topics.push_back(std::string(TOPIC_LOCALIZATION_TRUTH));
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  int progress = 0;
  int images = 0;
  int failures = 0;
  int total_features = 0;

  // If the user provided an existing directory in which to save the
  // failed images, do so.
  char * failed_images_dir = getenv("SAVE_FAILED_IMAGES_DIR");
  char filename_buffer[256];

  for (rosbag::MessageInstance const m : view) {
    progress++;

    if (m.isType<sensor_msgs::Image>()) {
      sensor_msgs::ImageConstPtr image_msg = m.instantiate<sensor_msgs::Image>();
      cv_bridge::CvImageConstPtr image;
      try {
        image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::MONO8);
      } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        break;
      }
      ff_msgs::VisualLandmarks vl_features;
      loc.Localize(image, &vl_features);
      if (vl_features.landmarks.size() == 0) {
        failures++;

        if (failed_images_dir != NULL) {
          double curr_image_time = image_msg->header.stamp.toSec();
          snprintf(filename_buffer, sizeof(filename_buffer),
                   "%s/%10.7f.jpg", failed_images_dir, curr_image_time);
          std::cout << "Writing image with failed localization: " << filename_buffer << std::endl;
          cv::imwrite(filename_buffer, image->image);
        }
      }

      images++;
      total_features += vl_features.landmarks.size();
    } else if (m.isType<geometry_msgs::PoseStamped>()) {
      geometry_msgs::PoseStampedConstPtr gt_msg = m.instantiate<geometry_msgs::PoseStamped>();
    }
    ff_common::PrintProgressBar(stdout, static_cast<float>(progress) / view.size());
  }

  bag.close();
  if (images - failures == 0) {
    printf("Failed to localize any images.\n");
  } else {
    printf("Localized %d / %d images with mean of %d features.\n",
         images - failures, images, total_features / (images - failures));
    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tend);
    clock_gettime(CLOCK_MONOTONIC, &tend2);
    printf("Time per Frame: %.5f s\n", ((static_cast<double>(tend2.tv_sec) + 1.0e-9*tend2.tv_nsec) -
                                       (static_cast<double>(tstart2.tv_sec) + 1.0e-9*tstart2.tv_nsec)) / images);
    printf("CPU Time per Frame: %.5f s\n", ((static_cast<double>(tend.tv_sec) + 1.0e-9*tend.tv_nsec) -
                                       (static_cast<double>(tstart.tv_sec) + 1.0e-9*tstart.tv_nsec)) / images);
  }
}


//  duration.hpp  --------------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009-2012 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*

This code was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#ifndef BOOST_CHRONO_TIME_POINT_HPP
#define BOOST_CHRONO_TIME_POINT_HPP

#include <boost/chrono/duration.hpp>
#include <iostream>

#ifndef BOOST_CHRONO_HEADER_ONLY
// this must occur after all of the includes and before any code appears:
#include <boost/config/abi_prefix.hpp> // must be the last #include
#endif

//----------------------------------------------------------------------------//
//                                                                            //
//                        20.9 Time utilities [time]                          //
//                                 synopsis                                   //
//                                                                            //
//----------------------------------------------------------------------------//

namespace boost {
namespace chrono {

  template <class Clock, class Duration = typename Clock::duration>
    class time_point;

} // namespace chrono

// common_type trait specializations

template <class Clock, class Duration1, class Duration2>
  struct common_type<chrono::time_point<Clock, Duration1>,
                     chrono::time_point<Clock, Duration2> >;

//----------------------------------------------------------------------------//
//      20.9.2.3 Specializations of common_type [time.traits.specializations] //
//----------------------------------------------------------------------------//

template <class Clock, class Duration1, class Duration2>
struct common_type<chrono::time_point<Clock, Duration1>,
                   chrono::time_point<Clock, Duration2> >
{
  typedef chrono::time_point<Clock,
    typename common_type<Duration1, Duration2>::type> type;
};

namespace chrono {

    // time_point arithmetic
    template <class Clock, class Duration1, class Rep2, class Period2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<Duration1, duration<Rep2, Period2> >::type>
    operator+(
            const time_point<Clock, Duration1>& lhs,
            const duration<Rep2, Period2>& rhs);
    template <class Rep1, class Period1, class Clock, class Duration2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<duration<Rep1, Period1>, Duration2>::type>
    operator+(
            const duration<Rep1, Period1>& lhs,
            const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Rep2, class Period2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<Duration1, duration<Rep2, Period2> >::type>
    operator-(
            const time_point<Clock, Duration1>& lhs,
            const duration<Rep2, Period2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    typename common_type<Duration1, Duration2>::type
    operator-(
            const time_point<Clock, Duration1>& lhs,
            const time_point<Clock,
            Duration2>& rhs);

    // time_point comparisons
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator==(
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator!=(
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator< (
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator<=(
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator> (
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);
    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool operator>=(
          const time_point<Clock, Duration1>& lhs,
          const time_point<Clock, Duration2>& rhs);

    // time_point_cast
    template <class ToDuration, class Clock, class Duration>
    inline BOOST_CONSTEXPR
    time_point<Clock, ToDuration> time_point_cast(const time_point<Clock, Duration>& t);

//----------------------------------------------------------------------------//
//                                                                            //
//      20.9.4 Class template time_point [time.point]                         //
//                                                                            //
//----------------------------------------------------------------------------//

    template <class Clock, class Duration>
    class time_point
    {
        BOOST_CHRONO_STATIC_ASSERT(boost::chrono::detail::is_duration<Duration>::value,
                BOOST_CHRONO_SECOND_TEMPLATE_PARAMETER_OF_TIME_POINT_MUST_BE_A_BOOST_CHRONO_DURATION, (Duration));
    public:
        typedef Clock                     clock;
        typedef Duration                  duration;
        typedef typename duration::rep    rep;
        typedef typename duration::period period;
        typedef Duration                  difference_type;

    private:
        duration d_;

    public:
        BOOST_FORCEINLINE BOOST_CONSTEXPR
        time_point() : d_(duration::zero())
        {}
        BOOST_FORCEINLINE BOOST_CONSTEXPR
        explicit time_point(const duration& d)
            : d_(d)
        {}

        // conversions
        template <class Duration2>
        BOOST_FORCEINLINE BOOST_CONSTEXPR
        time_point(const time_point<clock, Duration2>& t
                , typename boost::enable_if
                <
                    boost::is_convertible<Duration2, duration>
                >::type* = 0
        )
            : d_(t.time_since_epoch())
        {
        }
        // observer

        BOOST_CONSTEXPR
        duration time_since_epoch() const
        {
            return d_;
        }

        // arithmetic

#ifdef BOOST_CHRONO_EXTENSIONS
        BOOST_CONSTEXPR
        time_point  operator+() const {return *this;}
        BOOST_CONSTEXPR
        time_point  operator-() const {return time_point(-d_);}
        time_point& operator++()      {++d_; return *this;}
        time_point  operator++(int)   {return time_point(d_++);}
        time_point& operator--()      {--d_; return *this;}
        time_point  operator--(int)   {return time_point(d_--);}

        time_point& operator+=(const rep& r) {d_ += duration(r); return *this;}
        time_point& operator-=(const rep& r) {d_ -= duration(r); return *this;}

#endif

        time_point& operator+=(const duration& d) {d_ += d; return *this;}
        time_point& operator-=(const duration& d) {d_ -= d; return *this;}

        // special values

        static BOOST_CHRONO_LIB_CONSTEXPR time_point
        min BOOST_PREVENT_MACRO_SUBSTITUTION ()
        {
            return time_point((duration::min)());
        }
        static BOOST_CHRONO_LIB_CONSTEXPR time_point
        max BOOST_PREVENT_MACRO_SUBSTITUTION ()
        {
            return time_point((duration::max)());
        }
    };

//----------------------------------------------------------------------------//
//      20.9.4.5 time_point non-member arithmetic [time.point.nonmember]      //
//----------------------------------------------------------------------------//

    // time_point operator+(time_point x, duration y);

    template <class Clock, class Duration1, class Rep2, class Period2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<Duration1, duration<Rep2, Period2> >::type>
    operator+(const time_point<Clock, Duration1>& lhs,
            const duration<Rep2, Period2>& rhs)
    {
      typedef typename common_type<Duration1, duration<Rep2, Period2> >::type CDuration;
      typedef time_point<
          Clock,
          CDuration
      > TimeResult;
        return TimeResult(lhs.time_since_epoch() + CDuration(rhs));
    }

    // time_point operator+(duration x, time_point y);

    template <class Rep1, class Period1, class Clock, class Duration2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<duration<Rep1, Period1>, Duration2>::type>
    operator+(const duration<Rep1, Period1>& lhs,
            const time_point<Clock, Duration2>& rhs)
    {
        return rhs + lhs;
    }

    // time_point operator-(time_point x, duration y);

    template <class Clock, class Duration1, class Rep2, class Period2>
    inline BOOST_CONSTEXPR
    time_point<Clock,
        typename common_type<Duration1, duration<Rep2, Period2> >::type>
    operator-(const time_point<Clock, Duration1>& lhs,
            const duration<Rep2, Period2>& rhs)
    {
        return lhs + (-rhs);
    }

    // duration operator-(time_point x, time_point y);

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    typename common_type<Duration1, Duration2>::type
    operator-(const time_point<Clock, Duration1>& lhs,
            const time_point<Clock, Duration2>& rhs)
    {
        return lhs.time_since_epoch() - rhs.time_since_epoch();
    }

//----------------------------------------------------------------------------//
//      20.9.4.6 time_point comparisons [time.point.comparisons]              //
//----------------------------------------------------------------------------//

    // time_point ==

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator==(const time_point<Clock, Duration1>& lhs,
             const time_point<Clock, Duration2>& rhs)
    {
        return lhs.time_since_epoch() == rhs.time_since_epoch();
    }

    // time_point !=

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator!=(const time_point<Clock, Duration1>& lhs,
             const time_point<Clock, Duration2>& rhs)
    {
        return !(lhs == rhs);
    }

    // time_point <

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator<(const time_point<Clock, Duration1>& lhs,
            const time_point<Clock, Duration2>& rhs)
    {
        return lhs.time_since_epoch() < rhs.time_since_epoch();
    }

    // time_point >

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator>(const time_point<Clock, Duration1>& lhs,
            const time_point<Clock, Duration2>& rhs)
    {
        return rhs < lhs;
    }

    // time_point <=

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator<=(const time_point<Clock, Duration1>& lhs,
             const time_point<Clock, Duration2>& rhs)
    {
        return !(rhs < lhs);
    }

    // time_point >=

    template <class Clock, class Duration1, class Duration2>
    inline BOOST_CONSTEXPR
    bool
    operator>=(const time_point<Clock, Duration1>& lhs,
             const time_point<Clock, Duration2>& rhs)
    {
        return !(lhs < rhs);
    }

//----------------------------------------------------------------------------//
//      20.9.4.7 time_point_cast [time.point.cast]                            //
//----------------------------------------------------------------------------//

    template <class ToDuration, class Clock, class Duration>
    inline BOOST_CONSTEXPR
    time_point<Clock, ToDuration>
    time_point_cast(const time_point<Clock, Duration>& t)
    {
        return time_point<Clock, ToDuration>(
                duration_cast<ToDuration>(t.time_since_epoch()));
    }

} // namespace chrono
} // namespace boost

#ifndef BOOST_CHRONO_HEADER_ONLY
// the suffix header occurs after all of our code:
#include <boost/config/abi_suffix.hpp> // pops abi_prefix.hpp pragmas
#endif

#endif // BOOST_CHRONO_TIME_POINT_HPP


#define BOOST_TEST_MODULE pcraster dal types
#include <boost/test/unit_test.hpp>
#include "dal_Types.h"

BOOST_AUTO_TEST_CASE(id_of_smallest_type)
{
  using namespace dal;

  {
    // unsigned integers:
    // 1 byte  2^8  = 0 -        255
    // 2 bytes 2^16 = 0 -      65535
    // 4 bytes 2^32 = 0 - 4294967295

    Types types;
    BOOST_CHECK_EQUAL(types.idOfSmallestType(         "0"), TI_UINT1);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(         "1"), TI_UINT1);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(       "255"), TI_UINT1);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(       "256"), TI_UINT2);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(     "65535"), TI_UINT2);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(     "65536"), TI_UINT4);
    BOOST_CHECK_EQUAL(types.idOfSmallestType("4294967295"), TI_UINT4);
    BOOST_CHECK_EQUAL(types.idOfSmallestType("4294967296"), TI_REAL4);

    BOOST_CHECK_EQUAL(types.idOfSmallestType(         "-1"), TI_INT1);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(       "-128"), TI_INT1);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(       "-129"), TI_INT2);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(     "-32768"), TI_INT2);
    BOOST_CHECK_EQUAL(types.idOfSmallestType(     "-32769"), TI_INT4);
    BOOST_CHECK_EQUAL(types.idOfSmallestType("-2147483648"), TI_INT4);
    BOOST_CHECK_EQUAL(types.idOfSmallestType("-2147483649"), TI_REAL4);

    // Overgang REAL4 / REAL8...
    // String...

    BOOST_WARN_EQUAL(types.idOfSmallestType("NROWS"), TI_STRING);
  }
}

BOOST_AUTO_TEST_CASE(id_of_largest_type)
{
  using namespace dal;

  Types types;
  BOOST_CHECK_EQUAL(types.idOfLargestType(TI_UINT1, TI_UINT2), TI_UINT2);

  bool idOfLargestTypeImplemented = false;
  BOOST_WARN(idOfLargestTypeImplemented);
}


#include "gtest/gtest.h"
#include <armadillo>
#include "libsvm_runner.h"
#include "svm_basic.h"
#include "svm_client.h"
#include "svm_helpers.h"

using namespace arma;

//TEST(LibSVMRunnerTest, TrainingFilePredictionFile) {
//	SVMConfiguration svm_config;
//	mat A;
//	A << 0 << 0 << endr << 1 << 1 << endr;
//	vec b;
//	b << -1 << 1;
//	svm_config.setPrediction(false); // training model
//	svm_config.data = A;
//	svm_config.target = b;
//	svm_config.setModelFilename("bika.txt");
//	LibSVMRunner lib_svm_runner;
//	lib_svm_runner.processRequest(svm_config);
//	svm_config.setPrediction(true);
//	svm_config.setFilename("test.txt");
//	svm_config.setOutputFilename("test.txt");
//	lib_svm_runner.processRequest(svm_config);
//}
//
//TEST(LibSVMRunnerTest, ArmaTrainingPredictionFile) {
//	using namespace arma;
//	SVMConfiguration svm_config;
//	mat A;
//	A << 0 << 0 << endr << 1 << 1 << endr;
//	vec b;
//	b << -1 << 1;
//	svm_config.setPrediction(false); // training model
//	svm_config.data = A;
//	svm_config.target = b;
//	svm_config.setModelFilename("bika.txt");
//	LibSVMRunner lib_svm_runner;
//	lib_svm_runner.processRequest(svm_config);
//	svm_config.setPrediction(true);
//	svm_config.setFilename("test.txt");
//	svm_config.setOutputFilename("test.txt");
//	lib_svm_runner.processRequest(svm_config);
//}
//
//TEST(LibSVMRunnerTest, ArmaTrainingArmaPrediction) {
//
//	SVMConfiguration svm_config;
//	mat A;
//	A << 0 << 0 << endr << 1 << 1 << endr;
//	vec b;
//	b << -1 << 1;
//	svm_config.setPrediction(false); // training model
//	svm_config.data = A;
//	svm_config.target = b;
//	svm_config.setModelFilename("svm_test_model.txt");
//	LibSVMRunner lib_svm_runner;
//	lib_svm_runner.processRequest(svm_config);
//
//	svm_config.setPrediction(true);
//	lib_svm_runner.processRequest(svm_config);
//
//	ASSERT_EQ(-1.0, svm_config.result[0]);
//	ASSERT_EQ(1.0, svm_config.result[1]);
//}

TEST(LibSVMRunnerTest, ArmaTrainingConfigModel) {

	SVMConfiguration svm_config;
	mat A;
	A << 0 << 0 << endr << 1 << 1 << endr;
	vec b;
	b << -1 << 1;
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

//	svm_config.setPrediction(true);
//	lib_svm_runner.processRequest(svm_config);
//
//	ASSERT_EQ(-1.0, svm_config.result[0]);
//	ASSERT_EQ(1.0, svm_config.result[1]);
}

TEST(LibSVMRunnerTest, ArmaTrainingArmaPredictionConfigModel2x2) {
	SVMConfiguration svm_config;
	mat A;
	A << 0 << 0 << endr << 1 << 1 << endr;
	vec b;
	b << -1 << 1;
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

	svm_config.setPrediction(true);
	lib_svm_runner.processRequest(svm_config);

	ASSERT_EQ(-1.0, svm_config.result[0]);
	ASSERT_EQ(1.0, svm_config.result[1]);
}

TEST(LibSVMRunnerTest, DoubleArmaTrainingDoubleArmaPredictionConfigModel4x2) {

	SVMConfiguration svm_config;
	mat A;
	A << 0 << 0 << endr
	  << 1 << 1 << endr
	  << 2 << 2 << endr
	  << 3 << 3 << endr;
	vec b;
	b << -1 << -1 << 1 << 1;
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);
	lib_svm_runner.processRequest(svm_config);

	svm_config.setPrediction(true);
	lib_svm_runner.processRequest(svm_config);
	lib_svm_runner.processRequest(svm_config);

	ASSERT_EQ(-1.0, svm_config.result[0]);
	ASSERT_EQ(-1.0, svm_config.result[1]);
	ASSERT_EQ(1.0, svm_config.result[2]);
	ASSERT_EQ(1.0, svm_config.result[3]);
}

TEST(LibSVMRunnerTest, ArmaTrainingArmaPredictionConfigModel4x2) {

	SVMConfiguration svm_config;
	mat A;
	A << 0 << 0 << endr
	  << 1 << 1 << endr
	  << 2 << 2 << endr
	  << 3 << 3 << endr;
	vec b;
	b << -1 << -1 << 1 << 1;
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

	svm_config.setPrediction(true);
	lib_svm_runner.processRequest(svm_config);

	ASSERT_EQ(-1.0, svm_config.result[0]);
	ASSERT_EQ(-1.0, svm_config.result[1]);
	ASSERT_EQ(1.0, svm_config.result[2]);
	ASSERT_EQ(1.0, svm_config.result[3]);
}

TEST(LibSVMRunnerTest, ArmaTrainingArmaPredictionConfigModel4x2OtherPoints) {

	SVMConfiguration svm_config;
	mat A;
	A << 0 << 0 << endr
	  << 1 << 1 << endr
	  << 2 << 2 << endr
	  << 3 << 3 << endr;
	vec b;
	b << -1 << -1 << 1 << 1;
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

	mat A2;
	A2 << 0 << 0 << endr
	  << 30 << 40 << endr
	  << 50 << 60 << endr
	  << -1 << -1 << endr;

	vec b2;
	b2 << -1 << 1 << 1 << -1;

	svm_config.data = A2;
	svm_config.target = b2;

	svm_config.setPrediction(true);
	lib_svm_runner.processRequest(svm_config);

	ASSERT_EQ(-1.0, svm_config.result[0]);
	ASSERT_EQ(1.0, svm_config.result[1]);
	ASSERT_EQ(1.0, svm_config.result[2]);
	ASSERT_EQ(-1.0, svm_config.result[3]);
}

TEST(LibSVMRunnerTest, integration_svmclient_predict) {
	SVMConfiguration svm_config;
    arma::mat A = helper_learning_data_01();
    arma::mat b = helper_learning_target_01();
	svm_config.setPrediction(false); // training model
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

    std::cout << "Testing SVMClient prediction..." << std::endl << std::flush;
	svm_config.data = helper_testing_data_01();
    svm_config.setPrediction(true);
    SVMClient *svm_client = new SVMClient(&svm_config);
    svm_client->predict(svm_config.data);
    SVMConfiguration client_config = svm_client->getConfiguration();

    for (int i = 0; i < 4; ++i) {
        ASSERT_DOUBLE_EQ(
            client_config.result[i], helper_testing_target_01()[i]);
    }
}

TEST(LibSVMRunnerTest, svm_options_01) {
    std::cout << "Learning..." << std::endl << std::flush;
	SVMConfiguration svm_config;
    arma::mat A = helper_learning_data_01();
    arma::mat b = helper_learning_target_01();
	svm_config.setPrediction(false); // training model
    svm_config.svm_options = "-c 0.24";
	svm_config.data = A;
	svm_config.target = b;
	LibSVMRunner lib_svm_runner;
	lib_svm_runner.processRequest(svm_config);

    std::cout << "Testing SVMClient prediction..." << std::endl << std::flush;
	svm_config.data = helper_testing_data_01();
    svm_config.setPrediction(true);
    SVMClient *svm_client = new SVMClient(&svm_config);
    svm_client->predict(svm_config.data);
    SVMConfiguration client_config = svm_client->getConfiguration();

    for (int i = 0; i < 4; ++i) {
        // NOTE: `svm-predict` after `svm-train -c 0.24` predicts 1
        // (incorrectly) for all of this data
        ASSERT_DOUBLE_EQ(client_config.result[i], 1);
    }
}


#include <boost/graph/edmunds_karp_max_flow.hpp>


#ifdef compile_instructions
(echo "#include\""$0"\"" > $0x.cpp) && time clang++ -O3 -std=c++14 -Wfatal-errors -Wall -D_TEST_MULTI_INDEX_RANGE $0x.cpp -o $0x.x && $0x.x $@ && rm -rf $0x.cpp $0x.x; exit
#endif

#ifndef MULTI_INDEX_RANGE_HPP
#define MULTI_INDEX_RANGE_HPP

//#include<boost/iterator/iterator_facade.hpp>

#include<iterator> // std::random_iterator_tag // std::reverse_iterator

namespace boost{

namespace multi{

template<class Self, typename ValueType, class AccessCategory, typename Reference = ValueType&,  typename DifferenceType = typename std::pointer_traits<ValueType*>::difference_type, typename Pointer = ValueType*>
class iterator_facade{
	using self_type = Self;
	self_type& self(){return *this;}
	self_type const& self() const{return static_cast<Self const&>(*this);}
public:
	using value_type = ValueType;
	using reference = Reference;
	using pointer = Pointer;
	using difference_type = DifferenceType;
	using iterator_category = AccessCategory;
	auto operator!=(self_type const& o) const
//	->decltype(not(o == self()))
	{	return not(o == self());}
//	Self& operator++(){return ++self(); return *this;}
	self_type operator+(difference_type n) const{self_type r = self(); r += n; return r;}
	self_type operator-(difference_type n) const{self_type r = self(); r -= n; return r;}
};

//class iterator_core_access{};
}

namespace multi{

template<class IndexType>
class range{
	IndexType first_;
	IndexType last_;
public:
	using value_type = IndexType;
	using difference_type = std::make_signed_t<value_type>;
	using size_type = difference_type;
	using const_reference = value_type const /*&*/;
	using reference = const_reference;
	using const_pointer = value_type;
	using pointer = value_type;
	range() : first_{}, last_{first_}{}
	template<class Range, typename = std::enable_if_t<std::is_same<std::decay_t<Range>, value_type>{}> >
	constexpr range(Range&& o) : first_(o.first()), last_(o.last()){}
	constexpr range(value_type fl) : first_{fl}, last_{fl + 1}{}
	constexpr range(value_type f, value_type l) : first_{f}, last_{l}{}
	class const_iterator
		: public boost::multi::iterator_facade<const_iterator,
			value_type, std::random_access_iterator_tag,
			const_reference, difference_type
		>
	{
		typename const_iterator::value_type curr_;
		typename const_iterator::reference dereference() const{return curr_;}
		void increment(){++curr_;} void decrement(){--curr_;}
		void advance(typename const_iterator::difference_type n){curr_+=n;}
		bool equal(const_iterator const& y) const{return curr_ == y.curr_;}
		auto distance_to(const_iterator const& z) const{return z.curr_-curr_;}
		constexpr const_iterator(value_type current) : curr_(current){}
		friend class range;
	 //   friend class boost::iterator_core_access;
	public:
		auto operator==(const_iterator const& y) const{return curr_ == y.curr_;}
		const_iterator& operator++(){++curr_; return *this;}
		const_iterator& operator--(){--curr_; return *this;}
		typename const_iterator::reference operator*() const{return curr_;}
	};
	using iterator = const_iterator;
	using reverse_iterator = std::reverse_iterator<iterator>;
	using const_reverse_iterator = std::reverse_iterator<const_iterator>;
	constexpr const_reference first() const{return first_;}
	constexpr const_reference last()  const{return last_;}
	constexpr const_reference operator[](difference_type p) const{return first() + p;}
	constexpr const_reference front() const{return first();}
	constexpr const_reference back()  const{return last() - 1;}
	constexpr const_iterator cbegin() const{return const_iterator{first_};}
	constexpr const_iterator cend()   const{return const_iterator{last_};}
	constexpr reverse_iterator rbegin() const{return reverse_iterator{end()};}
	constexpr reverse_iterator rend() const{return reverse_iterator{begin()};}
	constexpr const_iterator begin() const{return cbegin();}
	constexpr const_iterator end() const{return cend();}
	constexpr bool empty() const{return first_ == last_;}
	friend constexpr bool empty(range const& s){return s.empty();}
	constexpr size_type size() const noexcept{return last_ - first_;}
	friend constexpr size_type size(range const& s){return s.size();}
	friend std::ostream& operator<<(std::ostream& os, range const& s){
		return s.empty()?os<<"[)":os <<"["<< s.first() <<", "<< s.last() <<")";
	}
	friend const_iterator begin(range const& self){return self.begin();}
	friend const_iterator end(range const& self){return self.end();}
	range& operator=(range const&) = default;
	friend constexpr bool operator==(range const& a, range const& b){
		return(a.empty()&& b.empty())||(a.first_==b.first_ && a.last_==b.last_);
	}
	friend constexpr
	bool operator!=(range const& r1, range const& r2){return not(r1 == r2);}
	size_type count(value_type const& value) const{
		if(value >= last_ or value < first_) return 0;
		return 1;
	}
	range::const_iterator find(value_type const& value) const{
		auto first = begin();
		if(value >= last_ or value < first_) return end();
		return first += value - *first;
	}
	friend range intersection(range const& r1, range const& r2){
		using std::max; using std::min;
		auto first = max(r1.first(), r2.first());
		auto last = min(r1.last(), r2.last());
		if(first < last) return {first, last};
		return {};
	}
	bool contains(value_type const& v) const{return (v >= first() and v < last())?true:false;}
};

//using index_range = range<index>;
/*
class strided_index_range : index_range{
	index stride_;
public:
	strided_index_range(index first, index last, index stride = 1) : index_range{first, last}, stride_{stride}{}
//	explicit operator index_range() const{return *this;}
	index stride() const{return stride_;}
	using index_range::front;
	index back() const{return front() + size()*stride();}
	size_type size() const{return (this->last_ - first_) / stride_;}
	friend std::ostream& operator<<(std::ostream& os, strided_index_range const& self){
		if(empty()
		if(self.first_ == self.last_) return os << "[)" << '\n';
		return os << '[' << self.first_ << ", " << self.last_ << ')';
	}
};*/

template<class IndexType>
class extension_t : public range<IndexType>{
	using range<IndexType>::range;
	public:
	constexpr extension_t() noexcept : range<IndexType>(0, 0){}
	constexpr extension_t(typename extension_t::value_type last) noexcept : range<IndexType>(0, last){}
	friend constexpr typename extension_t::size_type size(extension_t const& s){return s.size();}
	friend std::ostream& operator<<(std::ostream& os, extension_t const& self){
		if(self.empty()) return os << static_cast<range<IndexType> const&>(self);
		if(self.first() == 0) return os <<"["<< self.last() <<"]";
		return os << static_cast<range<IndexType> const&>(self);
	}
	friend bool operator==(extension_t const& a, extension_t const& b){return static_cast<range<IndexType> const&>(a)==static_cast<range<IndexType> const&>(b);}
	friend bool operator!=(extension_t const& a, extension_t const& b){return not(a==b);}
};

}}

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

#ifdef _TEST_MULTI_INDEX_RANGE

//#include <boost/spirit/include/karma.hpp>

#include<cassert>
#include<iostream>

namespace multi = boost::multi;

using std::cout;
using std::cerr;

int main(){

	cout << multi::range<int>{5, 5} <<'\n';
	cout << multi::extension_t<int>{5} <<'\n';
	multi::extension_t<int> ee{5};
	assert( ee == 5 );
	assert( multi::extension_t<int>{5} == 5 );
	cout << multi::extension_t<int>{5, 7} <<'\n';

	assert(( multi::extension_t<int>{5, 12}.count(10) == 1 ));

	assert( size(multi::range<int>{5, 5}) == 0 );
	assert( empty(multi::range<int>{5, 5}) );

	assert( size(multi::range<int>{}) == 0 );
	assert( empty(multi::range<int>{}) );

	for(auto const& i : multi::range<int>{5, 12}) cout<< i <<' ';
	cout <<'\n';

	cout << intersection(multi::range<int>{5, 12}, multi::range<int>{14, 16}) << '\n';
//	for(auto const& i : intersection(multi::range<int>{5, 12}, multi::range<int>{8, 16})) cout<< i <<' ';
//	cout <<'\n';

	multi::range<int> rr{5, 12};
	assert( rr.contains(6) );
	assert( not rr.contains(12) );
	for(auto it = rr.begin(); it != rr.end(); ++it) cout<< *it <<' ';
	cout<<'\n';
	for(auto it = rr.rbegin(); it != rr.rend(); ++it) cout<< *it <<' ';
	cout<<'\n';

//	cout<< *rr.rbegin() <<'\n';
//	for(auto it = rr.rbegin(); it != rr.rend(); ++it) cout<< *it <<' ';
//	cout <<'\n';

//	multi::extension<int> ei{5, 10};
#if 0
	std::iterator_traits<multi::range<multi::index>::const_iterator>::value_type p = multi::index{4};

	{
		multi::index_range ir{5, 10};
		cout << ir << " = {" << format(index_ % ", ", ir) << "}\n";
		std::vector<multi::index_range::value_type> v(5);
		copy(begin(ir), end(ir), begin(v));
		assert(v[0] == 5);
		for(auto& i : ir) cout << i << ' ';
		cout << '\n';
		auto f = ir.find(6);
		cerr << "*f " << *f << '\n';
		assert(*f == 6);
		using std::find;
		auto f2 = find(ir.begin(), ir.end(), 12);
		assert(f2 == ir.end());
		auto f3 = find(ir.begin(), ir.end(), 2);
		assert(f3 == ir.end());
	}
/*	{
		multi::strided_index_range ir{6, 12, 2};
		cout << ir << " = {" << format(index_ % ", ", ir) << "}\n";
		std::vector<multi::index_range::value_type> v(5);
		copy(begin(ir), end(ir), begin(v));
		assert( v[0] == 6 );
		assert( v[1] == 8 );
		for(auto& i : ir) cout << i <<' ';
		cout <<'\n';
	}*/
	{
		multi::index_range ir(5);
		cout << ir << " = {" << format(index_ % ", ", ir) << "}\n";
		assert(*begin(ir) == 5);
		assert(ir.front() == 5);
		assert(ir.back() == 5);
	}
	{
		multi::index_range ir; // partially formed
		ir = multi::index_range{8, 8};
		assert(ir.empty());
	}
	{
		multi::index_range ir = {};
		assert(ir.empty());
	}
	{
		multi::index_extension ie(5);
		cout << ie << " = {" << format(index_ % ", ", ie) << "}";
	}
#endif
}

#endif
#endif