/*****
* This is a C++ implementation of the paper "A Simple Optimistic skip-list Algorithm"
*   written by Maurice Herlihy, Yossi Lev, Victor Luchangco, Nir Shavit.
*   http://www.cs.bgu.ac.il/~mpam092/wiki.files/LazySkipList.pdf
*   Author: Kumagi
*/
#ifndef IZENELIB_AM_CONCURRENT_SKIPLIST_HPP
#define IZENELIB_AM_CONCURRENT_SKIPLIST_HPP
#include "node.hpp"
#include <boost/foreach.hpp>
#include <boost/random.hpp>
#include <boost/weak_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/optional.hpp>
#include <utility>

namespace izenelib{ namespace am { namespace concurrent {
template <typename T>
void nothing_deleter(const T*)
{
    //std::cerr << "good morning all!" << std::endl;
} // do nothing

template <typename key,typename value, int height = 8>
class SkipList
{
    typedef SkipList<key,value,height> skipList_t;
    typedef node<key,value> node_t;
    typedef boost::shared_ptr<node_t> shared_node;
    typedef boost::weak_ptr<node_t> weak_node;
    typedef boost::array<node_t*, height> ptr_node_array;
    typedef boost::array<weak_node, height> weak_node_array;
    typedef boost::array<shared_node, height> locked_node_array;

    typedef std::pair<weak_node_array,weak_node_array> nodelists;
    typedef typename node_t::scoped_lock scoped_lock;

    typedef boost::shared_ptr<scoped_lock> scoped_lock_ptr;
    typedef boost::optional<typename node_t::scoped_lock> optional_lock;

public:
    class iterator
    {
        shared_node node;
    public:
        iterator() {}
        explicit iterator(shared_node& s):node(s) {}
        node_t& operator*()
        {
            return *node;
        }
        node_t* operator->()
        {
            return &operator*();
        }
        const node_t& operator*() const
        {
            return *node;
        }
        const node_t* operator->() const
        {
            return &operator*();
        }
        bool operator==(const iterator& rhs)const
        {
            return node.get() == rhs.node.get();
        }
        bool operator!=(const iterator& rhs)const
        {
            return !operator==(rhs);
        }
        shared_node& get()
        {
            return node;
        }
        const shared_node& get()const
        {
            return node;
        }
        iterator& operator++()
        {
            while(true)
            {
                if(node->next[0].get() != NULL)
                {
                    node = node->next[0];
                }
                if(node->marked) continue;
                break;
            }
            return *this;
        }
    private:

//		iterator(const iterator&);
    };

private:
    node_t head;
    iterator shared_head, shared_tail;
    boost::mt19937 engine;
    boost::uniform_smallint<> range;
    mutable boost::variate_generator<boost::mt19937&, boost::uniform_smallint<> > rand;
public:
    SkipList(const key& min, const key& max, int randomseed = 0)
        :head(min ,value(), height)
        ,engine(static_cast<unsigned long>(randomseed))
        ,range(0,(1<<height) -1),rand(engine,range)
    {
        shared_node head_node(&head,nothing_deleter<node_t>);
        shared_node tail_node(new node_t(max,value(),0));
        iterator head_iter(head_node);
        iterator tail_iter(tail_node);
        shared_head = head_iter;
        shared_tail = tail_iter;
        shared_node& tail = shared_tail.get();
        for(int i=0; i<height; i++)
        {
            head.next[i] = tail;
        }
        head.fullylinked = true;
    }
    ~SkipList()
    {
    }
    bool contains(const key& k)
    {
        nodelists lists;
        const int lv = find(k, &lists);
        weak_node_array& succs = lists.second;
        if(lv == -1) return false;
        shared_node succ = succs[lv].lock();
        if(!succ) return false;
        return succ->fullylinked
               && !succ->marked;
    }

    iterator get(const key& k)
    {
        nodelists lists;
        const int lv = find(k, &lists);
        weak_node_array& succs = lists.second;
        if(lv == -1) return end();
        shared_node succ = succs[lv].lock();
        if(!succ) return end();
        if(succ->fullylinked && !succ->marked)
        {
            return iterator(succ);
        }
        else
        {
            return end();
        }
    }

    iterator lower_bound(const key& k)
    {
        nodelists lists;
        const int lv = find(k, &lists);
        weak_node_array& currs = lists.first;
        weak_node_array& succs = lists.second;
        shared_node curr = currs[0].lock();
        if(lv == -1) return iterator(curr);
        shared_node succ = succs[lv].lock();
        if(!succ) return end();
        if(succ->fullylinked && !succ->marked)
        {
            return iterator(succ);
        }
        else
        {
            shared_node curr = currs[0].lock();
            return iterator(curr);
        }
    }

    bool add(const key& k, const value& v)
    {
        const int top_layer = random_level();
        assert(top_layer < height);
        nodelists lists;
        std::auto_ptr<node_t> newnode(new node_t(k, v, top_layer));
        while(true)
        {
            const int lv = find(k, &lists);
            weak_node_array& preds = lists.first;
            weak_node_array& succs = lists.second;

            if(lv != -1)
            {
                const node_t* found = succs[lv].lock().get();
                if(!found) continue;
                if(!found->marked)
                {
                    while(!found->fullylinked)
                    {
                        ;
                    }
                    return false;
                }
                usleep(1);
                continue;
            }
            bool valid = true;

            // get shared_ptr from weak_ptr array
            locked_node_array locked_preds;
            locked_node_array locked_succs;
            for(int i=0; i<=top_layer; ++i)
            {
                locked_preds[i] = preds[i].lock();
                locked_succs[i] = succs[i].lock();
                if(!locked_preds[i] || !locked_succs[i])
                {
                    valid = false;
                    break;
                }
            }
            if(!valid)
            {
                continue;
            }

            node_t *prev_pred = NULL;
            std::vector<scoped_lock_ptr> pred_locks(top_layer+1);
            for(int layer = 0; valid && (layer <= top_layer); ++layer)
            {
                node_t* pred = locked_preds[layer].get();
                const node_t* succ = locked_succs[layer].get();

                if(pred != prev_pred)
                {
                    pred_locks[layer] = scoped_lock_ptr(new scoped_lock(pred->guard));
                    prev_pred = pred;
                }
                assert(pred);
                assert(succ);
                valid = !pred->marked
                        && !succ->marked
                        && pred->next[layer].get() == succ;

            }
            if(!valid)
            {
//				std::cerr << "unlock \n";
                continue; // unlock all
            }

            // start to insert
            for(int i = 0; i<=top_layer; ++i)
            {
                newnode->next[i] = shared_node(succs[i]);
                //std::cerr << "[" << newnode->next[i] << "]f";
            }

            shared_node newnode_insert(newnode.get());
            newnode.release(); // it's all reason why I use auto_ptr
            for(int layer = 0; layer <= top_layer; ++layer)
            {
                locked_preds[layer]->next[layer] = newnode_insert;
            }
            newnode_insert->fullylinked = true;
            return true;
        }
        assert(!"never reach");
    }

    iterator& begin()
    {
        return shared_head;
    }
    const iterator& begin()const
    {
        return shared_head;
    }
    iterator& end()
    {
        return shared_tail;
    }
    const iterator& end()const
    {
        return shared_tail;
    }

    bool remove(const key& k)
    {
        nodelists lists;
        bool is_marked = false;
        int top_layer = -1;
        shared_node victim;
        assert(victim.get() == NULL);
        while(true)
        {

            int lv = find(k, &lists);
            weak_node_array& preds = lists.first;
            weak_node_array& succs = lists.second;

            if(lv != -1)
            {
                victim = succs[lv].lock();
                if(!victim) return false;
            }
            if(is_marked ||
                    (lv != -1 && (victim->fullylinked
                                  && victim->top_layer == lv
                                  && !victim->marked)))
            {
                if(is_marked == false)  // only one time done
                {
                    top_layer = victim->top_layer;
                    victim->guard.lock(); // lock
                    if(victim->marked)
                    {
                        victim->guard.unlock();
                        return false;
                    }
                    victim->marked = true;
                    is_marked = true;
                }

                bool valid = true;
                // get shared_ptr from weak_ptr array
                locked_node_array locked_preds;
                locked_node_array locked_succs;
                for(int i=0; i<=top_layer; ++i)
                {
                    locked_preds[i] = preds[i].lock();
                    locked_succs[i] = succs[i].lock();
                    if(!locked_preds[i] || !locked_succs[i])
                    {
                        valid = false;
                        break;
                    }
                }
                if(!valid)
                {
                    continue;
                }

                node_t *prev_pred = NULL;
                std::vector<scoped_lock_ptr> pred_locks(top_layer+1);
                for(int layer = 0; valid && (layer <= top_layer); ++layer)
                {
                    node_t *pred = locked_preds[layer].get();
                    const node_t *succ = locked_succs[layer].get();
                    if(pred != prev_pred)
                    {
                        pred_locks[layer] = scoped_lock_ptr(new scoped_lock(pred->guard));
                        prev_pred = pred;
                    }
                    valid = !pred->marked && pred->next[layer].get() == succ;
                }
                if(!valid)
                {
                    continue;
                }

                for(int layer = top_layer; layer>=0; --layer)
                {
                    locked_preds[layer]->next[layer] = victim->next[layer];
                }
                victim->guard.unlock();
                return true;
            }
            else
                return false;
        }
    }

    int find(const key& target, nodelists* lists)
    {
        int found = -1;

        shared_node pred = shared_head.get();
        shared_node curr;
        *lists = nodelists();
        //lists->second.resize(height);
        for(int lv = height-1; lv >= 0; --lv)
        {
            curr = pred->next[lv];
            while(curr->first < target)
            {
                pred = curr;
                curr = pred->next[lv];
            }
            if(found == -1 && target == curr->first)
            {
                found = lv;
            }
            lists->first[lv] = weak_node(pred);
            lists->second[lv] = weak_node(curr);
        }

        return found;
    }

    void dump()const
    {
        const node_t* p = &head;
        while(p != NULL)
        {
            p->dump();
            p = p->next[0].get();
            std::cerr << std::endl;
        }
    }

    bool is_empty()const
    {
        return head.next[0].get() == end().get().get();
    }

    void clear()
    {
        while(shared_head->next[0] != shared_tail.get())
        {
            for(int i=0; i<height; i++)
            {
                shared_tail->next[i] = shared_tail->next[0]->next[0];
            }
        }
    }
public:
    uint32_t random_level()const
    {
        const uint32_t gen = rand();
        int bit = 1;
        int cnt=0;
        while(cnt < height-1)
        {
            if(gen & bit)
            {
                return cnt;
            }
            bit <<= 1;
            ++cnt;
        }
        return cnt;
    }
private:
    SkipList();
    SkipList(const skipList_t&);
    skipList_t& operator=(const skipList_t&);
};

}}}

#endif


#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/correspondence.h>

#include <pcl/features/normal_3d_omp.h>   //法线
#include <pcl/features/shot_omp.h>

#include <pcl/features/3dsc.h>     //3dsc
#include <pcl/features/board.h>
#include <pcl/features/feature.h>

//#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/uniform_sampling.h>
//#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/keypoints/iss_3d.h>

#include <pcl/recognition/cg/hough_3d.h>
#include <pcl/recognition/cg/geometric_consistency.h>

#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>

#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>

// ensaio de acerto de resolução
#include <iostream>

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <boost/lexical_cast.hpp>

#include <pcl/point_types.h>
#include <pcl/impl/point_types.hpp>

#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>

#include <sstream>
#include <pcl/visualization/cloud_viewer.h>
// tempo
#include <ctime>
//conversão integer to string
#include <boost/lexical_cast.hpp>

/*
Eigen::Affine3f A;
Eigen::Matrix4f M;
M = A.matrix();
A = M;   // assume that M.row(3) == [0 0 0 1]
*/

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis (pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
/*
  Eigen::Affine3f f;
  f>>"-0.06,0.04,0.04";
*/
  Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(-0.06f, 0.4f,0.4f);

  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
  viewer->addCoordinateSystem (1.0,sensorPose);
  viewer->initCameraParameters ();
  return (viewer);
}

typedef pcl::PointXYZ PointType;
typedef pcl::PointWithViewpoint PointTypeViewPoint;
typedef pcl::Normal NormalType;
typedef pcl::ReferenceFrame RFType;
typedef pcl::SHOT352 DescriptorType;
typedef pcl::ShapeContext1980 DescriptorType1;
//typedef pcl::ReferenaceFrame RFType;

std::string model_filename_;

double
computeCloudResolution (const pcl::PointCloud<PointType>::ConstPtr &cloud)
{
  double res = 0.0;
  int n_points = 0;
  int nres;

  std::vector<int> indices (2);
  std::vector<float> sqr_distances (2);
 //  pcl::search::KdTree<PointType> tree;
    pcl::KdTreeFLANN<PointType> tree;
  tree.setInputCloud (cloud);

  for (size_t i = 0; i < cloud->size (); ++i)
  {
    if (! pcl_isfinite ((*cloud)[i].x))//判断点的x的值是否位无效点也就是NAN点   如果是无效点就不会计算
    {
      continue;
    }
    //Considering the second neighbor since the first is the point itself.
  //int pcl::search::Search<PointT>::nearSearch();是计算给定点的k个邻域的点
   //i 代表索引点，2是寻找周围的点树（其中第一个点是它本身）indices寻找到的周围点的索引sqr_distances与相邻点的平方距离
    nres = tree.nearestKSearch (i, 2, indices, sqr_distances);  //返回寻找到的点数
    if (nres == 2)
    {
      res += sqrt (sqr_distances[1]);
      ++n_points;
    }
  }
  if (n_points != 0)
  {
    res /= n_points;
  }
  return res;
}

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

  std::vector<int> filenames;
  filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
  if (filenames.size () != 1)
  {
    std::cout << "Filenames missing.\n";
    std::cout <<"you  shoud input a cloud point with .pcd file"<<std::endl;
    exit (-1);
  }

  model_filename_ = argv[filenames[0]];
  scene_filename_ = argv[filenames[1]];

  pcl::PointCloud<PointTypeViewPoint>::Ptr modelviewpoint(new pcl::PointCloud<PointTypeViewPoint> ());
  pcl::PointCloud<PointType>::Ptr model (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr model_keypoints(new pcl::PointCloud<PointType> ());
  pcl::PointCloud<NormalType>::Ptr model_normals (new pcl::PointCloud<NormalType> ());
  pcl::PointCloud<NormalType>::Ptr model_keypoints_normals (new pcl::PointCloud<NormalType> ());

  pcl::PointCloud<DescriptorType>::Ptr model_descriptors (new pcl::PointCloud<DescriptorType> ());
  pcl::PointCloud<DescriptorType>::Ptr scene_descriptors (new pcl::PointCloud<DescriptorType> ());

  pcl::PointCloud<PointType>::Ptr scene (new pcl::PointCloud<PointType> ());
  pcl::PointCloud<PointType>::Ptr scene_keypoints (new pcl::PointCloud<PointType> ());

  pcl::PointCloud<NormalType>::Ptr scene_normals (new pcl::PointCloud<NormalType> ());

 if(pcl::io::loadPCDFile (model_filename_, *model)<0)
   {
  std::cout<<"Error loading model point ."<<std::endl;
  return (-1);
  }

 if(pcl::io::loadPCDFile (scene_filename_,*scene)<0)
   {
   std::cout<<"Error laoding scene cloud. "<<std::endl;
   return (-1);
  }
/*
  float vpx = modelviewpoint->points[0].vp_x;
   std::cout<<"vpx : "<<vpx <<std::endl;
  float vpy = modelviewpoint->points[0].vp_y;
   std::cout<<"vpy : "<<vpy <<std::endl;
  float vpz = modelviewpoint->points[0].vp_z;
     std::cout<<"vpz : "<<vpz <<std::endl;

  pcl::copyPointCloud<PointTypeViewPoint,PointType>(*modelviewpoint, *model);
  */
  std::cout<<"before the uniform_samping the points has: "<<model->points.size()<<std::endl;
  double res_model = computeCloudResolution(model);
  std::cout << "Model resolution:  " << res_model << std::endl;

  //compute Normal
 pcl::NormalEstimationOMP<PointType,NormalType> norm_est;
 norm_est.setKSearch(10);
// norm_est.setRadiusSearch(0.002)
 norm_est.setInputCloud(model);
 //norm_est.setViewPoint(vpx,vpy,vpz);
 norm_est.compute(*model_normals);

 norm_est.setInputCloud(scene);
 norm_est.compute (*scene_normal);

//Downsample cloud to extraxt keypoints
  pcl::UniformSampling<PointType> uniform_sampling;
  uniform_sampling.setInputCloud(model);
  uniform_sampling.setRadiusSearch(0.01*res_model);
  //pcl::PointCloud<int> keypointIndices1;
  uniform_sampling.filter(*model_keypoints);
std::cout<<"after the uniform_samping the mdoel keypoint points has: "<<model_keypoints->points.size()<<std::endl;

  uniform_sampling.setInputCloud (scene);
  uniform_sampling.setRadiusSearch(0.03*res_model);
  uniform_sampling.filter(*scene_keypoints);
std::cout<<"after the uniform_samping the scene keypoint points has: "<<scene_keypoints->points.size()<<std::endl;

//compute Decsriptor for keypoint
 pcl::SHOTEstimationOMP<PointType,NormalType,DescriptorType> descr_est;
 descr_est.setRadiusSearch(0.02*res_model);

 descr_est.setInputCloud(model_keypoints);
 descr_est.setInputNormals(model_normals);
 descr_est.setSearchSurface (model);
 descr_est.compute(*model_descriptor);

 descr_est.setInputCloud(scene_keypoints);
 descr_est.setInputNormals(scene_normals);
 descr_est.setSearchSurface(scene);
 descr_esr.compute(*scene_descriptors);

//find model   and  scene correspondence with kdtree
 pcl::CorrespondencePtr model_scene_corrs(new pcl::Correspondence());
 pcl::KdtreeFLANN<DescriptorType> match_search;
 match_search.setInputCloud(model_descriptors);

for (size_t i=0;i<scene_descriptors->size ();++i)
 {
  std::vector<int> neigh_indices(1);
  std::vector<float> neigh_sqr_dists (1);
   if(!pcl_isfinite (scene_desciptor->at (i).descriptors[0]))
  {
   continue;
  }

  int found_neighs = match_search.nearestKSearch (scene_descriptors-> at(i),1,neigh_indices,neigh_sqr_dists);
  if(found_neighs = 1&& neigh_sqr_dists[0]< 0.25)
    {
    pcl::Correspondence corr(neigh_indices[0],static_cast<int>(i),neigh_sqr_dists[0]);
  model_scene_corrs->push_back(corr);
   }
 }
std::cout << "Correspondences found: " << model_scene_corrs->size () << std::endl;

//actual Clustering
 std::vector<Eigen::Matrix4f,EIgen::aligned_allocator<EIgen::Matrix4f>> rototranslations;
 std::vector<pcl::Correspondences> clustered_corrs;

 //use Hough  compute (keypoints) reference Frame only for Hough

pcl::PointCloud<RFType>::Ptr model_rf (new pcl::PointCloud<RFType> ());
pcl::PointCloud<RFType>::Ptr scene_rf (new pcl::PointCloud<RFType> ());

pcl::BOARDLocalReferenceFrameEstimation<PointType,NormalType,RFType> rf_est;
 rf_est.setFindHoles(true);
 rf_est.setRadiusSearch (0.015* res_model);

 rf_est.setInputCloud(model_keypoints);
 rf_est.setInputNormals (model_normal);
 rf_est.setSearchSurface (model);
 rf_est.compute (*model_rf);

 rf_est.setInputCloud(scene_keypoints);
 rf_est.setInputNormals (scene_normal);
 rf_est.setSearchSurface (scene);
 rf_est.compute (*scene_rf);

//clustering
pcl::Hough3DGrouping<PointType,PointType,RFType,RFType> clusterer;
clusterer.setHoughBinSize(0.01*res_model);
clusterer.setHoughThreshold(5.0);

    gc_clusterer.setInputCloud (model_keypoints);
    gc_clusterer.setSceneCloud (scene_keypoints);
    gc_clusterer.setModelSceneCorrespondences (model_scene_corrs);

    //gc_clusterer.cluster (clustered_corrs);
    gc_clusterer.recognize (rototranslations, clustered_corrs);

   //output result
std::cout<<"MOdel instance found: " <<rototranslations.size()<<std::endl;
 for(size_t i =0; i<rototranslations.size();++i)
 {
  std::cout<<" \n  Intance "<< i+1 <<" : " std::endl;
  std::cout<<"        Correspondence belonging to this instance: "<<clustered_corrs[i].size() <<std::endl;

//print the rotation matrix and translation vector
  Eigen::Matrix3f rotation = rototranslation[i].block<3,3>(0,0);
  Eigen::Vector3f translation = rototranslations[i].block<3,1>(0,3);
    printf ("\n");
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
    printf ("        R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
    printf ("            | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
    printf ("\n");
    printf ("        t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));

  }

 //pcl::copyPointCloud(*model,keypointIndices1.points,*model_keypoints);
//*******************************************************************************//

////#include <pcl/visualization/cloud_viewer.h>////////////////////////

    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    //blocks until the cloud is actually rendered
    viewer.showCloud(model_keypoints);
/*
  pcl::PCDWriter writer;
 writer.write("cloud_out_vpxyz.pcd",*model);
*/
    //use the following functions to get access to the underlying more advanced/powerful
    //PCLVisualizer
/*
    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);

    //This will get called once per visualization iteration
    viewer.runOnVisualizationThread (viewerPsycho);
*/

    while (!viewer.wasStopped ())
    {
    //you can also do cool processing here
    //FIXME: Note that this is running in a separate thread from viewerPsycho
    //and you should guard against race conditions yourself...

    }
/*
 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1;
   viewer1 = simpleVis(model);
   viewer1->spin();
    return 0;
*/
//****************************************************************************//
//
/*
  boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
  viewer = simpleVis(model);
  viewer->spin();
   return 0;
*/
}


#include <boost/polygon/voronoi_geometry_type.hpp>


#include "linemod_detector_Tool.h"

#include <eigen3/Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include <iostream>
#include <boost/format.hpp>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/foreach.hpp>
#include <fstream>
#include <opencv2/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#define DEBUG
#include <idebug.h>

LinemodDetector::LinemodDetector(){
    icp_dist_min_ = 0.04f;
    px_match_min_ = 0.25;
    renderer_iterator_s = std::map <std::string, RendererIterator*>();
    initParam();
}

LinemodDetector::~LinemodDetector(){

}

void LinemodDetector::initParam(){

    //目标 点云距离
    th_obj_dist_ = 0.02f;//0.04
    verbose_ = true;

    static const int T_LVLS[] = {4, 8};
    std::vector< cv::Ptr<cv::linemod::Modality> > modalities;
    modalities.push_back(new cv::linemod::ColorGradient());
    modalities.push_back(new cv::linemod::DepthNormal());

    //Detector m默认构造函数 的话 第一个是形态 modalities 参数输入是彩色图或者是深度图 后面的是具体的形态的量化等级
    detector_ = new cv::linemod::Detector(modalities, std::vector<int>(T_LVLS, T_LVLS +2));
}

int LinemodDetector::loadData(const std::string path, const std::string &objectName, const std::string &type){

    IDebug("%s", "LinemodDetector loading data");
    if(Rs_.count(objectName)){
        IDebug("The %s object data alreadly loaded", objectName.c_str());
        return 0;
    }
    meshPath = path +"/mesh."+ type;
    std::string RsFileName= path + "/RS.txt";
    std::string TsFileName= path + "/TS.txt";
    std::string KsFileName= path + "/KS.txt";
    std::string DistancesFileName= path + "/distances.txt";
    std::string yamlConfigureName = path + "/settings.yaml";

    renderer_ = new Renderer3d(meshPath);
    renderer_->set_parameters(param_width_, param_height_,\
                              param_focal_length_x_, param_focal_length_y_, param_near_, param_far_);

    renderer_iterator_ = new RendererIterator(renderer_, param_n_points_);
    renderer_iterator_->angle_step_ = param_angle_step_;
    renderer_iterator_->radius_min_ = float(param_radius_min_);
    renderer_iterator_->radius_max_ = float(param_radius_max_);
    renderer_iterator_->radius_step_ = float(param_radius_step_);
    renderer_iterator_s.insert(std::pair<std::string, RendererIterator*>(objectName, renderer_iterator_));

    // 加载平移选择矩阵表
    std::vector<cv::Mat> Rs_tmp, Ts_tmp, Ks_tmp;

    loadVectorMat(RsFileName, Rs_tmp, 3, 3);
    loadVectorMat(TsFileName, Ts_tmp, 1, 3);
    loadVectorMat(KsFileName, Ks_tmp, 3, 3);

    //std::cout << "load Mat "<<std::endl;
    Rs_.insert(std::pair<std::string, std::vector<cv::Mat>>(objectName, Rs_tmp));
    Ts_.insert(std::pair<std::string, std::vector<cv::Mat>>(objectName, Ts_tmp));
    Ks_.insert(std::pair<std::string, std::vector<cv::Mat>>(objectName, Ks_tmp));

    // 加载距离表
    std::vector<float> distances_tmp;
    std::ifstream distancesIf(DistancesFileName);
    std::string disTmp;
    size_t disIndex = 0;
    while(distancesIf >> disTmp){
        distances_tmp.push_back(std::stof(disTmp));
        disIndex ++;
    }
    distances_.insert(std::pair<std::string, std::vector<float> >(objectName, distances_tmp));

    /**
    *   加载Detector
    */
    cv::linemod::Detector *detector = new cv::linemod::Detector();
    loadDetctor(path + "/detector.xml", detector);

    std::string object_id_in_db = detector->classIds()[0];
    for (size_t template_id = 0; template_id < detector->numTemplates();
         ++template_id) {
        //加载器 加载
        const std::vector<cv::linemod::Template> &templates_original = detector->getTemplates(object_id_in_db, template_id);
        //synthetic 合成的
        detector_->addSyntheticTemplate(templates_original, objectName);
    }

    if(detector_->classIds().empty())
        return -1;
    IDebug("%s", "LinemodDetector load  ok");
    return 0;
}

void LinemodDetector::loadVectorMat(std::string &fileName, std::vector<cv::Mat> &out, int rows, int cols){

    std::ifstream in(fileName);
    std::string tmpLine;
    std::vector<std::string> rss;

    while(std::getline(in, tmpLine, '|')){
        // 删除所有换行
        tmpLine = boost::algorithm::erase_all_copy(tmpLine, "\n");
        tmpLine.erase(0, 1);

        boost::split(rss, tmpLine, boost::is_any_of("  "));

        cv::Mat_<double> tmpMat_(rows,cols);

        for(int i = 0; i < rows; i ++)
            for(int j = 0; j < cols; j ++)
                tmpMat_(i,j) = std::stod(rss[i * cols + j].erase(rss[i * cols + j].length()-1));

        cv::Mat tmpMat = tmpMat_;

        out.push_back(tmpMat);
    }
    in.close();
}

void LinemodDetector::loadDetctor(const std::string file_name, cv::linemod::Detector *value){

    cv::FileStorage fs(file_name, cv::FileStorage::READ);
    value->read(fs.root());

    cv::FileNode fn = fs["classes"];
    for (cv::FileNodeIterator i = fn.begin(), iend = fn.end(); i != iend; ++i)
        value->readClass(*i);
}

int LinemodDetector::detection(){

    objs_.clear();
    poses.clear();

    IDebug("the pyramidLevels is %d ", detector_->pyramidLevels());
    IDebug("depth_ is %d ", depth_.depth());
    // 构建检测输入源
    std::vector<cv::Mat> sources;

    // 转换源深度图的格式
    cv::Mat depth;
    if (depth_.depth() == CV_32F)
        depth_.convertTo(depth, CV_16UC1, 1000.0);
    else if(depth_.depth() == CV_8U){
        depth_.convertTo(depth, CV_16UC1, 1000.0);
    } else if(depth_.depth() == CV_16U){
        depth = depth_;
    }

    cv::Mat color;
    if( color_.empty() ){
        IErrorPrint("%s","color_ non");
    }else{

    }

    color_.copyTo(color);
    // 将深度图作为检测源  不输入彩色图会出现 assert dept和dim >0
    IDebug("the color_ size is %d %d " , color.rows, color.cols);
    IDebug("the depth size is %d %d " , depth.rows, color_.cols);

    sources.push_back(color);
    sources.push_back(depth);

    /**
     * @brief matches 保存匹配成功的匹配对象
     */
    std::vector<cv::linemod::Match> matches;
    IDebug("%s","detector_->match ...............");
    detector_->match(sources, 81.0f, matches);
    IDebug("%s","detector_->match end........");
    IDebug("%s %d","detector size: ", matches.size());
    //将深度图转换为点云
    cv::Mat_<cv::Vec3f> depth_real_ref_raw;
    // cv::Mat depth_show;
    cv::Mat_<float> K, K_depth_;
    cv::Mat_<double> tmpMat_(3,3);
    //tmpMat_ <<  617.94, 0, 325.894, 0, 614.1937, 247.2789, 0, 0, 1;
    for( int i =0; i< tmpMat_.cols *tmpMat_.rows; i++){
        tmpMat_ << tmpMat_.at<float>(i);
    }

    K_depth_ = tmpMat_;
    K_depth_.convertTo(K, CV_32F);

#if CV_MAJOR_VERSION == 3
    std::cout << "depthTo3d : "<<depth.depth()<<" "<<depth.channels()<<std::endl;
    cv::rgbd::depthTo3d(depth, K, depth_real_ref_raw);
#else
    cv::depthTo3d(depth, K, depth_real_ref_raw);
#endif

    /**
     * @brief BOOST_FOREACH 遍历刚刚使用linemod模板匹配到所有匹配对象
     */
    BOOST_FOREACH(const cv::linemod::Match & match, matches){
         const std::vector<cv::linemod::Template>& templates =
                detector_->getTemplates(match.class_id, match.template_id);
        cv::Matx33d R_match;
        cv::Vec3d T_match;
        float D_match;
        cv::Mat K_match;

        R_match =  Rs_.at(match.class_id)[match.template_id].clone();
        T_match = Ts_.at(match.class_id)[match.template_id].clone();
        D_match = distances_.at(match.class_id)[match.template_id];
        K_match = Ks_.at(match.class_id)[match.template_id];
        //std::cout << match.x <<" "<< match.y<< " id :"<< match.template_id<<" "<<match.similarity<<std::endl;
        // cv::circle(color, cv::Point(match.x, match.y), 5,cv::Scalar(255,0,255));

        /**
         * 填充位姿，训练过程中，一个物体代表一个claas_id，因此在识别过程中detctor_只能识别一个物体
         * 而在识别过程中的，每渲染一张图片，代表的是一个template_id，因此，我们通过识别匹配中得到的template_id来对应训练过程中
         * 对应的渲染图片，由于渲染过程中，每个图片[template_id]的位姿都是已知的，所以我们就可以获取到当前匹配的位姿态
         */

        // cv::Matx33d R_match = Rs_.at(match.class_id)[match.template_id].clone();
        // cv::Vec3d T_match = Ts_.at(match.class_id)[match.template_id].clone();
        // float D_match = distances_.at(match.class_id)[match.template_id];
        // cv::Mat K_match = Ks_.at(match.class_id)[match.template_id];

        /**
         * 将通过模板匹配得到的粗略位姿 放到渲染器进行重新渲染，得到深度图，然后将得到的深度图转换为点云
         */
        cv::Mat mask ,rendorcolor;
        cv::Rect rect;
        cv::Matx33d R_temp(R_match.inv());
        cv::Vec3d up(-R_temp(0,1), -R_temp(1,1), -R_temp(2,1));
        cv::Mat depth_ref_;
        renderer_iterator_s.at(match.class_id)->renderDepthOnly(depth_ref_, mask, rect, -T_match, up);
        // renderer_iterator_s.at(match.class_id)->render(rendorcolor, depth_ref_, mask, rect, -T_match, up);
        // renderer_iterator_s.at(match.class_id)->renderImageOnly(renderColor,rectColor,tColor, upColor );
        // cv::imshow("renderColor",rendorcolor);
        // cv::waitKey(0);
        /**
         * @brief depth_real_model_raw  保存通过渲染器得到的深度图转换的点云
         */
        cv::Mat_<cv::Vec3f> depth_real_model_raw;
        cv::Mat renderK;
        K_match.convertTo(renderK, CV_32F);
        // IDebug("%s","depthTo3d ready ...............");
#if CV_MAJOR_VERSION == 3
        // try{
        cv::rgbd::depthTo3d(depth_ref_, renderK, depth_real_model_raw);
        // }catch(ex){
            // return -1;
        // }

#else
        cv::depthTo3d(depth_ref_, renderK, depth_real_model_raw);
#endif
        // cv::depthTo3d(depth_ref_, renderK, depth_real_model_raw);
        // IDebug("%s","depthTo3d ok ...............");
        /**
         * 生成点云和模型的包围矩形，<包围整个深度图>？
         */
        cv::Rect_<int> rect_model(0, 0, depth_real_model_raw.cols, depth_real_model_raw.rows);
        // 根据匹配得到的数据 修改包围矩形的X，Y
        cv::Rect_<int> rect_ref(rect_model);
        rect_ref.x += match.x;
        rect_ref.y += match.y;
        // IDebug("%s %d %d","rect_ref width heigh ", rect_ref.width, rect_ref.height);
        //cv::Mat testImg = color.clone();
        //imshow("testImg",testImg(rect_ref));
        //cv::waitKey(0);
        /**
         *  求图像深度图与rect_ref两个矩形的相交
         */
        rect_ref = rect_ref & cv::Rect(0, 0, depth_real_ref_raw.cols, depth_real_ref_raw.rows);

        // 当相交的区域宽小于5 或者长小于5时，跳过当前匹配
        if ((rect_ref.width < 5) || (rect_ref.height < 5))
            continue;

        // 得到两个矩形的最小
        if (rect_ref.width > rect_model.width)
            rect_ref.width = rect_model.width;
        if (rect_ref.height > rect_model.height)
            rect_ref.height = rect_model.height;
        if (rect_model.width > rect_ref.width)
            rect_model.width = rect_ref.width;
        if (rect_model.height > rect_ref.height)
            rect_model.height = rect_ref.height;

        //  全局点云
        cv::Mat_<cv::Vec3f> depth_real_ref = depth_real_ref_raw(rect_ref);
        // 局部 模板点云
        cv::Mat_<cv::Vec3f> depth_real_model = depth_real_model_raw(rect_model);

        // 获取裁剪过后的点云的中心点
        cv::Vec3f T_crop = depth_real_ref(depth_real_ref.rows / 2.0f, depth_real_ref.cols / 2.0f);
        // 加上物体的中心点到表面的距离
        T_crop(2) += D_match;

        // 检测得到的中心点坐标是否是正常的
        if (!cv::checkRange(T_crop))
            continue;

        //
        cv::Vec3f T_real_icp(T_crop);

        if (!cv::checkRange(R_match))
            continue;

        // 将匹配得到的模板用来获取训练时的旋转矩阵来当作icp算法的旋转矩阵
        cv::Matx33f R_real_icp(R_match);

        //  将之前生成的点云[mat对象]转为vector的点云对象
        std::vector<cv::Vec3f> pts_real_model_temp;
        std::vector<cv::Vec3f> pts_real_ref_temp;

        /**
         * @brief px_ratio_missing  传感器深度图转的点云比渲染得到的点云缺少的点云比
         */
        float px_ratio_missing = matToVec(depth_real_ref, depth_real_model,\
                                          pts_real_ref_temp, pts_real_model_temp);
        if (px_ratio_missing > (1.0f-px_match_min_))
            continue;

        // 执行第一次近似ICP
        float px_ratio_match_inliers = 0.0f;
        float icp_dist = icpCloudToCloud(pts_real_ref_temp, pts_real_model_temp,\
                                         R_real_icp, T_real_icp, px_ratio_match_inliers, 1);
        if (icp_dist > icp_dist_min_)
            continue;

        //执行最佳icp配准
        icp_dist = icpCloudToCloud(pts_real_ref_temp, pts_real_model_temp,\
                                   R_real_icp, T_real_icp, px_ratio_match_inliers, 2);

        // 保存相关的结果
        objs_.push_back(ObjData(pts_real_ref_temp, pts_real_model_temp, \
                                match.class_id, match.similarity, icp_dist, px_ratio_match_inliers, R_real_icp, T_crop));
    }

    //local non-maxima supression to find the best match at each position  非极大抑制
    int count_pass = 0;
    std::vector<ObjData>::iterator it_o = objs_.begin();
    for (; it_o != objs_.end(); ++it_o)
        if (!it_o->check_done)
        {

            //initialize the object to publishT_real_icp
            ObjData *o_match = &(*it_o);
            int size_th = static_cast<int>((float)o_match->pts_model.size()*0.85);
            //find the best object match among near objects
            std::vector <ObjData>::iterator it_o2 = it_o;
            ++it_o2;
            for (; it_o2 != objs_.end(); ++it_o2)
                if (!it_o2->check_done)
                    if (cv::norm(o_match->t, it_o2->t) < th_obj_dist_)
                    {
                        it_o2->check_done = true;
                        if ((it_o2->pts_model.size() > size_th) && (it_o2->icp_dist < o_match->icp_dist))
                            o_match = &(*it_o2);
                    }

            //perform the final precise icp
            float icp_px_match = 0.0f;
            float icp_dist = icpCloudToCloud(o_match->pts_ref, o_match->pts_model, o_match->r, o_match->t, icp_px_match, 0);

            if (verbose_)
                std::cout << o_match->match_class <<  " similar: " << o_match->match_sim << " icp_dist " << icp_dist << ", ";
            //icp_dist in the same units as the sensor data
            //this distance is used to compute the ratio of inliers (points laying within this distance between the point clouds)
            icp_dist = 0.007f; //0.007
            float px_inliers_ratio = getL2distClouds(o_match->pts_model, o_match->pts_ref, icp_dist);
            if (verbose_)
                std::cout << " ratio " << o_match->icp_px_match << " or " << px_inliers_ratio << std::endl;

            pose p;
            p.objectName = o_match->match_class;

            RT2Pose(o_match->r, o_match->t, p);
            this->poses.push_back(p);

            ++count_pass;
        }

    if (verbose_ && (matches.size()>0)){
        IDebug("%s","depthTo3d ok ...............");
        std::cout << "matches size: " << objs_.size() << " / " << count_pass << " / " << matches.size() << std::endl;
        return 0;
    }

    IErrorPrint("%s","nothing detection   ");
    return -1;

}

void LinemodDetector::RT2Pose(const cv::Matx33f &R, const cv::Vec3f &T, pose &pose){

    Eigen::Matrix3d t_R;
    cv::cv2eigen((cv::Matx33d)R, t_R);
    Eigen::Quaterniond q(t_R);
    Eigen::Vector4d q_tmp = q.coeffs();

    //pose.quaternion.x = q_tmp[0];
    //pose.quaternion.y = q_tmp[1];
    //pose.quaternion.z = q_tmp[2];
    //pose.quaternion.w = q_tmp[3];

    pose.quaternion.x = 0;
    pose.quaternion.y = 0;
    pose.quaternion.z = 0;
    pose.quaternion.w = 1;
    pose.position.x = T(0);
    pose.position.y = T(1);
    pose.position.z = T(2);

}

int LinemodDetector::readYamlData(const YAML::Node &node)
{
    try{
      this->param_n_points_ = node["linemod"]["param_n_points_"].as<int>();
      this->param_angle_step_ = node["linemod"]["param_angle_step_"].as<int>();
      this->param_radius_min_ = node["linemod"]["param_radius_min_"].as<float>();
      this->param_radius_max_ = node["linemod"]["param_radius_max_"].as<float>();
      this->param_radius_step_ = node["linemod"]["param_radius_step_"].as<float>();
      this->param_width_ = node["linemod"]["param_width_"].as<int>();
      this->param_height_ = node["linemod"]["param_height_"].as<int>();
      this->param_focal_length_x_ = node["linemod"]["param_focal_length_x_"].as<float>();
      this->param_focal_length_y_ = node["linemod"]["param_focal_length_y_"].as<float>();
      this->param_near_ = node["linemod"]["param_near_"].as<float>();
      this->param_far_ = node["linemod"]["param_far_"].as<int>();
      float temp3[9];
      this->tmpMat_ = cv::Mat(cv::Size(3,3), CV_32FC1);
      for(int i =0; i< 3;i++){
          for(int j =0; j< 3;j++){
          temp3[i*3+j]= node["linemod"]["tmpMat_"][i*3+j].as<float>();
          this->tmpMat_.at<float>(i,j) = temp3[i*3+j];
            }
        }
    }catch(YAML::Exception e)
    {
        std::cerr<<"Linemod YAML:" << e.msg<<std::endl;
        return -1;
    }

    return 0;
//      std::cout<<tmpMat_<<std::endl;

}

int LinemodDetector::getResult(std::vector<pose> &poses){
    // for( int i = 0; i < poses.size(); i++){
    //     Eigen::Quaterniond CubeRotationShow =  Eigen::Quaterniond(poses[i].quaternion.w, poses[i].quaternion.x,\
    //                                                           poses[i].quaternion.y,poses[i].quaternion.z);
    //     //"输出 Z-Y-X，即RPY  "
    //     Eigen::Vector3d eulerAngle= CubeRotationShow.matrix().eulerAngles(2,1,0);
    //     CubeRotationShow = Eigen::AngleAxisd(eulerAngle[0]-1.57, Eigen::Vector3d::UnitX()) *
    //             Eigen::AngleAxisd(eulerAngle[1], Eigen::Vector3d::UnitY()) *
    //             Eigen::AngleAxisd(eulerAngle[2], Eigen::Vector3d::UnitY());
    //     poses[i].quaternion.x = CubeRotationShow.x();
    //     poses[i].quaternion.y = CubeRotationShow.y();
    //     poses[i].quaternion.z = CubeRotationShow.z();
    //     poses[i].quaternion.x = CubeRotationShow.w();
    // }

    poses = this->poses;

    if(poses.empty())
        return -1;

    IDebug("x = %lf, y = %lf, z = %lf, qx = %lf, qy = %lf, qz = %lf, qw = %lf", \
           this->poses[0].position.x, this->poses[0].position.y, this->poses[0].position.z,\
            this->poses[0].quaternion.x, this->poses[0].quaternion.y, this->poses[0].quaternion.z, this->poses[0].quaternion.w);

    return 0;
}

void LinemodDetector::setDepthImg(const cv::Mat &inputImg){
    // 保存输入的图像
    if( inputImg.cols != param_width_ || param_height_ != inputImg.rows){
        cv::resize(inputImg, depth_, cv::Size(param_width_, param_height_));
    }else{
        depth_ = inputImg;
    }
}

void LinemodDetector::setColorImg(const cv::Mat &inputImg){
    // 保存输入的图像
    if( inputImg.cols != param_width_ || param_height_ != inputImg.rows){
        cv::resize(inputImg, color_, cv::Size(param_width_, param_height_));
    }else{
        color_ = inputImg;
    }

}

float LinemodDetector::icpCloudToCloud(const std::vector<cv::Vec3f> &pts_ref,\
                                       std::vector<cv::Vec3f> &pts_model, \
                                       cv::Matx33f &R, cv::Vec3f &T, float &px_inliers_ratio, int mode){

    //optimal rotation matrix
    cv::Matx33f R_optimal;
    //optimal transformation
    cv::Vec3f T_optimal;

    //the number of desired iterations defined depending on the mode
    int icp_it_th = 40; //maximal number of iterations 35
    if (mode == 1)
        icp_it_th = 10; //minimal number of iterations 4
    else if (mode == 2)
        icp_it_th = 4;

    // 两个点云之间所需的最小距离
    // const float dist_th = 0.012f;
    const float dist_th = 0.008f;
    // 两个点云距离之间的平均值
    float dist_mean = 0.0f;

    // 计算两个点云的欧式距离
    px_inliers_ratio = getL2distClouds(pts_model, pts_ref, dist_mean, mode);
    //The difference between two previously obtained mean distances between the reference and the model point clouds
    float dist_diff = std::numeric_limits<float>::max();

    //the number of performed iterations
    int iter = 0;
    while (( ((dist_mean > dist_th) && (dist_diff > 0.0001f)) || (mode == 1) ) && (iter < icp_it_th))
    {
        ++iter;

        //subsample points from the match and ref clouds
        if (pts_model.empty() || pts_ref.empty())
            continue;

        // 为每个点云计算中心点
        //compute centroids of each point subset
        cv::Vec3f m_centroid, r_centroid;
        getMean(pts_model, m_centroid);
        getMean(pts_ref, r_centroid);

        //compute the covariance matrix
        cv::Matx33f covariance (0,0,0, 0,0,0, 0,0,0);
        std::vector<cv::Vec3f>::iterator it_s = pts_model.begin();
        std::vector<cv::Vec3f>::const_iterator it_ref = pts_ref.begin();
        for (; it_s < pts_model.end(); ++it_s, ++it_ref)
            covariance += (*it_s) * (*it_ref).t();

        // 通过SVD求解最优解
        cv::Mat w, u, vt;
        cv::SVD::compute(covariance, w, u, vt);
        //compute the optimal rotation
        R_optimal = cv::Mat(vt.t() * u.t());

        //compute the optimal translation
        T_optimal = r_centroid - R_optimal * m_centroid;
        if (!cv::checkRange(R_optimal) || !cv::checkRange(T_optimal))
            continue;

        //transform the point cloud
        transformPoints(pts_model, pts_model, R_optimal, T_optimal);

        //compute the distance between the transformed and ref point clouds
        dist_diff = dist_mean;
        px_inliers_ratio = getL2distClouds(pts_model, pts_ref, dist_mean, mode);
        dist_diff -= dist_mean;

        //update the translation matrix: turn to opposite direction at first and then do translation
        T = R_optimal * T;
        //do translation
        cv::add(T, T_optimal, T);
        //update the rotation matrix
        R = R_optimal * R;
        //std::cout << " it " << iter << "/" << icp_it_th << " : " << std::fixed << dist_mean << " " << d_diff << " " << px_inliers_ratio << " " << pts_model.size() << std::endl;
    }

    return dist_mean;

}

/** Computes the L2 distance between two vectors of 3D points of the same size */
float LinemodDetector::getL2distClouds(const std::vector<cv::Vec3f> &model, const std::vector<cv::Vec3f> &ref, \
                                        float &dist_mean, const float mode)
{
    int nbr_inliers = 0;
    int counter = 0;
    float ratio_inliers = 0.0f;

    float dist_expected = dist_mean * 3.0f;
    dist_mean = 0.0f;

    //use the whole region
    std::vector<cv::Vec3f>::const_iterator it_match = model.begin();
    std::vector<cv::Vec3f>::const_iterator it_ref = ref.begin();
    for(; it_match != model.end(); ++it_match, ++it_ref)
    {
        if (!cv::checkRange(*it_ref))
            continue;

        if (cv::checkRange(*it_match))
        {
            float dist = cv::norm(*it_match - *it_ref);
            if ((dist < dist_expected) || (mode == 0))
                dist_mean += dist;
            if (dist < dist_expected)
                ++nbr_inliers;
        }
        ++counter;
    }

    if (counter > 0)
    {
        dist_mean /= float(nbr_inliers);
        ratio_inliers = float(nbr_inliers) / float(counter);
    }
    else
        dist_mean = std::numeric_limits<float>::max();

    return ratio_inliers;
}

void LinemodDetector::transformPoints(const std::vector<cv::Vec3f> &src, \
                                      std::vector<cv::Vec3f>& dst, const cv::Matx33f &R, const cv::Vec3f &T)
{
    std::vector<cv::Vec3f>::const_iterator it_src = src.begin();
    std::vector<cv::Vec3f>::iterator it_dst = dst.begin();
    for (; it_src != src.end(); ++it_src, ++it_dst) {
        if (!cv::checkRange(*it_src))
            continue;
        (*it_dst) = R * (*it_src) + T;
    }
}

void LinemodDetector::getMean(const std::vector<cv::Vec3f> &pts, cv::Vec3f& centroid)
{
    centroid = cv::Vec3f(0.0f, 0.0f, 0.0f);
    size_t n_points = 0;
    for (std::vector<cv::Vec3f>::const_iterator it = pts.begin(); it != pts.end(); ++it) {
        if (!cv::checkRange(*it))
            continue;
        centroid += (*it);
        ++n_points;
    }

    if (n_points > 0)
    {
        centroid(0) /= float(n_points);
        centroid(1) /= float(n_points);
        centroid(2) /= float(n_points);
    }
}

float LinemodDetector::matToVec(const cv::Mat_<cv::Vec3f> &src_ref, const cv::Mat_<cv::Vec3f> &src_mod, \
                                std::vector<cv::Vec3f>& pts_ref, std::vector<cv::Vec3f>& pts_mod)
{
    pts_ref.clear();
    pts_mod.clear();
    int px_missing = 0;

    cv::MatConstIterator_<cv::Vec3f> it_ref = src_ref.begin();
    cv::MatConstIterator_<cv::Vec3f> it_mod = src_mod.begin();
    for (; it_ref != src_ref.end(); ++it_ref, ++it_mod)
    {
        if (!cv::checkRange(*it_ref))
            continue;

        pts_ref.push_back(*it_ref);
        if (cv::checkRange(*it_mod))
        {
            pts_mod.push_back(*it_mod);
        }
        else
        {
            pts_mod.push_back(cv::Vec3f(0.0f, 0.0f, 0.0f));
            ++px_missing;
        }
    }

    float ratio = 0.0f;
    if (pts_ref.size() > 0)
        ratio = static_cast<float>(px_missing) /static_cast<float>(pts_ref.size());
    return ratio;
}


//  (C) Copyright John maddock 1999.
//  (C) David Abrahams 2002.  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)
//
// use this header as a workaround for missing <limits>

//  See http://www.boost.org/libs/compatibility/index.html for documentation.

#ifndef BOOST_LIMITS
#define BOOST_LIMITS

#include <boost/config.hpp>

#ifdef BOOST_NO_LIMITS
# include <boost/detail/limits.hpp>
#else
# include <limits>
#endif

#include <boost/detail/extended_integer.hpp>  // for BOOST_HAS_XINT, etc.

#if (defined(BOOST_HAS_LONG_LONG) && defined(BOOST_NO_LONG_LONG_NUMERIC_LIMITS)) \
      || (defined(BOOST_HAS_MS_INT64) && defined(BOOST_NO_MS_INT64_NUMERIC_LIMITS))
// Add missing specializations for numeric_limits:
#if !defined(BOOST_HAS_XINT) || !(BOOST_HAS_XINT)
#error "Shouldn't have gotten here based on preceeding preprocessor statements"
#endif
#define BOOST_LLT    ::boost::detail::xint_t
#define BOOST_ULLT  ::boost::detail::uxint_t

#include <climits>  // for CHAR_BIT

namespace std
{
  template<>
  class numeric_limits< BOOST_LLT >
  {
   public:

      BOOST_STATIC_CONSTANT(bool, is_specialized = true);
      static BOOST_LLT min BOOST_PREVENT_MACRO_SUBSTITUTION (){ return BOOST_XINT_MIN; }
      static BOOST_LLT max BOOST_PREVENT_MACRO_SUBSTITUTION (){ return BOOST_XINT_MAX; }
      BOOST_STATIC_CONSTANT(int, digits = sizeof(BOOST_LLT) * CHAR_BIT -1);
      BOOST_STATIC_CONSTANT(int, digits10 = (CHAR_BIT * sizeof (BOOST_LLT) - 1) * 301L / 1000);
      BOOST_STATIC_CONSTANT(bool, is_signed = true);
      BOOST_STATIC_CONSTANT(bool, is_integer = true);
      BOOST_STATIC_CONSTANT(bool, is_exact = true);
      BOOST_STATIC_CONSTANT(int, radix = 2);
      static BOOST_LLT epsilon() throw() { return 0; };
      static BOOST_LLT round_error() throw() { return 0; };

      BOOST_STATIC_CONSTANT(int, min_exponent = 0);
      BOOST_STATIC_CONSTANT(int, min_exponent10 = 0);
      BOOST_STATIC_CONSTANT(int, max_exponent = 0);
      BOOST_STATIC_CONSTANT(int, max_exponent10 = 0);

      BOOST_STATIC_CONSTANT(bool, has_infinity = false);
      BOOST_STATIC_CONSTANT(bool, has_quiet_NaN = false);
      BOOST_STATIC_CONSTANT(bool, has_signaling_NaN = false);
      BOOST_STATIC_CONSTANT(bool, has_denorm = false);
      BOOST_STATIC_CONSTANT(bool, has_denorm_loss = false);
      static BOOST_LLT infinity() throw() { return 0; };
      static BOOST_LLT quiet_NaN() throw() { return 0; };
      static BOOST_LLT signaling_NaN() throw() { return 0; };
      static BOOST_LLT denorm_min() throw() { return 0; };

      BOOST_STATIC_CONSTANT(bool, is_iec559 = false);
      BOOST_STATIC_CONSTANT(bool, is_bounded = true);
      BOOST_STATIC_CONSTANT(bool, is_modulo = true);

      BOOST_STATIC_CONSTANT(bool, traps = false);
      BOOST_STATIC_CONSTANT(bool, tinyness_before = false);
      BOOST_STATIC_CONSTANT(float_round_style, round_style = round_toward_zero);

  };

  template<>
  class numeric_limits< BOOST_ULLT >
  {
   public:

      BOOST_STATIC_CONSTANT(bool, is_specialized = true);
      static BOOST_ULLT min BOOST_PREVENT_MACRO_SUBSTITUTION (){ return (BOOST_ULLT) 0u; }
      static BOOST_ULLT max BOOST_PREVENT_MACRO_SUBSTITUTION (){ return BOOST_UXINT_MAX; }
      BOOST_STATIC_CONSTANT(int, digits = sizeof(BOOST_LLT) * CHAR_BIT);
      BOOST_STATIC_CONSTANT(int, digits10 = (CHAR_BIT * sizeof (BOOST_LLT)) * 301L / 1000);
      BOOST_STATIC_CONSTANT(bool, is_signed = false);
      BOOST_STATIC_CONSTANT(bool, is_integer = true);
      BOOST_STATIC_CONSTANT(bool, is_exact = true);
      BOOST_STATIC_CONSTANT(int, radix = 2);
      static BOOST_ULLT epsilon() throw() { return 0; };
      static BOOST_ULLT round_error() throw() { return 0; };

      BOOST_STATIC_CONSTANT(int, min_exponent = 0);
      BOOST_STATIC_CONSTANT(int, min_exponent10 = 0);
      BOOST_STATIC_CONSTANT(int, max_exponent = 0);
      BOOST_STATIC_CONSTANT(int, max_exponent10 = 0);

      BOOST_STATIC_CONSTANT(bool, has_infinity = false);
      BOOST_STATIC_CONSTANT(bool, has_quiet_NaN = false);
      BOOST_STATIC_CONSTANT(bool, has_signaling_NaN = false);
      BOOST_STATIC_CONSTANT(bool, has_denorm = false);
      BOOST_STATIC_CONSTANT(bool, has_denorm_loss = false);
      static BOOST_ULLT infinity() throw() { return 0; };
      static BOOST_ULLT quiet_NaN() throw() { return 0; };
      static BOOST_ULLT signaling_NaN() throw() { return 0; };
      static BOOST_ULLT denorm_min() throw() { return 0; };

      BOOST_STATIC_CONSTANT(bool, is_iec559 = false);
      BOOST_STATIC_CONSTANT(bool, is_bounded = true);
      BOOST_STATIC_CONSTANT(bool, is_modulo = true);

      BOOST_STATIC_CONSTANT(bool, traps = false);
      BOOST_STATIC_CONSTANT(bool, tinyness_before = false);
      BOOST_STATIC_CONSTANT(float_round_style, round_style = round_toward_zero);

  };
}
#endif

#endif


//
// Copyright (c) 2013-2016 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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 BEAST_WEBSOCKET_DETAIL_HYBI13_HPP
#define BEAST_WEBSOCKET_DETAIL_HYBI13_HPP

#include <beast/core/detail/base64.hpp>
#include <beast/core/detail/sha1.hpp>
#include <boost/utility/string_ref.hpp>
#include <cstdint>
#include <string>
#include <type_traits>

namespace beast {
namespace websocket {
namespace detail {

template<class Gen>
std::string
make_sec_ws_key(Gen& g)
{
    union U
    {
        std::array<std::uint32_t, 4> a4;
        std::array<std::uint8_t, 16> a16;
    };
    U u;
    for(int i = 0; i < 4; ++i)
        u.a4[i] = g();
    return beast::detail::base64_encode(u.a16.data(), u.a16.size());
}

template<class = void>
std::string
make_sec_ws_accept(boost::string_ref const& key)
{
    std::string s(key.data(), key.size());
    s += "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
    beast::detail::sha1_context ctx;
    beast::detail::init(ctx);
    beast::detail::update(ctx, s.data(), s.size());
    std::array<std::uint8_t,
        beast::detail::sha1_context::digest_size> digest;
    beast::detail::finish(ctx, digest.data());
    return beast::detail::base64_encode(
        digest.data(), digest.size());
}

} // detail
} // websocket
} // beast

#endif


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_AdjointBremsstrahlungCrossSectionEvaluator.hpp
//! \author Luke Kersting
//! \brief  Adjoint bremsstrahlung cross section evaluator declaration
//!
//---------------------------------------------------------------------------//

#ifndef DATA_GEN_ADJOINT_BREMSSTRAHLUNG_CROSS_SECTION_EVALUATOR_HPP
#define DATA_GEN_ADJOINT_BREMSSTRAHLUNG_CROSS_SECTION_EVALUATOR_HPP

// Boost Includes
#include <boost/scoped_ptr.hpp>
/*
// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/distribution.hpp>
#include <boost/bind.hpp>
*/
// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectroatomicReaction.hpp"
#include "Utility_OneDDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace DataGen{

//! The adjoint bremsstrahlung cross section class
class AdjointBremsstrahlungCrossSectionEvaluator
{

public:

  //! Typedef for the bremsstrahlung distribution
  typedef Teuchos::Array<Utility::Pair<double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  BremsstrahlungDistribution;

  //! Constructor
  AdjointBremsstrahlungCrossSectionEvaluator(
    Teuchos::RCP<MonteCarlo::ElectroatomicReaction>& bremsstrahlung_reaction,
    const BremsstrahlungDistribution& energy_loss_distribution );

  //! Destructor
  ~AdjointBremsstrahlungCrossSectionEvaluator()
  { /* ... */ }

  //! Evaluate the differential adjoint bremsstrahlung cross section (dc/dx)
  double evaluateDifferentialCrossSection(
	  const double incoming_energy,
          const double outgoing_energy ) const;

  //! Return the cross section value at a given energy
  double evaluateCrossSection( const double energy,
			       const double precision = 1e-6 ) const;

private:

  // The forward reaction data
  Teuchos::RCP<MonteCarlo::ElectroatomicReaction> d_bremsstrahlung_reaction;

  // The energy loss distribution
  BremsstrahlungDistribution d_energy_loss_distribution;
};

} // end DataGen namespace

#endif // end DATA_GEN_ADJOINT_BREMSSTRAHLUNG_CROSS_SECTION_EVALUATOR_HPP

//---------------------------------------------------------------------------//
// end DataGen_AdjointBremsstrahlungCrossSectionEvaluator.hpp
//---------------------------------------------------------------------------//


/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: TKruse
 *********************************************************************/

#include <base_local_planner/obstacle_cost_function.h>
#include <cmath>
#include <Eigen/Core>
#include <ros/console.h>

namespace base_local_planner {

ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap)
    : costmap_(costmap), sum_scores_(false) {
  if (costmap != NULL) {
    world_model_ = new base_local_planner::CostmapModel(*costmap_);
  }
}

ObstacleCostFunction::~ObstacleCostFunction() {
  if (world_model_ != NULL) {
    delete world_model_;
  }
}

void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
  // TODO: move this to prepare if possible
  max_trans_vel_ = max_trans_vel;
  max_scaling_factor_ = max_scaling_factor;
  scaling_speed_ = scaling_speed;
}

void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
  footprint_spec_ = footprint_spec;
}

bool ObstacleCostFunction::prepare() {
  return true;
}

double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
  double cost = 0;
  double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
  double px, py, pth;
  if (footprint_spec_.size() == 0) {
    // Bug, should never happen
    ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
    return -9;
  }

  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
    traj.getPoint(i, px, py, pth);
    double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

    if(f_cost < 0){
        return f_cost;
    }

    if(sum_scores_)
        cost +=  f_cost;
    else
        cost = f_cost;
  }
  return cost;
}

double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
  double vmag = hypot(traj.xv_, traj.yv_);

  //if we're over a certain speed threshold, we'll scale the robot's
  //footprint to make it either slow down or stay further from walls
  double scale = 1.0;
  if (vmag > scaling_speed) {
    //scale up to the max scaling factor linearly... this could be changed later
    double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
    scale = max_scaling_factor * ratio + 1.0;
  }
  return scale;
}

double ObstacleCostFunction::footprintCost (
    const double& x,
    const double& y,
    const double& th,
    double scale,
    std::vector<geometry_msgs::Point> footprint_spec,
    costmap_2d::Costmap2D* costmap,
    base_local_planner::WorldModel* world_model) {

  //check if the footprint is legal
  // TODO: Cache inscribed radius
  double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);

  if (footprint_cost < 0) {
    return -6.0;
  }
  unsigned int cell_x, cell_y;

  //we won't allow trajectories that go off the map... shouldn't happen that often anyways
  if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
    return -7.0;
  }

  double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));

  return occ_cost;
}

} /* namespace base_local_planner */


//==================================================================================================
/**
  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_ARCH_X86_AVX2_SIMD_FUNCTION_MINUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX2_SIMD_FUNCTION_MINUS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;

   BOOST_DISPATCH_OVERLOAD( minus_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                          , bs::pack_<bd::ints64_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sub_epi64(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( minus_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                          , bs::pack_<bd::ints32_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sub_epi32(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( minus_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints16_<A0>, bs::avx_>
                          , bs::pack_<bd::ints16_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sub_epi16(a0,a1);
      }
   };

   BOOST_DISPATCH_OVERLOAD( minus_
                          , (typename A0)
                          , bs::avx2_
                          , bs::pack_<bd::ints8_<A0>, bs::avx_>
                          , bs::pack_<bd::ints8_<A0>, bs::avx_>
                          )
   {
      BOOST_FORCEINLINE A0 operator()(A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
      {
        return _mm256_sub_epi8(a0,a1);
      }
   };
} } }

#include <boost/simd/arch/x86/avx2/simd/function/minus_s.hpp>

#endif


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

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

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

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

#include <boost/test/unit_test.hpp>
#include <ored/configuration/conventions.hpp>
#include <ored/utilities/indexparser.hpp>
#include <oret/toplevelfixture.hpp>

using namespace QuantLib;
using namespace boost::unit_test_framework;
using namespace ore;
using namespace std;

namespace {

boost::shared_ptr<data::Conventions> convs() {
    boost::shared_ptr<data::Conventions> conventions(new data::Conventions());
    boost::shared_ptr<ore::data::Convention> swapIndexEURConv(
        new ore::data::SwapIndexConvention("EUR-CMS-2Y", "EUR-6M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexEURLongConv(
        new ore::data::SwapIndexConvention("EUR-CMS-30Y", "EUR-6M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexUSDConv(
        new ore::data::SwapIndexConvention("USD-CMS-2Y", "USD-3M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexUSDLongConv(
        new ore::data::SwapIndexConvention("USD-CMS-30Y", "USD-3M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexGBPConv(
        new ore::data::SwapIndexConvention("GBP-CMS-2Y", "GBP-3M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexGBPLongConv(
        new ore::data::SwapIndexConvention("GBP-CMS-30Y", "GBP-6M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexCHFConv(
        new ore::data::SwapIndexConvention("CHF-CMS-2Y", "CHF-3M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexCHFLongConv(
        new ore::data::SwapIndexConvention("CHF-CMS-30Y", "CHF-6M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexJPYConv(
        new ore::data::SwapIndexConvention("JPY-CMS-2Y", "JPY-LIBOR-6M-SWAP-CONVENTIONS"));
    boost::shared_ptr<ore::data::Convention> swapIndexJPYLongConv(
        new ore::data::SwapIndexConvention("JPY-CMS-30Y", "JPY-LIBOR-6M-SWAP-CONVENTIONS"));

    conventions->add(swapIndexEURConv);
    conventions->add(swapIndexEURLongConv);
    conventions->add(swapIndexUSDConv);
    conventions->add(swapIndexUSDLongConv);
    conventions->add(swapIndexGBPConv);
    conventions->add(swapIndexGBPLongConv);
    conventions->add(swapIndexCHFConv);
    conventions->add(swapIndexCHFLongConv);
    conventions->add(swapIndexJPYConv);
    conventions->add(swapIndexJPYLongConv);

    boost::shared_ptr<ore::data::Convention> swapEURConv(new ore::data::IRSwapConvention(
        "EUR-6M-SWAP-CONVENTIONS", "TARGET", "Annual", "MF", "30/360", "EUR-EURIBOR-6M"));
    boost::shared_ptr<ore::data::Convention> swapUSDConv(
        new ore::data::IRSwapConvention("USD-3M-SWAP-CONVENTIONS", "US", "Semiannual", "MF", "30/360", "USD-LIBOR-3M"));
    boost::shared_ptr<ore::data::Convention> swapGBPConv(
        new ore::data::IRSwapConvention("GBP-3M-SWAP-CONVENTIONS", "UK", "Semiannual", "MF", "A365", "GBP-LIBOR-3M"));
    boost::shared_ptr<ore::data::Convention> swapGBPLongConv(
        new ore::data::IRSwapConvention("GBP-6M-SWAP-CONVENTIONS", "UK", "Semiannual", "MF", "A365", "GBP-LIBOR-6M"));
    boost::shared_ptr<ore::data::Convention> swapCHFConv(
        new ore::data::IRSwapConvention("CHF-3M-SWAP-CONVENTIONS", "ZUB", "Annual", "MF", "30/360", "CHF-LIBOR-3M"));
    boost::shared_ptr<ore::data::Convention> swapCHFLongConv(
        new ore::data::IRSwapConvention("CHF-6M-SWAP-CONVENTIONS", "ZUB", "Annual", "MF", "30/360", "CHF-LIBOR-6M"));
    boost::shared_ptr<ore::data::Convention> swapJPYConv(new ore::data::IRSwapConvention(
        "JPY-LIBOR-6M-SWAP-CONVENTIONS", "JP", "Semiannual", "MF", "A365", "JPY-LIBOR-6M"));

    conventions->add(swapEURConv);
    conventions->add(swapUSDConv);
    conventions->add(swapGBPConv);
    conventions->add(swapGBPLongConv);
    conventions->add(swapCHFConv);
    conventions->add(swapCHFLongConv);
    conventions->add(swapJPYConv);

    return conventions;
}

struct test_data {
    const char* str;
    const char* index_name;
    Period tenor;
};

static struct test_data index_data[] = {
    // parsing string,     index name,                     tenor
    {"EUR-EONIA-1D", "EoniaON Actual/360", 1 * Days},
    {"GBP-SONIA-1D", "SoniaON Actual/365 (Fixed)", 1 * Days},
    {"JPY-TONAR-1D", "TONARON Actual/365 (Fixed)", 1 * Days},
    {"CHF-TOIS", "CHF-TOISTN Actual/360", 1 * Days},
    {"USD-FedFunds", "FedFundsON Actual/360", 1 * Days},
    {"CHF-SARON", "CHF-SARONON Actual/360", 1 * Days},
    {"DKK-DKKOIS", "DKK-DKKOISTN Actual/360", 1 * Days},
    {"SEK-SIOR", "SEK-SIORTN Actual/360", 1 * Days},
    {"NOK-NOWA", "NowaON Actual/Actual (ISMA)", 1 * Days},
    {"NZD-OCR", "NzocrON Actual/365 (Fixed)", 1 * Days},
    {"BRL-CDI", "BRL-CDION Business/252(Brazil)", 1 * Days},

    {"AUD-LIBOR-1W", "AUDLibor1W Actual/360", 1 * Weeks},
    {"AUD-LIBOR-1M", "AUDLibor1M Actual/360", 1 * Months},
    {"AUD-LIBOR-2M", "AUDLibor2M Actual/360", 2 * Months},
    {"AUD-LIBOR-3M", "AUDLibor3M Actual/360", 3 * Months},
    {"AUD-LIBOR-6M", "AUDLibor6M Actual/360", 6 * Months},
    {"AUD-LIBOR-12M", "AUDLibor1Y Actual/360", 1 * Years},
    {"AUD-LIBOR-1Y", "AUDLibor1Y Actual/360", 1 * Years},

    {"AUD-BBSW-1W", "AUD-BBSW1W Actual/365 (Fixed)", 1 * Weeks},
    {"AUD-BBSW-1M", "AUD-BBSW1M Actual/365 (Fixed)", 1 * Months},
    {"AUD-BBSW-2M", "AUD-BBSW2M Actual/365 (Fixed)", 2 * Months},
    {"AUD-BBSW-3M", "AUD-BBSW3M Actual/365 (Fixed)", 3 * Months},
    {"AUD-BBSW-6M", "AUD-BBSW6M Actual/365 (Fixed)", 6 * Months},
    {"AUD-BBSW-12M", "AUD-BBSW1Y Actual/365 (Fixed)", 1 * Years},
    {"AUD-BBSW-1Y", "AUD-BBSW1Y Actual/365 (Fixed)", 1 * Years},

    {"EUR-EURIBOR-1W", "Euribor1W Actual/360", 1 * Weeks},
    {"EUR-EURIBOR-2W", "Euribor2W Actual/360", 2 * Weeks},
    {"EUR-EURIBOR-3W", "Euribor3W Actual/360", 3 * Weeks},
    {"EUR-EURIBOR-1M", "Euribor1M Actual/360", 1 * Months},
    {"EUR-EURIBOR-2M", "Euribor2M Actual/360", 2 * Months},
    {"EUR-EURIBOR-3M", "Euribor3M Actual/360", 3 * Months},
    {"EUR-EURIBOR-4M", "Euribor4M Actual/360", 4 * Months},
    {"EUR-EURIBOR-5M", "Euribor5M Actual/360", 5 * Months},
    {"EUR-EURIBOR-6M", "Euribor6M Actual/360", 6 * Months},
    {"EUR-EURIBOR-7M", "Euribor7M Actual/360", 7 * Months},
    {"EUR-EURIBOR-8M", "Euribor8M Actual/360", 8 * Months},
    {"EUR-EURIBOR-9M", "Euribor9M Actual/360", 9 * Months},
    {"EUR-EURIBOR-10M", "Euribor10M Actual/360", 10 * Months},
    {"EUR-EURIBOR-11M", "Euribor11M Actual/360", 11 * Months},
    {"EUR-EURIBOR-12M", "Euribor1Y Actual/360", 1 * Years},
    {"EUR-EURIBOR-1Y", "Euribor1Y Actual/360", 1 * Years},

    {"EUR-LIBOR-1W", "EURLibor1W Actual/360", 1 * Weeks},
    {"EUR-LIBOR-1M", "EURLibor1M Actual/360", 1 * Months},
    {"EUR-LIBOR-2M", "EURLibor2M Actual/360", 2 * Months},
    {"EUR-LIBOR-3M", "EURLibor3M Actual/360", 3 * Months},
    {"EUR-LIBOR-6M", "EURLibor6M Actual/360", 6 * Months},
    {"EUR-LIBOR-12M", "EURLibor1Y Actual/360", 1 * Years},
    {"EUR-LIBOR-1Y", "EURLibor1Y Actual/360", 1 * Years},

    {"CAD-CDOR-1W", "CDOR1W Actual/365 (Fixed)", 1 * Weeks},
    {"CAD-CDOR-1M", "CDOR1M Actual/365 (Fixed)", 1 * Months},
    {"CAD-CDOR-2M", "CDOR2M Actual/365 (Fixed)", 2 * Months},
    {"CAD-CDOR-3M", "CDOR3M Actual/365 (Fixed)", 3 * Months},
    {"CAD-CDOR-6M", "CDOR6M Actual/365 (Fixed)", 6 * Months},
    {"CAD-CDOR-12M", "CDOR1Y Actual/365 (Fixed)", 1 * Years},
    {"CAD-CDOR-1Y", "CDOR1Y Actual/365 (Fixed)", 1 * Years},

    {"CAD-BA-1W", "CDOR1W Actual/365 (Fixed)", 1 * Weeks},
    {"CAD-BA-1M", "CDOR1M Actual/365 (Fixed)", 1 * Months},
    {"CAD-BA-2M", "CDOR2M Actual/365 (Fixed)", 2 * Months},
    {"CAD-BA-3M", "CDOR3M Actual/365 (Fixed)", 3 * Months},
    {"CAD-BA-6M", "CDOR6M Actual/365 (Fixed)", 6 * Months},
    {"CAD-BA-12M", "CDOR1Y Actual/365 (Fixed)", 1 * Years},
    {"CAD-BA-1Y", "CDOR1Y Actual/365 (Fixed)", 1 * Years},

    {"CZK-PRIBOR-6M", "CZK-PRIBOR6M Actual/360", 6 * Months},

    {"USD-LIBOR-1W", "USDLibor1W Actual/360", 1 * Weeks},
    {"USD-LIBOR-1M", "USDLibor1M Actual/360", 1 * Months},
    {"USD-LIBOR-2M", "USDLibor2M Actual/360", 2 * Months},
    {"USD-LIBOR-3M", "USDLibor3M Actual/360", 3 * Months},
    {"USD-LIBOR-6M", "USDLibor6M Actual/360", 6 * Months},
    {"USD-LIBOR-12M", "USDLibor1Y Actual/360", 1 * Years},
    {"USD-LIBOR-1Y", "USDLibor1Y Actual/360", 1 * Years},

    {"GBP-LIBOR-1W", "GBPLibor1W Actual/365 (Fixed)", 1 * Weeks},
    {"GBP-LIBOR-1M", "GBPLibor1M Actual/365 (Fixed)", 1 * Months},
    {"GBP-LIBOR-2M", "GBPLibor2M Actual/365 (Fixed)", 2 * Months},
    {"GBP-LIBOR-3M", "GBPLibor3M Actual/365 (Fixed)", 3 * Months},
    {"GBP-LIBOR-6M", "GBPLibor6M Actual/365 (Fixed)", 6 * Months},
    {"GBP-LIBOR-12M", "GBPLibor1Y Actual/365 (Fixed)", 1 * Years},
    {"GBP-LIBOR-1Y", "GBPLibor1Y Actual/365 (Fixed)", 1 * Years},

    {"JPY-LIBOR-1W", "JPYLibor1W Actual/360", 1 * Weeks},
    {"JPY-LIBOR-1M", "JPYLibor1M Actual/360", 1 * Months},
    {"JPY-LIBOR-2M", "JPYLibor2M Actual/360", 2 * Months},
    {"JPY-LIBOR-3M", "JPYLibor3M Actual/360", 3 * Months},
    {"JPY-LIBOR-6M", "JPYLibor6M Actual/360", 6 * Months},
    {"JPY-LIBOR-12M", "JPYLibor1Y Actual/360", 1 * Years},
    {"JPY-LIBOR-1Y", "JPYLibor1Y Actual/360", 1 * Years},

    {"JPY-TIBOR-1W", "Tibor1W Actual/365 (Fixed)", 1 * Weeks},
    {"JPY-TIBOR-1M", "Tibor1M Actual/365 (Fixed)", 1 * Months},
    {"JPY-TIBOR-2M", "Tibor2M Actual/365 (Fixed)", 2 * Months},
    {"JPY-TIBOR-3M", "Tibor3M Actual/365 (Fixed)", 3 * Months},
    {"JPY-TIBOR-6M", "Tibor6M Actual/365 (Fixed)", 6 * Months},
    {"JPY-TIBOR-12M", "Tibor1Y Actual/365 (Fixed)", 1 * Years},
    {"JPY-TIBOR-1Y", "Tibor1Y Actual/365 (Fixed)", 1 * Years},

    {"CAD-LIBOR-1W", "CADLibor1W Actual/365 (Fixed)", 1 * Weeks},
    {"CAD-LIBOR-1M", "CADLibor1M Actual/365 (Fixed)", 1 * Months},
    {"CAD-LIBOR-2M", "CADLibor2M Actual/365 (Fixed)", 2 * Months},
    {"CAD-LIBOR-3M", "CADLibor3M Actual/365 (Fixed)", 3 * Months},
    {"CAD-LIBOR-6M", "CADLibor6M Actual/365 (Fixed)", 6 * Months},
    {"CAD-LIBOR-12M", "CADLibor1Y Actual/365 (Fixed)", 1 * Years},
    {"CAD-LIBOR-1Y", "CADLibor1Y Actual/365 (Fixed)", 1 * Years},

    {"CHF-LIBOR-1W", "CHFLibor1W Actual/360", 1 * Weeks},
    {"CHF-LIBOR-1M", "CHFLibor1M Actual/360", 1 * Months},
    {"CHF-LIBOR-2M", "CHFLibor2M Actual/360", 2 * Months},
    {"CHF-LIBOR-3M", "CHFLibor3M Actual/360", 3 * Months},
    {"CHF-LIBOR-6M", "CHFLibor6M Actual/360", 6 * Months},
    {"CHF-LIBOR-12M", "CHFLibor1Y Actual/360", 1 * Years},
    {"CHF-LIBOR-1Y", "CHFLibor1Y Actual/360", 1 * Years},

    {"SAR-SAIBOR-1W", "SAR-SAIBOR1W Actual/360", 1 * Weeks},
    {"SAR-SAIBOR-1M", "SAR-SAIBOR1M Actual/360", 1 * Months},
    {"SAR-SAIBOR-2M", "SAR-SAIBOR2M Actual/360", 2 * Months},
    {"SAR-SAIBOR-3M", "SAR-SAIBOR3M Actual/360", 3 * Months},
    {"SAR-SAIBOR-6M", "SAR-SAIBOR6M Actual/360", 6 * Months},

    {"SEK-STIBOR-1W", "SEK-STIBOR1W Actual/360", 1 * Weeks},
    {"SEK-STIBOR-1M", "SEK-STIBOR1M Actual/360", 1 * Months},
    {"SEK-STIBOR-2M", "SEK-STIBOR2M Actual/360", 2 * Months},
    {"SEK-STIBOR-3M", "SEK-STIBOR3M Actual/360", 3 * Months},
    {"SEK-STIBOR-6M", "SEK-STIBOR6M Actual/360", 6 * Months},

    {"SEK-LIBOR-1W", "SEKLibor1W Actual/360", 1 * Weeks},
    {"SEK-LIBOR-1M", "SEKLibor1M Actual/360", 1 * Months},
    {"SEK-LIBOR-2M", "SEKLibor2M Actual/360", 2 * Months},
    {"SEK-LIBOR-3M", "SEKLibor3M Actual/360", 3 * Months},
    {"SEK-LIBOR-6M", "SEKLibor6M Actual/360", 6 * Months},
    {"SEK-LIBOR-12M", "SEKLibor1Y Actual/360", 1 * Years},
    {"SEK-LIBOR-1Y", "SEKLibor1Y Actual/360", 1 * Years},

    {"NOK-NIBOR-1W", "NOK-NIBOR1W Actual/360", 1 * Weeks},
    {"NOK-NIBOR-1M", "NOK-NIBOR1M Actual/360", 1 * Months},
    {"NOK-NIBOR-2M", "NOK-NIBOR2M Actual/360", 2 * Months},
    {"NOK-NIBOR-3M", "NOK-NIBOR3M Actual/360", 3 * Months},
    {"NOK-NIBOR-6M", "NOK-NIBOR6M Actual/360", 6 * Months},
    {"NOK-NIBOR-9M", "NOK-NIBOR9M Actual/360", 9 * Months},
    {"NOK-NIBOR-12M", "NOK-NIBOR1Y Actual/360", 1 * Years},
    {"NOK-NIBOR-1Y", "NOK-NIBOR1Y Actual/360", 1 * Years},

    {"HKD-HIBOR-1W", "HKD-HIBOR1W Actual/365 (Fixed)", 1 * Weeks},
    {"HKD-HIBOR-2W", "HKD-HIBOR2W Actual/365 (Fixed)", 2 * Weeks},
    {"HKD-HIBOR-1M", "HKD-HIBOR1M Actual/365 (Fixed)", 1 * Months},
    {"HKD-HIBOR-2M", "HKD-HIBOR2M Actual/365 (Fixed)", 2 * Months},
    {"HKD-HIBOR-3M", "HKD-HIBOR3M Actual/365 (Fixed)", 3 * Months},
    {"HKD-HIBOR-6M", "HKD-HIBOR6M Actual/365 (Fixed)", 6 * Months},
    {"HKD-HIBOR-12M", "HKD-HIBOR1Y Actual/365 (Fixed)", 1 * Years},
    {"HKD-HIBOR-1Y", "HKD-HIBOR1Y Actual/365 (Fixed)", 1 * Years},

    {"SGD-SIBOR-1M", "SGD-SIBOR1M Actual/365 (Fixed)", 1 * Months},
    {"SGD-SIBOR-3M", "SGD-SIBOR3M Actual/365 (Fixed)", 3 * Months},
    {"SGD-SIBOR-6M", "SGD-SIBOR6M Actual/365 (Fixed)", 6 * Months},
    {"SGD-SIBOR-12M", "SGD-SIBOR1Y Actual/365 (Fixed)", 1 * Years},
    {"SGD-SIBOR-1Y", "SGD-SIBOR1Y Actual/365 (Fixed)", 1 * Years},

    {"SGD-SOR-1M", "SGD-SOR1M Actual/365 (Fixed)", 1 * Months},
    {"SGD-SOR-3M", "SGD-SOR3M Actual/365 (Fixed)", 3 * Months},
    {"SGD-SOR-6M", "SGD-SOR6M Actual/365 (Fixed)", 6 * Months},
    {"SGD-SOR-12M", "SGD-SOR1Y Actual/365 (Fixed)", 1 * Years},
    {"SGD-SOR-1Y", "SGD-SOR1Y Actual/365 (Fixed)", 1 * Years},

    {"DKK-LIBOR-1W", "DKKLibor1W Actual/360", 1 * Weeks},
    {"DKK-LIBOR-1M", "DKKLibor1M Actual/360", 1 * Months},
    {"DKK-LIBOR-2M", "DKKLibor2M Actual/360", 2 * Months},
    {"DKK-LIBOR-3M", "DKKLibor3M Actual/360", 3 * Months},
    {"DKK-LIBOR-6M", "DKKLibor6M Actual/360", 6 * Months},
    {"DKK-LIBOR-12M", "DKKLibor1Y Actual/360", 1 * Years},
    {"DKK-LIBOR-1Y", "DKKLibor1Y Actual/360", 1 * Years},

    {"DKK-CIBOR-1W", "DKK-CIBOR1W Actual/360", 1 * Weeks},
    {"DKK-CIBOR-1M", "DKK-CIBOR1M Actual/360", 1 * Months},
    {"DKK-CIBOR-2M", "DKK-CIBOR2M Actual/360", 2 * Months},
    {"DKK-CIBOR-3M", "DKK-CIBOR3M Actual/360", 3 * Months},
    {"DKK-CIBOR-6M", "DKK-CIBOR6M Actual/360", 6 * Months},
    {"DKK-CIBOR-12M", "DKK-CIBOR1Y Actual/360", 1 * Years},
    {"DKK-CIBOR-1Y", "DKK-CIBOR1Y Actual/360", 1 * Years},

    {"HUF-BUBOR-6M", "HUF-BUBOR6M Actual/360", 6 * Months},
    {"IDR-IDRFIX-6M", "IDR-IDRFIX6M Actual/360", 6 * Months},
    {"INR-MIFOR-6M", "INR-MIFOR6M Actual/365 (Fixed)", 6 * Months},
    {"MXN-TIIE-6M", "MXN-TIIE6M Actual/360", 6 * Months},
    {"PLN-WIBOR-6M", "PLN-WIBOR6M Actual/365 (Fixed)", 6 * Months},
    {"SKK-BRIBOR-6M", "SKK-BRIBOR6M Actual/360", 6 * Months},

    {"NZD-BKBM-1M", "NZD-BKBM1M Actual/Actual (ISDA)", 1 * Months},
    {"NZD-BKBM-2M", "NZD-BKBM2M Actual/Actual (ISDA)", 2 * Months},
    {"NZD-BKBM-3M", "NZD-BKBM3M Actual/Actual (ISDA)", 3 * Months},
    {"NZD-BKBM-4M", "NZD-BKBM4M Actual/Actual (ISDA)", 4 * Months},
    {"NZD-BKBM-5M", "NZD-BKBM5M Actual/Actual (ISDA)", 5 * Months},
    {"NZD-BKBM-6M", "NZD-BKBM6M Actual/Actual (ISDA)", 6 * Months},

    {"KRW-KORIBOR-1M", "KRW-KORIBOR1M Actual/365 (Fixed)", 1 * Months},
    {"KRW-KORIBOR-2M", "KRW-KORIBOR2M Actual/365 (Fixed)", 2 * Months},
    {"KRW-KORIBOR-3M", "KRW-KORIBOR3M Actual/365 (Fixed)", 3 * Months},
    {"KRW-KORIBOR-4M", "KRW-KORIBOR4M Actual/365 (Fixed)", 4 * Months},
    {"KRW-KORIBOR-5M", "KRW-KORIBOR5M Actual/365 (Fixed)", 5 * Months},
    {"KRW-KORIBOR-6M", "KRW-KORIBOR6M Actual/365 (Fixed)", 6 * Months},

    {"TWD-TAIBOR-1M", "TWD-TAIBOR1M Actual/365 (Fixed)", 1 * Months},
    {"TWD-TAIBOR-2M", "TWD-TAIBOR2M Actual/365 (Fixed)", 2 * Months},
    {"TWD-TAIBOR-3M", "TWD-TAIBOR3M Actual/365 (Fixed)", 3 * Months},
    {"TWD-TAIBOR-4M", "TWD-TAIBOR4M Actual/365 (Fixed)", 4 * Months},
    {"TWD-TAIBOR-5M", "TWD-TAIBOR5M Actual/365 (Fixed)", 5 * Months},
    {"TWD-TAIBOR-6M", "TWD-TAIBOR6M Actual/365 (Fixed)", 6 * Months},

    {"TRY-TRLIBOR-1M", "TRLibor1M Actual/360", 1 * Months},
    {"TRY-TRLIBOR-2M", "TRLibor2M Actual/360", 2 * Months},
    {"TRY-TRLIBOR-3M", "TRLibor3M Actual/360", 3 * Months},
    {"TRY-TRLIBOR-4M", "TRLibor4M Actual/360", 4 * Months},
    {"TRY-TRLIBOR-5M", "TRLibor5M Actual/360", 5 * Months},
    {"TRY-TRLIBOR-6M", "TRLibor6M Actual/360", 6 * Months},

    {"MYR-KLIBOR-1M", "MYR-KLIBOR1M Actual/365 (Fixed)", 1 * Months},
    {"MYR-KLIBOR-2M", "MYR-KLIBOR2M Actual/365 (Fixed)", 2 * Months},
    {"MYR-KLIBOR-3M", "MYR-KLIBOR3M Actual/365 (Fixed)", 3 * Months},
    {"MYR-KLIBOR-4M", "MYR-KLIBOR4M Actual/365 (Fixed)", 4 * Months},
    {"MYR-KLIBOR-5M", "MYR-KLIBOR5M Actual/365 (Fixed)", 5 * Months},
    {"MYR-KLIBOR-6M", "MYR-KLIBOR6M Actual/365 (Fixed)", 6 * Months}};

static struct test_data swap_index_data[] = {
    {"EUR-CMS-2Y", "EURLiborSwapIsdaFix2Y 30/360 (Bond Basis)", 2 * Years},
    {"EUR-CMS-30Y", "EURLiborSwapIsdaFix30Y 30/360 (Bond Basis)", 30 * Years},
    {"USD-CMS-2Y", "USDLiborSwapIsdaFix2Y 30/360 (Bond Basis)", 2 * Years},
    {"USD-CMS-30Y", "USDLiborSwapIsdaFix30Y 30/360 (Bond Basis)", 30 * Years},
    {"GBP-CMS-2Y", "GBPLiborSwapIsdaFix2Y Actual/365 (Fixed)", 2 * Years},
    {"GBP-CMS-30Y", "GBPLiborSwapIsdaFix30Y Actual/365 (Fixed)", 30 * Years},
    {"CHF-CMS-2Y", "CHFLiborSwapIsdaFix2Y 30/360 (Bond Basis)", 2 * Years},
    {"CHF-CMS-30Y", "CHFLiborSwapIsdaFix30Y 30/360 (Bond Basis)", 30 * Years},
    {"JPY-CMS-2Y", "JPYLiborSwapIsdaFix2Y Actual/365 (Fixed)", 2 * Years},
    {"JPY-CMS-30Y", "JPYLiborSwapIsdaFix30Y Actual/365 (Fixed)", 30 * Years},
};

} // namespace

BOOST_FIXTURE_TEST_SUITE(OREDataTestSuite, ore::test::TopLevelFixture)

BOOST_AUTO_TEST_SUITE(IndexTests)

BOOST_AUTO_TEST_CASE(testIborIndexParsing) {

    BOOST_TEST_MESSAGE("Testing Ibor Index name parsing...");

    Size len = sizeof(index_data) / sizeof(index_data[0]);
    for (Size i = 0; i < len; ++i) {
        string str(index_data[i].str);
        string index_name(index_data[i].index_name);
        Period tenor(index_data[i].tenor);

        boost::shared_ptr<IborIndex> ibor;
        try {
            ibor = ore::data::parseIborIndex(str);
        } catch (std::exception& e) {
            BOOST_FAIL("Ibor Parser failed to parse \"" << str << "\" [exception:" << e.what() << "]");
        } catch (...) {
            BOOST_FAIL("Ibor Parser failed to parse \"" << str << "\" [unhandled]");
        }
        if (ibor) {
            BOOST_CHECK_EQUAL(ibor->name(), index_name);
            BOOST_CHECK_EQUAL(ibor->tenor(), tenor);

            BOOST_TEST_MESSAGE("Parsed \"" << str << "\" and got " << ibor->name());
        } else
            BOOST_FAIL("Ibor Parser(" << str << ") returned null pointer");
    }
}

BOOST_AUTO_TEST_CASE(testIborIndexParsingFails) {

    BOOST_TEST_MESSAGE("Testing Ibor Index parsing fails...");

    // Test invalid strings
    BOOST_CHECK_THROW(ore::data::parseIborIndex("EUR-EONIA-1M"), QuantLib::Error);
    BOOST_CHECK_THROW(ore::data::parseIborIndex("EUR-EURIBOR-1D"), QuantLib::Error);
    BOOST_CHECK_THROW(ore::data::parseIborIndex("EUR-FALSE-6M"), QuantLib::Error);
    BOOST_CHECK_THROW(ore::data::parseIborIndex("It's a trap!"), QuantLib::Error);
}

BOOST_AUTO_TEST_CASE(testSwapIndexParsing) {

    BOOST_TEST_MESSAGE("Testing Swap Index name parsing...");

    Handle<YieldTermStructure> h; // dummy

    Size len = sizeof(swap_index_data) / sizeof(swap_index_data[0]);
    data::Conventions conventions = *convs();

    for (Size i = 0; i < len; ++i) {
        string str(swap_index_data[i].str);
        string index_name(swap_index_data[i].index_name);
        Period tenor(swap_index_data[i].tenor);

        boost::shared_ptr<data::Convention> tmp = conventions.get(str);
        boost::shared_ptr<data::SwapIndexConvention> swapCon =
            boost::dynamic_pointer_cast<data::SwapIndexConvention>(tmp);
        tmp = conventions.get(swapCon->conventions());
        boost::shared_ptr<data::IRSwapConvention> con = boost::dynamic_pointer_cast<data::IRSwapConvention>(tmp);
        QL_REQUIRE(con, "no swap convention");
        boost::shared_ptr<SwapIndex> swap;
        try {
            swap = ore::data::parseSwapIndex(str, h, h, con);
        } catch (std::exception& e) {
            BOOST_FAIL("Swap Parser failed to parse \"" << str << "\" [exception:" << e.what() << "]");
        } catch (...) {
            BOOST_FAIL("Swap Parser failed to parse \"" << str << "\" [unhandled]");
        }
        if (swap) {
            BOOST_CHECK_EQUAL(swap->name(), index_name);
            BOOST_CHECK_EQUAL(swap->tenor(), tenor);

            BOOST_TEST_MESSAGE("Parsed \"" << str << "\" and got " << swap->name());
        } else
            BOOST_FAIL("Swap Parser(" << str << ") returned null pointer");
    }
}

BOOST_AUTO_TEST_SUITE_END()

BOOST_AUTO_TEST_SUITE_END()


#include "stddefx.h"

#ifndef INCLUDED_CALC_TSSOUTPUTVALUE
#include "calc_tssoutputvalue.h"
#define INCLUDED_CALC_TSSOUTPUTVALUE
#endif

// std libraries

#ifndef INCLUDED_IOMANIP
#include <iomanip>
#define INCLUDED_IOMANIP
#endif

#ifndef INCLUDED_STDEXCEPT
#include <stdexcept>
#define INCLUDED_STDEXCEPT
#endif

#ifndef INCLUDED_BOOST_STATIC_ASSERT
#include <boost/static_assert.hpp>
#define INCLUDED_BOOST_STATIC_ASSERT
#endif

// our libraries

#ifndef INCLUDED_DAL_TYPE
#include "dal_Type.h"
#define INCLUDED_DAL_TYPE
#endif

#ifndef INCLUDED_CALC
#include "calc.h"      // TssRow stuff
#define INCLUDED_CALC
#endif

#ifndef INCLUDED_APPARGS
# include "appargs.h"
#define INCLUDED_APPARGS
#endif

#ifndef INCLUDED_NEW
#include "com_new.h"
#define INCLUDED_NEW
#endif
#ifndef INCLUDED_COM_FILE
#include "com_file.h"
#define INCLUDED_COM_FILE
#endif
#ifndef INCLUDED_COM_EXCEPTION
#include "com_exception.h"
#define INCLUDED_COM_EXCEPTION
#endif
#ifndef INCLUDED_COM_CSFCELL
#include "com_csfcell.h"
#define INCLUDED_COM_CSFCELL
#endif

// modules

#ifndef INCLUDED_CALC_MAP2CSF
#include "calc_map2csf.h"
#define INCLUDED_CALC_MAP2CSF
#endif

#ifndef INCLUDED_CALC_AVERAGEMAP
#include "calc_averagemap.h"
#define INCLUDED_CALC_AVERAGEMAP
#endif
#ifndef INCLUDED_CALC_APIMAP
#include "calc_apimap.h"
#define INCLUDED_CALC_APIMAP
#endif

#ifndef INCLUDED_CALC_FIELD
#include "calc_field.h"
#define INCLUDED_CALC_FIELD
#endif

#ifndef INCLUDED_CALC_IOSTRATEGY
#include "calc_iostrategy.h"
#define INCLUDED_CALC_IOSTRATEGY
#endif

#ifndef INCLUDED_CALC_MEMORYEXCHANGEITEM
#include "calc_MemoryExchangeItem.h"
#define INCLUDED_CALC_MEMORYEXCHANGEITEM
#endif

const size_t calc::FileTimeoutput::maxCacheSize=128;

namespace calc { namespace detail {

static int doTimeoutput(
  double      *val,
  const Field *id,
  const Field *expr,
  size_t nrCols)
{
  int result=0;
  if (expr->cri() == CRI_f) {
    AverageMap am;
    if (id->cri()==CRI_1)
      am.apply(id->src_1(),id->nrValues(),
               expr->src_f(),expr->nrValues());
    else
      am.apply(id->src_4(),id->nrValues(),
               expr->src_f(),expr->nrValues());
    am.setResults(val,nrCols);
  } else {
     // timeoutputting class data the old "inefficient" way
     //
     // CRI_1 CRI_4
     // create a rasterDim of 1 row by nrCells cols
     //   allowing to work with MaskPacking data
     size_t nrCells=std::max(id->nrValues(),expr->nrValues());
     geo::RasterDim rs(1,nrCells);
     calc::ApiMapC<MAP_INT4> idM(rs,id->src(),id->isSpatial(),id->cr());

     calc::ApiMapC<MAP_INT4> exprM(rs,expr->src(),expr->isSpatial(),expr->cr());
     result = AddToTssRowINT4(val,nrCols, idM.map(), exprM.map());
  }
  return result;
}

static size_t maxId(const Field *id)
{
  size_t nrCols= 0;
  size_t maxIndex;
  switch(id->cri()) {
    case CRI_1: {
     const UINT1* m = com::csfCellMax(id->src_1(),id->src_1()+id->nrValues());
     maxIndex=m-id->src_1();
     if (maxIndex!=id->nrValues())
       nrCols=id->src_1()[maxIndex];
    } break;
    case CRI_4: {
     const INT4* m = com::csfCellMax(id->src_4(),id->src_4()+id->nrValues());
     maxIndex=m-id->src_4();
     if (maxIndex!=id->nrValues())
       nrCols=id->src_4()[maxIndex];
    } break;
    default: PRECOND(FALSE);
  }
  return nrCols;
}

 template<typename T>
  class MemoryExchangeItemTssRow: public MemoryExchangeItem
  {
   size_t  d_len;
   char   *d_buffer;
   struct Header {
     UINT4  id; // identification of header type
     UINT4  vt; // value type
     UINT4  nrDim; // number of dimensions
     UINT4  lenDim1; // length of dimension 1
   };
   public:
     MemoryExchangeItemTssRow(std::string const& name,
                              size_t id,
                              const std::vector<double>& data):
          MemoryExchangeItem(name, id),
          d_len(sizeof(Header)+(sizeof(T)*data.size())),
          d_buffer(new char[d_len])
        {
          Header header;
          header.id = 1;
          header.vt = dal::TypeTraits<T>::csfCr;
          header.nrDim=1;
          header.lenDim1=(UINT4)data.size();

          BOOST_STATIC_ASSERT(sizeof(Header) == (4 * sizeof(UINT4)));
          *((Header *)d_buffer) = header;

          T *dest = (T *)(d_buffer+sizeof(Header));
          for(size_t i=0; i < data.size(); ++i) {
             if(pcr::isMV(data[i]))
               pcr::setMV(dest[i]);
             else
               dest[i] = (T)(data[i]);
          }
        }
    ~MemoryExchangeItemTssRow() {
     delete[] d_buffer;
    }
    void *rawValue    () const
    {
      return d_buffer;
    }
    void  beMemCpySrc (void *dest) const
    {
      std::memcpy(d_buffer, dest, d_len);
    }
  };

} }

calc::TssOutputValue::TssOutputValue()
{
}
calc::TssOutputValue::~TssOutputValue()
{
}

size_t calc::FileTimeoutput::initNrRowsCached() const
{
  size_t nrt=d_stackInfo.lastInt();

  if (appHeader == APP_NOHEADER) {
    // then write at last timeStep, to support current tcl-interface
    return nrt;
  }
  if (d_stackInfo.flushTssAtEachTimeStep())
    return 1;
  return std::min<size_t>(nrt,maxCacheSize);
}

//! ctor
/*!
 * Note that the file is not created here, but in timeStep nr 1.
 */
calc::FileTimeoutput::FileTimeoutput(
    const StackInfo& stackInfo,
    size_t nrCols):
  TssOutputValue(),
  d_stackInfo(stackInfo),
  d_nrCols(nrCols),
  d_fileErrorOccured(false),
  d_fileCreated(false),
  d_lastStepToFile(0),
  d_value(0)
{
  PRECOND(nrInSet(d_stackInfo.vs()) == 1);
  PRECOND(d_nrCols > 0);
  d_nrRowsCached=initNrRowsCached();
  PRECOND(d_nrRowsCached > 0);
  d_value=com::new2d<double>(d_nrRowsCached,d_nrCols);
}

calc::FileTimeoutput::~FileTimeoutput()
{
  if (! d_fileErrorOccured) {
    // bugzilla 87 and 160  pcrcalc561
    // if tss creation was the cause of the exception then
    //  it fails again.
    // do not generate new exceptions here!
    try {
     finish();
    } catch(...) {
    }
  }
  com::delete2d<double>(d_value);
}

void calc::FileTimeoutput::finish()
{
  // if data left, write it otherwise not
  // the if prevent the case that we only
  // write a header and no data
  if (d_step.size())
    flushToFile();
}

bool calc::FileTimeoutput::reportTimeStep(size_t t) const
{
  if (appHeader == APP_NOHEADER) // obey report clause
   return d_stackInfo.reportTimeStep(t);
  return true;
}

//! set row to correct position and return buffer of row
/*!
 * \returns 0 if currentTimeStep does not have to be written
 */
double *calc::FileTimeoutput::setRow(size_t currentTimeStep)
{
  /* pcrcalc/test13[cd] */
  if (!reportTimeStep(currentTimeStep))
    return 0;
  if (d_step.size() && currentTimeStep == d_step.back()) {
    // pcrcalc/test317 writing a value twice in loop!
    return d_value[d_step.size()-1];
  }

  if (d_step.size() == d_nrRowsCached)
    flushToFile();

  POSTCOND(d_step.size() < d_nrRowsCached);
  d_step.push_back(currentTimeStep);

  return d_value[d_step.size()-1];
}

void calc::FileTimeoutput::openFile(std::ofstream& ofs)
{
  if (d_fileCreated) {
   // otherwise we open for appending
   com::open(ofs,d_stackInfo.stackName(), std::ios::app);
   return;
  }
  // begin, create file
  d_fileCreated = true;
  com::open(ofs,d_stackInfo.stackName());

  if (appHeader == APP_DEFHEADER) {
    // print header
    CSF_VS vs = vs2CsfVs(d_stackInfo.vs());
    if (vs == VS_UNDEFINED)
     ofs << "summary\n";
    else
     ofs << "timeseries " << toString(d_stackInfo.vs()) << "\n";
    ofs << d_nrCols+1 << "\n"
        << "timestep\n";
    for(size_t c = 0; c < d_nrCols; c++)
     ofs << c+1 << "\n";
  }
}
/*
 * \todo  dangerous typecase VS->CSF_VS
 */
void calc::FileTimeoutput::printLine(
  size_t         t,
  double        *v,
  std::ofstream& f)
{
  const char *mvFmt;
  int         valFmt;

  switch((CSF_VS)d_stackInfo.vs()) { // dangerous typecast
     case VS_LDD    :
     case VS_BOOLEAN: mvFmt = "1e31"; // len 4
                      valFmt = 4; break;
     case VS_ORDINAL:
     case VS_NOMINAL: mvFmt = "      1e31"; // len 10
                      valFmt = 10; break;
     default        : mvFmt = "       1e31"; // len 11
                      valFmt = 11; break;
                      // was %11.6g 6 is default precision
  }

  // print time at begin of line
  f << std::setw(8) << t;

  // print columns of row on line of file
  for(size_t c = 0; c < d_nrCols; c++)
    if (!v || IS_MV_REAL8(v+c))
       f << mvFmt;
    else {
       double d = v[c];
       if (d_stackInfo.vs() == VS_D)
         d = AppOutputDirection(d);
       f << " " << std::setw(valFmt) << d;
    }
  f << std::endl;
  if (!f.good()) {
      d_fileErrorOccured=true;
      throw com::FileError(d_stackInfo.stackName(),"writing data failed");
  }
}

void calc::FileTimeoutput::flushToFile()
{
  assert(!d_fileErrorOccured);
  std::ofstream f;
  try {
   openFile(f);
  } catch(...) {
    d_fileErrorOccured=true;
    throw;
  }

  // print each row of time table on new line in file
  for(size_t i=0; i < d_step.size(); ++i) {
   if (appHeader != APP_NOHEADER) {
     // write intervening TODO last end is not written
     for(size_t j=d_lastStepToFile+1; j < d_step[i]; ++j)
      printLine(j,0,f);
   }
   printLine(d_step[i],d_value[i],f);
   d_lastStepToFile=d_step[i];
  }
  d_step.clear();
}

void calc::FileTimeoutput::nonspatial(
  const Field *f,
  size_t currentTimeStep)
{
  double *v=setRow(currentTimeStep);
  if (v)
   f->getCell(*v,0);
}

void calc::FileTimeoutput::timeoutput(
  const Field *id,
  const Field *expr,
  size_t currentTimeStep)
{
  // val[0] will hold for id 1, val[2] for id 2, etc.
  double *val = setRow(currentTimeStep);

  if (!val) // do not write this time step
    return;

  int result= detail::doTimeoutput(val,id,expr, d_nrCols);

  if (result) {
    d_fileErrorOccured=true;
    throw std::runtime_error("Failed to add data to timeseries");
  }
}

//! create if \a id contains values > 0, return 0 otherwise
/*! StackInfo::d_vs is updated if created
 */
calc::FileTimeoutput* calc::createFileTimeoutput(
    const StackInfo& stackInfo,
    const Field* id)
{
  size_t nrCols = detail::maxId(id);
  if (nrCols) {
    return new FileTimeoutput(stackInfo,nrCols);
  }
  return 0;
}

calc::MemoryTimeoutput::MemoryTimeoutput(
    const std::string& name,
    size_t             memoryId,
    IOStrategy&        ios):
     TssOutputValue(),
     d_name(name),
     d_memoryId(memoryId),
     d_ios(ios)
{
}

void calc::MemoryTimeoutput::finish()
{
}

void calc::MemoryTimeoutput::timeoutput(
    const Field *id,
    const Field *expr,
    size_t )
{
  size_t nrCols = detail::maxId(id);
  // reuse code above with std::vector<double> in
  // then recast in case of INT4 or UINT1
  std::vector<double> val(nrCols);
  int result = detail::doTimeoutput(&(val[0]), id, expr, nrCols);
  if (result) {
    throw std::runtime_error("Failed to add data to timeseries");
  }

  MemoryExchangeItem *data(0);
  switch(expr->cri()) {
   case CRI_1:
   {
     data = new detail::MemoryExchangeItemTssRow<UINT1>(d_name, d_memoryId,val);
     break;
   }
   case CRI_4:
   {
     data = new detail::MemoryExchangeItemTssRow<INT4>(d_name, d_memoryId,val);
     break;
   }
   case CRI_f:
   {
     data = new detail::MemoryExchangeItemTssRow<REAL4>(d_name, d_memoryId,val);
     break;
   } break;
   default:
    assert(false);
  }
  d_ios.transferMemoryExchangeItemIntoDataTransferArray(data);
}