#ifndef STRONGLY_CONNECTED_COMPONENT_GRAPH_HPP__
#define STRONGLY_CONNECTED_COMPONENT_GRAPH_HPP__

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

#include <crab/common/debug.hpp>
#include <crab/common/types.hpp>
#include <crab/cg/cg.hpp>

/*
   Strongly connected component graph
 */

using namespace boost;

namespace crab {
   namespace analyzer {
     namespace graph_algo {

      template< typename G>
      class scc_graph {

       public:

        typedef typename G::node_t node_t;

       private:

        /// --- begin internal representation of the scc_graph
        struct  vertex_t { std::size_t m_comp; node_t m_repr;};
        typedef adjacency_list<setS, //disallow parallel edges
                               vecS, bidirectionalS,
                               property<vertex_color_t,
                                        default_color_type,
                                        vertex_t> > scc_graph_t;
        typedef boost::shared_ptr <scc_graph_t> scc_graph_ptr;
        typedef typename boost::graph_traits<scc_graph_t>::vertex_descriptor vertex_descriptor_t;
        typedef typename boost::graph_traits<scc_graph_t>::edge_descriptor edge_descriptor_t;
        typedef typename boost::graph_traits<scc_graph_t>::vertex_iterator vertex_iterator;
        typedef typename boost::graph_traits<scc_graph_t>::out_edge_iterator out_edge_iterator;
        typedef typename boost::graph_traits<scc_graph_t>::in_edge_iterator in_edge_iterator;
        /// --- end internal representation of the scc_graph

        typedef boost::unordered_map< node_t, std::size_t > component_map_t;
        typedef boost::unordered_map <std::size_t, vertex_descriptor_t> comp_to_vertex_map_t;
        typedef boost::unordered_map <std::size_t, std::vector<node_t> > comp_members_map_t;

        // Wrapper for edges
        // XXX: BGL complains if we use std::pair<edge_t,edge_t>
        template <typename T>
        struct Edge  {
          T m_s;
          T m_d;
          Edge (){ }
          Edge (T s, T d): m_s (s), m_d (d) { }
          T Src () const { return m_s; }
          T Dest () const { return m_d; }
          bool operator== (const Edge<T> &o) const {
            return (m_s == o.Src () && m_d == o.Dest ());
          }
          bool operator!= (const Edge<T> &o) const {
            return !(*this == o);
          }
        };
        // input graph
        G m_g;
        // if true all members in the same SCC are sorted in
        // pre-order, otherwise post-order.
        bool m_same_scc_order;
        // output dag
        scc_graph_ptr m_sccg;
        // map each m_g node to a SCC id
        component_map_t m_comp_map;
        // map each SCC id to an internal m_sccg's node id
        comp_to_vertex_map_t m_comp_to_vertex_map;
        // map each SCC id to a vector of m_g's nodes (its members)
        comp_members_map_t m_comp_members_map;
       public:

        typedef Edge<node_t> edge_t;

       private:

        struct MkNode :
            public std::unary_function < vertex_descriptor_t, node_t > {

          scc_graph_t* _g;

          MkNode (): _g(nullptr) { }
          MkNode (scc_graph_t* g): _g (g) { }
          node_t& operator()(const vertex_descriptor_t& v) const {
            return (*_g)[v].m_repr;
          }
        };

        struct MkEdge :
            public std::unary_function < edge_descriptor_t, Edge<node_t> > {

          scc_graph_t *_g;

          MkEdge(): _g(nullptr) {}
          MkEdge(scc_graph_t *g): _g (g) { }
          Edge<node_t> operator()(const edge_descriptor_t& e) const {
            node_t& s = (*_g) [boost::source (e, *_g)].m_repr;
            node_t& t = (*_g) [boost::target (e, *_g)].m_repr;
            return Edge<node_t> (s,t);
          }
        };

        std::size_t get_comp_id (const node_t&n) const {
          auto it = m_comp_map.find (n);
          assert (it != m_comp_map.end ());
          return it->second;
        }

        struct preorder_visitor: public boost::default_dfs_visitor {
          vector<node_t> m_order_vs;
          void discover_vertex(node_t v, G g) { m_order_vs.push_back (v); }
        };

        struct postorder_visitor: public boost::default_dfs_visitor {
          vector<node_t> m_order_vs;
          void finish_vertex(node_t v, G g) { m_order_vs.push_back (v); }
        };

        template<typename OrderVis>
        vector<node_t> sort () const {
          typedef boost::unordered_map< node_t, default_color_type > color_map_t;
          color_map_t color;
          for (auto v : boost::make_iterator_range (vertices (m_g))) {
            color[v] = default_color_type();
          }
          boost::associative_property_map< color_map_t > cm (color);

          // find root
          node_t root;
          bool root_found = false;
          for (auto v: boost::make_iterator_range (vertices (m_g))) {
            if (in_degree (v, m_g) == 0) {
              root = v;
              root_found = true;
              break;
            }
          }

          OrderVis vis;
          if (root_found)
            boost::detail::depth_first_visit_impl (m_g, root, vis, cm,
                                                   boost::detail::nontruth2());

          for (auto u: boost::make_iterator_range (vertices (m_g))) {
            if (get(cm, u) == default_color_type::white_color)
              boost::detail::depth_first_visit_impl(m_g, u, vis, cm, boost::detail::nontruth2());
          }

          assert (vis.m_order_vs.size () == num_vertices (m_g));

          return vis.m_order_vs;
        }

        bool check_comp_map () {
          // --- Check that all vertices in the graph m_g are keys in
          //     the component map
          for (auto v: boost::make_iterator_range (vertices (m_g))) {
             auto it = m_comp_map.find (v);
             if (it == m_comp_map.end ())
               return false;
          }
          return true;
        }

       public:
        typedef boost::transform_iterator<MkNode, vertex_iterator> node_iterator;
        typedef boost::transform_iterator<MkEdge, in_edge_iterator> pred_iterator;
        typedef boost::transform_iterator<MkEdge, out_edge_iterator> succ_iterator;

       public:

        scc_graph(G g, bool order = false /*default post-order*/)
            : m_g (g),
              m_same_scc_order (order),
              m_sccg (boost::make_shared<scc_graph_t>()) {

          CRAB_LOG ("sccg", crab::outs() << g << "\n");

          typedef boost::unordered_map< node_t, node_t > root_map_t;
          typedef boost::unordered_map< node_t, default_color_type > color_map_t;
          typedef boost::associative_property_map< component_map_t > property_component_map_t;
          typedef boost::associative_property_map< root_map_t > property_root_map_t;
          typedef boost::associative_property_map< color_map_t > property_color_map_t;

          component_map_t discover_time;
          root_map_t _root_map;
          color_map_t color_map;

          for (auto const &v : boost::make_iterator_range (vertices (m_g))) {
            m_comp_map [v] = 0;
            color_map [v] = default_color_type();
            discover_time [v] = 0;
            _root_map [v] = node_t ();
          }

          boost::strong_components(m_g,
                                   property_component_map_t(m_comp_map),
                                   root_map(property_root_map_t(_root_map))
                                   .color_map(property_color_map_t(color_map))
                                   .discover_time_map(property_component_map_t(discover_time)));

          CRAB_LOG ("sccg",
              crab::outs() << "comp map: \n";
              for (auto p: m_comp_map) {
               crab::outs() <<"\t" << p.first << " --> " << p.second << "\n";
              });

          // build SCC Dag

          for (auto p : m_comp_map) {
            auto it = m_comp_to_vertex_map.find (p.second );
            if (it != m_comp_to_vertex_map.end ())
              continue;
            vertex_descriptor_t v = add_vertex (*m_sccg);
            (*m_sccg) [v].m_comp = p.second;
            m_comp_to_vertex_map.insert (std::make_pair (p.second, v));
            CRAB_LOG("sccg",
                     crab::outs() << "Added scc graph node " << p.second <<  "--- id=" <<  v << "\n";);
          }

          for (const node_t &u : boost::make_iterator_range (vertices (m_g))) {
            for (auto e : boost::make_iterator_range (out_edges (u, m_g))) {
              const node_t &d = target (e, m_g);

              if (m_comp_map [u] == m_comp_map [d])
                continue;

              auto res = add_edge (m_comp_to_vertex_map[m_comp_map [u]],
                                   m_comp_to_vertex_map[m_comp_map [d]],
                                   *m_sccg);

              if (res.second)
                CRAB_LOG("sccg",
                         crab::outs() << "Added scc graph edge "
                         << m_comp_map [u] << " --> " <<  m_comp_map [d] << "\n");
            }
          }

          assert (check_comp_map ());

          // Build a map from scc id to their node members
          for (auto v: (m_same_scc_order ? sort<preorder_visitor> () :
                                           sort<postorder_visitor> ())) {
            std::size_t id = m_comp_map [v];
            auto it  = m_comp_members_map.find (id);
            if (it != m_comp_members_map.end ()) {
              it->second.push_back (v);
            }
            else {
              std::vector<node_t> comp_mems;
              comp_mems.push_back (v);
              m_comp_members_map.insert (std::make_pair (id, comp_mems));
              // update the representative in m_sccg
              //
              // The representative is similar to the values in the
              // root_map's entries. However, we found cases where two members
              // of the same SCC have assigned a different root. I
              // think this contradicts to what the BOOST doc says but
              // I didn't have time to figure out the problem so we
              // choose our own representative.
              (*m_sccg) [m_comp_to_vertex_map[id]].m_repr = v;
            }
          }
          CRAB_LOG ("sccg",
               crab::outs() << "Built SCC graph \n";
               write (crab::outs());
               );
        }

        // return the members of the scc component that contains n
        std::vector<node_t>& get_component_members (const node_t& n) {
          return m_comp_members_map[m_comp_map [n]];
        }

        std::pair<node_iterator, node_iterator>
        nodes () const {
          auto p = boost::vertices (*m_sccg);
          return std::make_pair (make_transform_iterator (p.first, MkNode (&*m_sccg)),
                                 make_transform_iterator (p.second, MkNode (&*m_sccg)));
        }

        std::pair<succ_iterator, succ_iterator>
        succs (const node_t &n) const {
          auto It = m_comp_to_vertex_map.find(get_comp_id (n));
          if (It == m_comp_to_vertex_map.end())
            CRAB_ERROR("Sccg could not find node!");

          auto p = boost::out_edges (It->second, *m_sccg);
          return std::make_pair (make_transform_iterator (p.first, MkEdge (&*m_sccg)),
                                 make_transform_iterator (p.second, MkEdge (&*m_sccg)));
        }

        std::pair<pred_iterator, pred_iterator>
        preds (const node_t &n) const {
          auto It = m_comp_to_vertex_map.find(get_comp_id (n));
          if (It == m_comp_to_vertex_map.end())
            CRAB_ERROR("Sccg could not find node!");

          auto p = boost::in_edges (It->second, *m_sccg);
          return std::make_pair (make_transform_iterator (p.first, MkEdge (&*m_sccg)),
                                 make_transform_iterator (p.second, MkEdge (&*m_sccg)));
        }

        std::size_t num_nodes () const {
           return boost::num_vertices (*m_sccg);
        }

        std::size_t num_succs (const node_t &n) const {
          auto It = m_comp_to_vertex_map.find(get_comp_id (n));
          if (It == m_comp_to_vertex_map.end())
            CRAB_ERROR("Sccg could not find node!");

          return boost::out_degree (It->second, *m_sccg);
        }

        std::size_t num_preds (const node_t &n) const {
          auto It = m_comp_to_vertex_map.find(get_comp_id (n));
          if (It == m_comp_to_vertex_map.end())
            CRAB_ERROR("Sccg could not find node!");

          return boost::in_degree (It->second, *m_sccg);
        }

        void write(crab_os& o) const {
          o << "SCCG=\nvertices={";
          for (auto v: boost::make_iterator_range (nodes ()))
             o << v << ";";
          o << "}\n";

          o <<"edges=\n";
          for (auto v: boost::make_iterator_range (nodes ())){
            if (num_succs (v) > 0) {
              for (auto e: boost::make_iterator_range (succs (v)))  {
                o << e.Src () << "--> " << e.Dest () << "\n";
              }
            }
          }

          CRAB_LOG ("sccg",
               o << "Component map: \n";
               for (auto p: m_comp_map) {
                 o <<"\t" << p.first << " --> SCC ID " << p.second << "\n";
               });

        }

      };
     } // end namespace graph_algo
   } // end namespace analyzer
} // end namespace crab

#endif


#include <cstdlib>
#include <iostream>
#include <string>
#include <ios>
#include <boost/crc.hpp>

int main()
{
    std::string s("bincrafters");
    boost::crc_32_type crc32;
    crc32.process_bytes(s.c_str(), s.size());
    std::cout << "CRC32 of " << s << " is: " << std::hex << "0x" << crc32.checksum() << std::endl;
    return EXIT_SUCCESS;
}


//drc-descript-brief 0016_1349291130753495
// - read in 0016* files
// match to all others in that directory
//
// this was finished in jan 2012.
// next step is to create an application which uses this as part of an lcm stream

#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>

#include <dirent.h>

#include <GL/gl.h>

#include <boost/foreach.hpp>
#include <boost/shared_ptr.hpp>

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/nonfree/features2d.hpp"

#include <bot_lcmgl_client/lcmgl.h>

#include "registeration.hpp"

#include <pronto_utils/pronto_vis.hpp>

using namespace std;

//lcm_t* _bot_param_lcm;
//pronto_vis* pc_vis_;

Reg::Reg(boost::shared_ptr<lcm::LCM> &lcm_):
        lcm_(lcm_){

  lcmgl_= bot_lcmgl_init(lcm_->getUnderlyingLCM(), "registeration");

  int reset =0;
  // Vis Config:
  pc_vis_ = new pronto_vis(lcm_->getUnderlyingLCM());
  float colors_0f[] ={1.0,0.0,0.0};
  vector <float> colors_0;
  colors_0.assign(colors_0f,colors_0f+4*sizeof(float));
  float colors_1f[] ={0.0,0.0,1.0};
  vector <float> colors_1;
  colors_1.assign(colors_1f,colors_1f+4*sizeof(float));

  // obj: id name type reset
  // pts: id name type reset objcoll usergb rgb
  pc_vis_->obj_cfg_list.push_back( obj_cfg(1000,"Pose A",5,reset) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1002,"Cloud A"     ,1,reset, 1000,1,colors_0));
  colors_0[0] = 0.7;
  colors_0[1] = 0.7;
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(1001,"Cloud A - inliers"     ,1,reset, 1000,1,colors_0));

  pc_vis_->obj_cfg_list.push_back( obj_cfg(2000,"Pose B",5,reset) );
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(2002,"Cloud B"     ,1,reset, 2000,1,colors_1));
  colors_1[1] = 0.7;
  colors_1[2] = 0.7;
  pc_vis_->ptcld_cfg_list.push_back( ptcld_cfg(2001,"Cloud B - inliers"     ,1,reset, 2000,1,colors_1));

  verbose_ =false;
}

void Reg::draw_reg(std::vector<ImageFeature> features,    int status, Eigen::Isometry3d pose)
{
  // reorder as:
  // z, -x, -y (or 2,-0,-1)

  bot_lcmgl_push_matrix(lcmgl_);
  bot_lcmgl_rotated(lcmgl_, -90, 0, 0, 1);
  bot_lcmgl_rotated(lcmgl_, -90, 1, 0, 0);

//  bot_lcmgl_translated(_lcmgl, 0, pose.translation().y(), 0);

  bot_lcmgl_color3f(lcmgl_, 0, 1, 1);
  bot_lcmgl_point_size(lcmgl_, 1.5f);
  bot_lcmgl_begin(lcmgl_, GL_POINTS);

  if (status==0){
    bot_lcmgl_color3f(lcmgl_, 0, 0, 1);
  }else{ // status 1 when changing key frames:
    bot_lcmgl_color3f(lcmgl_, 1, 0, 0);
  }
  for (size_t i=0; i < features.size(); ++i) {
    ImageFeature f = features[i];

    bot_lcmgl_vertex3f(lcmgl_, f.xyz[0],  f.xyz[1], f.xyz[2]);
//    bot_lcmgl_vertex3f(lcmgl_, f.xyz[2],  -f.xyz[0],-f.xyz[1]);
  }
  bot_lcmgl_end(lcmgl_);
  bot_lcmgl_pop_matrix(lcmgl_);

  // disabling this allows draw_reg to daisy chain onto draw()
  //bot_lcmgl_switch_buffer(lcmgl_);
}

void Reg::draw_both_reg(std::vector<ImageFeature> features0,    std::vector<ImageFeature> features1,
    Eigen::Isometry3d pose0,   Eigen::Isometry3d pose1){
  draw_reg(features0,    0,  pose0);
  draw_reg(features1,    1, pose1);
  bot_lcmgl_switch_buffer(lcmgl_);
}

void features2cloud(std::vector<ImageFeature> features, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud){
  cloud->width   = features.size();
  cloud->height   = 1;
  cloud->points.resize (cloud->width  *cloud->height);
  for (size_t i=0;i<features.size(); i++){
    cloud->points[i].x = features[i].xyz[0];
    cloud->points[i].y = features[i].xyz[1];
    cloud->points[i].z = features[i].xyz[2];
    cloud->points[i].r = 1;
    cloud->points[i].g = 1;
    cloud->points[i].b = 1;
  }
}

// Send a subset of the feature set:
void features2cloud(std::vector<ImageFeature> features, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud,
    std::vector<int> features_indices){
  cloud->width   = features_indices.size();
  cloud->height   = 1;
  cloud->points.resize (cloud->width  *cloud->height);
  for (size_t j=0;j<features_indices.size(); j++){
    int i = features_indices[j];
    cloud->points[j].x = features[i].xyz[0];
    cloud->points[j].y = features[i].xyz[1];
    cloud->points[j].z = features[i].xyz[2];
    cloud->points[j].r = 1;
    cloud->points[j].g = 1;
    cloud->points[j].b = 1;
  }
}

void Reg::send_both_reg(std::vector<ImageFeature> features0,    std::vector<ImageFeature> features1,
    Eigen::Isometry3d pose0,   Eigen::Isometry3d pose1,
    int64_t utime0, int64_t utime1           ){

  Eigen::Matrix3d M;
  M <<  0,  0, 1,
       -1,  0, 0,
        0, -1, 0;
  pose0 = M * pose0;
  pose1 = M * pose1;

  Isometry3dTime pose0T = Isometry3dTime(utime0, pose0);
  pc_vis_->pose_to_lcm_from_list(1000, pose0T);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud0 (new pcl::PointCloud<pcl::PointXYZRGB> ());
  features2cloud(features0,cloud0);
  pc_vis_->ptcld_to_lcm_from_list(1002, *cloud0, utime0, utime0);

  Isometry3dTime pose1T = Isometry3dTime(utime1, pose1);
  pc_vis_->pose_to_lcm_from_list(2000, pose1T);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB> ());
  features2cloud(features1,cloud1);
  pc_vis_->ptcld_to_lcm_from_list(2002, *cloud1, utime1, utime1);

}

void draw_inliers(cv::Mat &imgs, std::vector<ImageFeature> features0,    std::vector<ImageFeature> features1,
    std::vector<int> feature_inliers0,    std::vector<int> feature_inliers1){

  cv::Point p;
  p.x =3; p.y = 10;
  CvScalar color_out = CV_RGB(255,0,0);
  for (size_t j=0;j< feature_inliers0.size(); j++){
    int i0 = feature_inliers0[j];
    cv::Point p0;
    p0.x = features0[i0].base_uv[0];
    p0.y = features0[i0].base_uv[1];
    cv::circle( imgs, p0, 5, color_out, 0 );

    int i1 = feature_inliers1[j];
    cv::Point p1;
    p1.x = features1[i1].base_uv[0] + imgs.cols/2; // offset by half the double image with
    p1.y = features1[i1].base_uv[1];
    cv::circle( imgs, p1, 5, color_out, 0 );
    cv::line(imgs, p0, p1, color_out, 2);
  }
}

void Reg::send_both_reg_inliers(std::vector<ImageFeature> features0,    std::vector<ImageFeature> features1,
    Eigen::Isometry3d pose0,   Eigen::Isometry3d pose1,
    std::vector<int> feature_inliers0,    std::vector<int> feature_inliers1 ,
    int64_t utime0, int64_t utime1){

  Eigen::Matrix3d M;
  M <<  0,  0, 1,
       -1,  0, 0,
        0, -1, 0;
  pose0 = M * pose0;
  pose1 = M * pose1;

  Isometry3dTime pose0T = Isometry3dTime(utime0, pose0);
  pc_vis_->pose_to_lcm_from_list(1000, pose0T);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud0 (new pcl::PointCloud<pcl::PointXYZRGB> ());
  features2cloud(features0,cloud0,feature_inliers0);
  pc_vis_->ptcld_to_lcm_from_list(1001, *cloud0, utime0, utime0);

  Isometry3dTime pose1T = Isometry3dTime(utime1, pose1);
  pc_vis_->pose_to_lcm_from_list(2000, pose1T);

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB> ());
  features2cloud(features1,cloud1,feature_inliers1);
  pc_vis_->ptcld_to_lcm_from_list(2001, *cloud1, utime1, utime1);

}

// 'lexicographic' comparison
static bool DMatch_lt(const cv::DMatch& a, const cv::DMatch& b) {
  //return ( (a.trainIdx < b.trainIdx) || (a.queryIdx < b.queryIdx) );
  if (a.trainIdx != b.trainIdx) { return a.trainIdx < b.trainIdx; }
  return a.queryIdx < b.queryIdx;
}

void features_to_keypoints(const std::vector<ImageFeature> & features, std::vector<cv::KeyPoint> & kpts) {
  kpts.clear();
  for(std::vector<ImageFeature>::const_iterator it=features.begin(); it != features.end(); ++it) {
    const ImageFeature & f = *it;
    // @todo use Mei's true scale here
    //    kpts.push_back(cv::KeyPoint(f.uvd(0), f.uvd(1), 1.0));
    kpts.push_back(cv::KeyPoint(f.base_uv(0), f.base_uv(1), 20.0)); // was in hordur's code
  }
}

void compute_descriptors(cv::Mat &image, vector<ImageFeature> & features, std::string & name, cv::DescriptorExtractor & extractor,
    cv::Mat &descriptors, std::vector<cv::KeyPoint>& keypoints){
  features_to_keypoints(features, keypoints);
  extractor.compute(image, keypoints, descriptors);
}

void read_features(std::string fname,
    std::vector<ImageFeature>& features ){

  printf( "About to read: %s - ",fname.c_str());
  int counter=0;
  string line0;
  std::ifstream myfile (fname.c_str());
  if (myfile.is_open()){

    getline (myfile,line0);
    //cout << line0 << " is first line\n";

    counter =0;
    while ( myfile.good() ){
      string line;
      getline (myfile,line);
      if (line.size() > 4){
        ImageFeature f;
        int i,track_id;
        double v[15];
        int d[3];
        int res = sscanf(line.c_str(), "%d,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d,%d",&i,
            &(v[0]), &(v[1]), &(v[2]), &(v[3]), &(v[4]), // id, uv, base_uv
            &(v[5]), &(v[6]), &(v[7]),  &(v[8]), &(v[9]), &(v[10]),// uvd xyz
            &(v[11]), &(v[12]), &(v[13]), &(v[14]), // xyzd
            &(d[0]), &(d[1]), &(d[2])        );

        f.track_id=v[0]; f.uv[0]=v[1];f.uv[1]=v[2];
        f.base_uv[0]=v[3];f.base_uv[1]=v[4];
        f.uvd[0]=v[5];f.uvd[1]=v[6];f.uvd[2]=v[7];
        f.xyz[0]=v[8];f.xyz[1]=v[9];f.xyz[2]=v[10];
        f.xyzw[0]=v[11];f.xyzw[1]=v[12];f.xyzw[2]=v[13];f.xyzw[3]=v[14];
        f.color[0] = d[0];f.color[1] = d[1];f.color[2] = d[2];

        /*

        cout << line << " is line\n";
        cout << "i: " << i <<"\n";
        cout << "f.track_id: " << f.track_id <<"\n";
        cout << "f.uv: " << f.uv[0] << " "<< f.uv[1] <<"\n";
        cout << "f.base_uv: " << f.base_uv[0] << " "<< f.base_uv[1] <<"\n";
        cout << "f.uvd: " << f.uvd[0] << " "<< f.uvd[1]<< " "<< f.uvd[2]<<"\n";
        cout << "f.xyz: " << f.xyz[0] << " "<< f.xyz[1]<< " "<< f.xyz[2]<<"\n";
        cout << "f.xyzw: " << f.xyzw[0] << " "<< f.xyzw[1]<< " "<< f.xyzw[2]<< " "<< f.xyzw[3]<<"\n";
        cout << "f.color: " << (int)f.color[0] << " "<< (int)f.color[1] << " "<< (int)f.color[2] <<"\n";
         */
        features.push_back(f);
      }
    }
    myfile.close();
  } else{
    printf( "Unable to open features file\n%s",fname.c_str());
    return;
  }
  cout << "read " << features.size() << " features\n";
}

Eigen::Isometry3d pose_estimate(FrameMatchPtr match,
    std::vector<char> & inliers,
    Eigen::Isometry3d & motion,
    Eigen::MatrixXd & motion_covariance,
    Eigen::Matrix<double, 3, 4> & proj_matrix) {
  using namespace pose_estimator;

  PoseEstimator pe(proj_matrix);

  if ((match->featuresA_indices.size()!=match->featuresB_indices.size()))
    cout <<    "Number of features doesn't match\n";

  size_t num_matches = match->featuresA_indices.size();

  if(num_matches < 3)
    cout << "Need at least three matches to estimate pose";

  motion.setIdentity();
  motion_covariance.setIdentity();

  Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor> src_xyzw(4, num_matches);
  Eigen::Matrix<double, 4, Eigen::Dynamic, Eigen::ColMajor>dst_xyzw(4, num_matches);
  for (size_t i=0; i < num_matches; ++i) {

    //    const ImageFeature& featureA(int i) const {
    //      assert (frameA);
    //    int ix = featuresA_indices.at(i);
    //      return frameA->features().at(ix);
    //    }
    //src_xyzw.col(i) = match->featureA(i).xyzw;
    //dst_xyzw.col(i) = match->featureB(i).xyzw;

    int ixA = match->featuresA_indices.at(i);
    int ixB = match->featuresB_indices.at(i);
    //cout << ixA << " | " << ixB << "\n";
    //cout << match->featuresA.size() << " fA size\n";
    //cout <<  match->featuresA[ixA].xyzw[0] << "\n";
    //cout <<  match->featuresA[ixA].xyzw[0] << "\n";

    src_xyzw.col(i) = match->featuresA[ixA].xyzw;
    dst_xyzw.col(i) = match->featuresB[ixB].xyzw;
  }

  // PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance);
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> motion_covariance_col_major;

  pose_estimator::PoseEstimateStatus status = pe.estimate(src_xyzw, dst_xyzw, &inliers, &motion, &motion_covariance_col_major); //&motion_covariance);
  motion_covariance = motion_covariance_col_major;

  match->status = status;
  match->delta = motion;

  /*
  int num_inliers = std::accumulate(inliers.begin(), inliers.end(), 0);
  const char* str_status = PoseEstimateStatusStrings[status];
  std::cerr << "Motion: " << str_status << " feats: " << match->featuresA_indices.size()
            << " inliers: " << num_inliers
            << " Pose: " << motion
            << " Delta: " << match->delta
            //<< " Cov:\n" << motion_covariance << "\n"
            << " " << match->timeA << " " << match->timeB << std::endl;
   */

  return motion;
}

void Reg::send_lcm_image(cv::Mat &img, std::string channel ){
  int n_colors=3;

  bot_core_image_t image;
  image.utime =0;
  image.width = img.cols;
  image.height= img.rows;
  image.row_stride =n_colors*img.cols;
  image.pixelformat =BOT_CORE_IMAGE_T_PIXEL_FORMAT_RGB;
  image.size =n_colors*img.cols*img.rows;
  image.data = img.data;
  image.nmetadata =0;
  image.metadata = NULL;
  bot_core_image_t_publish( lcm_->getUnderlyingLCM() , channel.c_str(), &image);

}

void Reg::align_images(cv::Mat &img0, cv::Mat &img1,
                           std::vector<ImageFeature> &features0, std::vector<ImageFeature> &features1,
                           int64_t utime0, int64_t utime1, FrameMatchPtr &match){
    /// 2. Extract Image Descriptors:
  std::vector<cv::KeyPoint> keypoints0, keypoints1;
  cv::Mat descriptors0, descriptors1;
  cv::BriefDescriptorExtractor extractor(32); // size of descriptor in bytes
  std::string desc_name= "brief";
  compute_descriptors(img0, features0, desc_name, extractor,descriptors0,keypoints0);
  compute_descriptors(img1, features1, desc_name, extractor,descriptors1,keypoints1);

  if (verbose_){
    cout << descriptors0.rows << "descripters in 0\n";
    cout << descriptors1.rows << "descripters in 1\n";
  }

  /// 3: Matching descriptor vectors with a brute force matcher
  cv::BFMatcher matcher(cv::NORM_HAMMING); // used by hordur
  std::vector< cv::DMatch > matches0in1,matches1in0;
  matcher.match(descriptors0, descriptors1, matches0in1); // each feature in 0 found in 1
  matcher.match(descriptors1, descriptors0, matches1in0); // each feature in 1 found in 0
  BOOST_FOREACH (cv::DMatch& match, matches1in0) {
    std::swap(match.trainIdx, match.queryIdx);
  }

  /// 3b keep intersection, aka the mutual best matches.
  std::sort(matches0in1.begin(), matches0in1.end(), DMatch_lt);
  std::sort(matches1in0.begin(), matches1in0.end(), DMatch_lt);
  std::vector<cv::DMatch> matches;
  std::set_intersection(matches0in1.begin(), matches0in1.end(),
      matches1in0.begin(), matches1in0.end(),
      std::back_inserter(matches),
      DMatch_lt);
  if (verbose_){
    std::cout << matches0in1.size() << " matches0in1 found\n";
    std::cout << matches1in0.size() << " matches1in0 found\n";
    std::cout << matches.size() << " intersect matches found\n";
  }

  /// 3c Draw descriptor matches
  cv::Mat img_matches0in1, img_matches1in0;
  cv::drawMatches( img0, keypoints0, img1, keypoints1, matches0in1, img_matches0in1 );
//  imshow("Matches0in1", img_matches0in1 );

  cv::drawMatches( img0, keypoints0, img1, keypoints1, matches1in0, img_matches1in0  );
 // imshow("Matches1in0 [matches1in0 reordered]", img_matches1in0 );

  cv::Mat img_matches_inter;
  cv::drawMatches( img0, keypoints0, img1, keypoints1, matches, img_matches_inter );
 // imshow("Matches Intersection", img_matches_inter );

  /// 4 Get delta pose estimate:
  std::vector<int> idxA;
  std::vector<int> idxB;
  for (size_t i = 0; i < descriptors0.rows; ++i){
    //if (1==1){//(desc_A->valid[i]) {
    //descriptors0.row(i).copyTo(descriptors0.row(validA));
    idxA.push_back(i);
    //validA++;
    //}
  }
  for (size_t i = 0; i < descriptors1.rows; ++i){
    //if (desc_B->valid[i]) {
    //descriptorsB.row(validB) = desc_B->descriptors.row(i);
    //desc_B->descriptors.row(i).copyTo(descriptorsB.row(validB));
    idxB.push_back(i);
    //validB++;
    //}
  }

  std::vector<char> inliers;
  int num_inliers = 0;
  Eigen::Isometry3d motion;
  BOOST_FOREACH (const cv::DMatch& dmatch, matches) {
    //match->featuresA_indices.push_back(idxA[dmatch.queryIdx]);
    //match->featuresB_indices.push_back(idxB[dmatch.trainIdx]);
    match->featuresA_indices.push_back(dmatch.queryIdx);
    match->featuresB_indices.push_back(dmatch.trainIdx);
  }
  BOOST_FOREACH (const ImageFeature& feature, features0) {
    match->featuresA.push_back(feature);
  }
  BOOST_FOREACH (const ImageFeature& feature, features1) {
    match->featuresB.push_back(feature);
  }

  BOOST_FOREACH (cv::DMatch& dmatch, matches) {
    //dmatch.queryIdx = idxA[dmatch.queryIdx];
    //dmatch.trainIdx = idxB[dmatch.trainIdx];
  }
  if (matches.size() >= 3) {
    Eigen::Isometry3d delta;
    delta.setIdentity();
    Eigen::MatrixXd covariance;

    Eigen::Matrix<double, 3, 4> projection_matrix;
    // fx 0  cx 0
    // 0  fy cy 0
    // 0  0  0  0
    // from newcollege_stereo config: (left)
    //projection_matrix << 389.956085,  0, 254.903519, 0,
    //                     0, 389.956085,  201.899490, 0,
    //                     0,   0,   1, 0;

    // bumblebee left:
    //projection_matrix << 836.466,  0, 513.198, 0,
    //                     0, 835.78,  397.901, 0,
    //                     0,   0,   1, 0;

    // loader multisense left:
    projection_matrix << 606.0344848632812,  0, 512.0, 0,
                         0, 606.0344848632812,  272.0, 0,
                         0,   0,   1, 0;

    delta = pose_estimate(match, inliers, motion, covariance,
        projection_matrix);

    Eigen::Quaterniond delta_quat = Eigen::Quaterniond(delta.rotation());
    if(verbose_){
      cout << delta.translation().x() << " "
        << delta.translation().y() << " "
        << delta.translation().z() << " | "
        << delta_quat.w() << " "
        << delta_quat.x() << " "
        << delta_quat.y() << " "
        << delta_quat.z() << "\n";
    }

    Eigen::Isometry3d nullpose;
    nullpose.setIdentity();
    //    draw_both_reg(features0, features1,nullpose0,delta);
    draw_both_reg(features0, features1,nullpose,delta);

    size_t j = 0;
    for (size_t i = 0; i < inliers.size(); ++i){
      if (inliers[i]) {
        match->featuresA_indices[j] = match->featuresA_indices[i];
        match->featuresB_indices[j] = match->featuresB_indices[i];
        j++;
      }
    }
    match->featuresA_indices.resize(j);
    match->featuresB_indices.resize(j);
    match->n_inliers = inliers.size();//
    //cout << inliers.size() << " inliers found\n";
    if (match->status == pose_estimator::SUCCESS){ // used 15 earlier... ask Hordur
      if (verbose_){
	      // all the features:
	      send_both_reg(features0, features1,delta,nullpose, utime0, utime1);

        // just the inliers:
        send_both_reg_inliers(features0, features1,nullpose,delta,
                        match->featuresA_indices, match->featuresB_indices,
                        utime0, utime1);
      }

      // Draw the inlier set:
      cv::Mat img_copy = img_matches_inter ;
      draw_inliers(img_copy,features0, features1,match->featuresA_indices, match->featuresB_indices);
      //    imshow("Matches Inliers", img_copy);
      send_lcm_image(img_copy, "REGISTERATION" );
    }
  } else {
    // TODO: this should have a different code e.g. insufficient matches:
    match->status = pose_estimator::INSUFFICIENT_INLIERS;
    num_inliers = 0;
  }

}


#include <glm/models/normal.hpp>

#include <armadillo>

using namespace arma;

normal::normal(const std::string &link_name)
    : glm_model( "normal", link_name )
{
}

normal::normal(glm_link *link)
    : glm_model( "normal", link )
{
}

normal::~normal()
{
}

bool
normal::valid_mu(const arma::vec &mu) const
{
    return mu.is_finite( );
}

vec
normal::var(const arma::vec &mu) const
{
    return arma::ones<arma::vec>( mu.n_elem );
}

double
normal::dispersion(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float k) const
{
    return arma::as_scalar( ( arma::trans( (y - mu) % ( y - mu ) ) * ( 1 - missing ) ) / ( arma::sum( 1 - missing ) - k ) );
}

double
normal::likelihood(const arma::vec &mu, const arma::vec &y, const arma::uvec &missing, float dispersion) const
{
    float sigma2 = dispersion;
    return arma::as_scalar( ( -0.5*log( 2 * datum::pi ) -0.5*log( sigma2 ) - (1.0/(2*sigma2))*arma::trans( ( ( y - mu ) % ( y - mu ) ) ) ) * ( 1 - missing ) );
}

bool
normal::is_binary() const
{
    return false;
}


//
// Created by qfeng on 17-5-25.
//

#include "model/linear_model.h"
#include <fstream>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include "log/algocomp_log.h"
#include "encode/feature_hash.h"
#include <iostream>
#include <boost/algorithm/string.hpp>

using namespace boost::algorithm;

namespace algocomp {

    LinearModel::LinearModel(uint64_t weight_size) {
        _weights.resize(weight_size);
        _logger = spdlog::get(ACLog::_logger_name);
        _logger->info("Initialize linear model with weight_size={}.", weight_size);
    }

    LinearModel::~LinearModel() {
        _weights.setZero();
        _logger->info("Leave linear model.");
    }

    uint64_t LinearModel::weight_size() {
        return _weights.size();
    }

    float LinearModel::get(uint64_t index) {
        return _weights.coeff(index);
    }

    void LinearModel::update_weight(uint64_t index, float value) {
        if (value != 0) _weights.coeffRef(index) = value;
    }

    float LinearModel::score(const Eigen::SparseVector<float> &sample) {
        // Eigen dot func will assert size of two vector
        return _bias + _weights.dot(sample);
    }

    unique_ptr<LinearModel> LinearModel::load_vw_model(const string &model_path) {
        auto logger = spdlog::get(ACLog::_logger_name);
        try {
            uint64_t constant_hash_code = 0;
            ifstream model_file;
            model_file.open(model_path);
            if (model_file.is_open()) {
                unique_ptr<LinearModel> output_model;
                string line;
                bool start_reading_weights = false;
                while (!model_file.eof()) {
                    getline(model_file, line);
                    if (start_reading_weights && line.length() > 0) {
                        vector<string> splits;
                        boost::algorithm::split(splits, line, boost::algorithm::is_any_of(":"));
                        uint64_t index = boost::lexical_cast<uint64_t>(splits[0]);
                        float value = boost::lexical_cast<float>(splits[1]);
                        // Get the value of intercept
                        if (index == constant_hash_code) output_model->_bias = value;
                        else output_model->update_weight(index, value);
                    } else if (boost::algorithm::starts_with(line, "bits")) {
                        vector<string> splits;
                        boost::algorithm::split(splits, line, boost::algorithm::is_any_of(":"));
                        uint64_t bits = boost::lexical_cast<uint64_t>(splits[1]);
                        uint64_t hash_size = 1 << bits;
                        output_model = unique_ptr<LinearModel>(new LinearModel(hash_size));
                        // Get the index of intercept
                        constant_hash_code = FeatureHash::hash_constant(hash_size);
                    } else if (boost::algorithm::starts_with(line, ":")) {
                        start_reading_weights = true;
                    }
                }
                if (model_file.is_open()) model_file.close();
                if (output_model) {
                    logger->info("Natively loaded vw linear model from {}.", model_path);
                    return output_model;
                } else logger->error("Fail to read model file when natively loading vw linear model from {}.", model_path);
            } else logger->error("Fail to open model file when natively loading vw linear model from {}.", model_path);
        } catch (exception &ex) {
            logger->error("Fail to natively load vw linear model from {}, detail: {}.", model_path, ex.what());
        }
        return nullptr;
    }

    unique_ptr<LinearModel> LinearModel::load_skl_model(const string &model_path) {
        auto logger = spdlog::get(ACLog::_logger_name);
        try {
            ifstream model_file;
            model_file.open(model_path);
            if (model_file.is_open()) {
                unique_ptr<LinearModel> output_model = unique_ptr<LinearModel>(new LinearModel(pow(2, 30)));
                string line;
                while (!model_file.eof()) {
                    getline(model_file, line);
                    line = trim_copy(line);
                    if (line.length() == 0) continue;
                    vector<string> splits;
                    boost::algorithm::split(splits, line, boost::algorithm::is_any_of(" "));
                    if (splits.size() != 2) {
                        logger->error("Invalid skl model coef {}", line);
                        continue;
                    }

                    if (splits[0] == "0") {
                        float value = boost::lexical_cast<float>(splits[1]);
                        output_model->_bias = value;
                    } else {
                        uint64_t index = boost::lexical_cast<uint64_t>(splits[0]);
                        float value = boost::lexical_cast<float>(splits[1]);
                        output_model->update_weight(index, value);
                    }
                }
                if (model_file.is_open()) model_file.close();
                if (output_model) {
                    logger->info("Natively loaded skl linear model from {}.", model_path);
                    return output_model;
                } else logger->error("Fail to read model file when natively loading skl linear model from {}.", model_path);
            } else logger->error("Fail to open model file when natively loading skl linear model from {}.", model_path);
        } catch (exception &ex) {
            logger->error("Fail to natively load skl linear model from {}, detail: {}.", model_path, ex.what());
        }
        return nullptr;

    }

}


//
// Created by 关鑫 on 2019-05-30.
//

#include <algorithm>
#include <cstdio>
#include <iostream>
#include <queue>
#include <random>
#include <unordered_map>

#include <boost/heap/fibonacci_heap.hpp>
#include <boost/heap/binomial_heap.hpp>
#include <boost/heap/d_ary_heap.hpp>

#include "utils/AllocatorTracker.h"
#include "utils/Benchmark.h"

#include "PriorityQueueX.h"
#include "PriorityQueueU.h"

#define MAXV 1000000

#define TESTN 40000

std::default_random_engine randEngine;
std::uniform_int_distribution< int > intRand( 0, MAXV - 1 );
std::uniform_real_distribution< float > floatRand( 0.f, 100.f );
std::uniform_real_distribution< float > smallFloatRand( 0.f, 10.f );

int32_t compact( int32_t x, int32_t y ) {
    return ( x << 16 ) | ( y & 0xFFFF );
}

int64_t compact( int64_t x, int64_t y, int64_t x2, int64_t y2 ) {
    return ( (x & 0xFFFF) << 48 ) | ( ( y & 0xFFFF ) << 32) | ( ( x2 & 0xFFFF ) << 16) | ( y2 & 0xFFFF );
}

struct Vertex {
    int x;
    int y;
    int data;
    float dist;

    Vertex() {
        x = intRand( randEngine );
        y = intRand( randEngine );
        dist = floatRand( randEngine );
    }
};

Vertex vertexes[MAXV];

// dup算法，需要额外存储dist，记录入队时候的值
// 如果出队时和节点自身的值不同，则说明是过期数据
// 缺点：无法获取队列size，因为里面有过期值，内存稍大
// 优点：实现简单，用std::priority_queue即可

struct DupKey {
    Vertex *vertex;
    float dist;

    DupKey() {}

    DupKey( Vertex *v, float d ) : vertex( v ), dist( d ) {}
};

struct DupKeyCmp {
    bool operator()( const DupKey &a, const DupKey &b ) const { return a.dist > b.dist; }
};

typedef AllocatorTracker< DupKey > DupTracker;

typedef std::priority_queue< DupKey, std::vector< DupKey, DupTracker >, DupKeyCmp > DupHeap;

void testDup() {

    DupTracker tracker;

    DupHeap distQ( tracker );

    for( int i = 0; i < 8 * TESTN; ++i ) distQ.emplace( vertexes + intRand( randEngine ), floatRand( randEngine ) );

    printf( " =============== DupHeap Begin =============\n" );

    printf( "HeapSize: %zu\n", distQ.size() );
    printf( "MemorySize: %lld %lld\n", tracker.get_cur_size(), tracker.get_peak_size() );

    printf( " =============== DupHeap End =============\n" );

//    while( distQ.size() > 0 ) {
//        auto point = distQ.top();
//        distQ.pop();
//        auto dist = point.vertex->dist;
//        if( dist != point.dist ) continue;
//        // std::__heap
//        // TODO 处理相邻的数据点
//        for( auto x = point.x - 1; x <= point.x + 1; x++ ) {
//            for( auto y = point.y - 1; y <= point.y + 1; y++ ) {
//                if( ( x != point.x || y != point.y ) && point.dist < BUBBLE_SIZE && rect.IsInRect( x, y ) ) {
//                    auto &cell = spLocalBubbledMap->ToUnsafe().At( x, y );
//                    if( IsNullRef( cell ) ) continue;
//                    float newDist = dist + GetDistance( point.x, point.y, x, y );
//                    if( newDist < cell ) {
//                        cell = newDist;
//                        distQ.push( DupKey( newDist, x, y ) );
//                    }
//                }
//            }
//        }
//    }
}

// 使用fibonacci堆，它会给每个插入的点返回一个handle，用于内部定位
// 那么外部就要记录节点和handle的映射关系，例如使用unordered_map
// 缺点：内存较大，运行时间的常数项较大
// 优点：使用较简单

struct FibKey {
    int32_t point;
    Vertex *vertex;
    int64_t k1;
    int64_t k2;

    FibKey() : vertex( nullptr ) {}
    FibKey( Vertex *v ) : vertex( v ) {}
};

struct FibKeyCmp {
    bool operator()( const FibKey &a, const FibKey &b ) const { return a.vertex->dist > b.vertex->dist; }
};

typedef AllocatorTracker< FibKey > FibTracker;

typedef boost::heap::fibonacci_heap< FibKey, FibTracker, boost::heap::compare< FibKeyCmp > > FibHeap;

void testFibonacci() {

    FibHeap distQ;
    std::unordered_map< int, FibHeap::handle_type, std::hash< int >, std::equal_to< int >,
            AllocatorTracker< std::pair< const int, FibHeap::handle_type > > > handleMap;

    for( int i = 0; i < TESTN; ++i ) {
        int idx = intRand( randEngine );
        auto& v = vertexes[idx];
        handleMap.emplace( compact( v.x, v.y ), distQ.emplace( vertexes + idx ) );
    }

    for( int i = 0; i < TESTN; ++i ) {
        auto key = distQ.top();
        auto& v = *(key.vertex);
        auto handle = handleMap[compact( v.x, v.y )];
        v.dist -= smallFloatRand( randEngine );
        distQ.update( handle, key );
    }

    while( distQ.size() > 0 ) {
        // distQ.top();
        distQ.pop();
    }

    printf( " =============== FibHeap Begin =============\n" );

    printf( "HeapSize: %zu\n", distQ.size() );
    printf( "HeapMemory: %lld %lld\n", distQ.get_allocator().get_cur_size(), distQ.get_allocator().get_peak_size() );
    printf( "MapMemory: %lld %lld\n", handleMap.get_allocator().get_cur_size(),
            handleMap.get_allocator().get_peak_size() );

    printf( " =============== FibHeap End =============\n" );
}

std::unordered_map< int32_t, int32_t, std::hash< int32_t>, std::equal_to< int32_t >,
        AllocatorTracker< std::pair< const int32_t, int32_t > > > pqxHandleMap;

struct PqxKey {
    int loc;
    // int loc2;
    Vertex *vertex;
    int64_t k1;
    int64_t k2;

    PqxKey() : loc( 0 ), vertex( nullptr ) {}
    PqxKey( Vertex *v ) : vertex( v ) {}
    PqxKey( const PqxKey &other ) : loc( other.loc ), vertex( other.vertex ) {}
    PqxKey( PqxKey &&other ) : loc( other.loc ), vertex( other.vertex ) {
        other.vertex = nullptr;
    }

    PqxKey &operator=( PqxKey &&other ) {

        vertex = other.vertex;
        other.vertex = nullptr;
        pqxHandleMap[compact(vertex->x, vertex->y)] = this->loc;
        // printf( "write %p = %d\n", vertex, this->loc );
        return *this;
    }
};

struct PqxKeyLoc {
    int& operator()( const PqxKey &a ) const { return pqxHandleMap[compact( a.vertex->x, a.vertex->y )]; }
};

struct PqxKeyCmp {
    bool operator()( const PqxKey &a, const PqxKey &b ) const { return a.vertex->dist > b.vertex->dist; }
};

typedef AllocatorTracker< PqxKey > PqxTracker;

typedef robot::PriorityQueueX< PqxKey, PqxKeyLoc, std::deque< PqxKey, PqxTracker >, PqxKeyCmp > PqxHeap;

void testPriorityQueueX() {

    PqxTracker tracker;

    PqxHeap distQ( tracker );

    std::unordered_map< int, int, std::hash< int >, std::equal_to< int >,
            AllocatorTracker< std::pair< const int, int > > > handleMap;

    int last = -1;

    for( int i = 0; i < 10; ++i ) {
        last = intRand( randEngine );
        distQ.emplace( vertexes + last );
    }

//    printf( "erase %f\n", vertexes[last].dist );
//    distQ.erase( PqxKey( vertexes + last ) );

    float delta = 100.f;
    printf( "update %f -> %f\n", vertexes[last].dist, vertexes[last].dist + delta );
    vertexes[last].dist += delta;
    distQ.update( vertexes + last );

    while( distQ.size() > 0 ) {
        printf( "read %p = %f\n", distQ.top().vertex, distQ.top().vertex->dist );
        distQ.pop();
    }

    for( int i = 0; i < TESTN; ++i ) {
        distQ.emplace( vertexes + intRand( randEngine ) );
    }

    for( int i = 0; i < TESTN; ++i ) {
        auto key = distQ.top();
        auto& v = *(key.vertex);
        v.dist -= smallFloatRand( randEngine );
        distQ.update( &v );
    }

    while( distQ.size() > 0 ) {
//        distQ.top();
        distQ.pop();
    }

    printf( " =============== PqxHeap Begin =============\n" );

    printf( "HeapSize: %zu\n", distQ.size() );
    printf( "HeapMemory: %lld %lld\n", tracker.get_cur_size(), tracker.get_peak_size() );
    printf( "MapMemory: %lld %lld\n", pqxHandleMap.get_allocator().get_cur_size(), pqxHandleMap.get_allocator().get_peak_size() );

    printf( " =============== PqxHeap End =============\n" );
}

typedef std::unordered_map< int32_t, int32_t, std::hash< int32_t >, std::equal_to< int32_t >,
        AllocatorTracker< std::pair< const int32_t, int32_t > > > PquMap;

struct PquKey : public robot::PquKeyBase {
    int32_t point;
    Vertex *vertex;
    int64_t k1;
    int64_t k2;

    PquKey() : vertex( nullptr ) {}
    PquKey( Vertex *v ) : point( compact(v->x, v->y) ), vertex( v ) {}
    PquKey( const PquKey &other ) : point( other.point ), vertex( other.vertex ) {}
    PquKey( PquKey &&other ) : point( std::move( other.point) ), vertex( std::move( other.vertex ) ) {
        other.vertex = nullptr;
    }

//    int32_t getLoc() {
//        if( !locator || !vertex ) return -1;
//        return (*(PquMap*)locator)[compact(vertex->x, vertex->y)];
//    }

    int32_t getLoc() {
        if( !locator || !vertex ) return -1;
        auto &locMap = *(PquMap*)locator;
        auto it = locMap.find(point);

        return it == locMap.end() ? -1 : it->second;
    }

    void setLoc( int32_t loc ) {
        this->loc = loc;
        if( locator ) (*(PquMap*)locator)[compact(vertex->x, vertex->y)] = this->loc;
    }

    void eraseLoc() {
        if( locator ) ((PquMap*)locator)->erase( compact(vertex->x, vertex->y) );
        this->loc = -1;
        this->locator = nullptr;
    }

    void setLocator( void* locator ) {
        this->locator = locator;
    }

    PquKey &operator=( PquKey &&other ) {
        vertex = other.vertex;
        other.vertex = nullptr;
        if( locator ) (*(PquMap*)locator)[compact(vertex->x, vertex->y)] = this->loc;
        return *this;
    }
};

struct PquKeyCmp {
    bool operator()( const PquKey &a, const PquKey &b ) const { return a.vertex->dist > b.vertex->dist; }
};

typedef AllocatorTracker< PquKey > PquTracker;

typedef robot::PriorityQueueU< PquKey, std::deque< PquKey, PquTracker >, PquKeyCmp > PquHeap;

void testPriorityQueueU() {

    PquTracker tracker;

    PquMap pquMap;

    PquHeap distQ( &pquMap, tracker );

    std::vector< int > v;
    int last = -1;

    for( int i = 0; i < 10; ++i ) {
        last = intRand( randEngine );
        distQ.emplace( vertexes + last );
        v.emplace_back( last );
    }

//    printf( "erase %f\n", vertexes[last].dist );
//    distQ.erase( PquKey( vertexes + last ) );

    float delta = 100.f;
    printf( "update %f -> %f\n", vertexes[last].dist, vertexes[last].dist + delta );
    for( auto it : pquMap ) printf( "%d %d\n", it.first, it.second );
    vertexes[last].dist += delta;
    distQ.update( vertexes + last );

    printf( "erase %f\n", vertexes[0].dist );
    distQ.erase( vertexes + 0 );

    while( distQ.size() > 0 ) {
        printf( "read %p = %f\n", distQ.top().vertex, distQ.top().vertex->dist );
        distQ.pop();
    }

    for( int i = 0; i < TESTN; ++i ) {
        distQ.emplace( vertexes + intRand( randEngine ) );
    }

    for( int i = 0; i < TESTN; ++i ) {
        auto key = distQ.top();
        auto& v = *(key.vertex);
        v.dist -= smallFloatRand( randEngine );
        distQ.update( &v );
    }

    while( distQ.size() > 0 ) {
        distQ.pop();
    }

    printf( " =============== PquHeap Begin =============\n" );

    printf( "HeapSize: %zu\n", distQ.size() );
    printf( "HeapMemory: %lld %lld\n", tracker.get_cur_size(), tracker.get_peak_size() );
    printf( "MapMemory: %lld %lld\n", pquMap.get_allocator().get_cur_size(), pquMap.get_allocator().get_peak_size() );
    printf( "MapSize: %zu\n", pquMap.size() );
    // printf( "MapMemory: %lld %lld\n", pqxHandleMap.get_allocator().get_cur_size(), pqxHandleMap.get_allocator().get_peak_size() );

    printf( " =============== PquHeap End =============\n" );
}

void init() {

}

struct Say {
    ~Say() {
        printf( "Say Des\n" );
    }
};

int main() {

    init();

    Benchmark bm;

    testDup();
    auto fibTime = bm.run_once( [](){ testFibonacci(); } );
    auto pqxTime = bm.run_once( [](){ testPriorityQueueX(); } );
    auto pquTime = bm.run_once( [](){ testPriorityQueueU(); } );

    printf( "FibTime: %lld\n", fibTime );
    printf( "PqxTime: %lld\n", pqxTime );
    printf( "PquTime: %lld\n", pquTime );

    return 0;
}


/*
 * File:   snapshot.cc
 * Author: ts337
 *
 * Created on 17 June 2011, 12:43
 */

#include "coela_core/src/image_utils.h"
#include "coela_core/src/pixel_array_routines.h"
#include "coela_core/src/ds9_region.h"

#include "coela_luckypipe/src/ccd_dataset_info.h"
#include "coela_luckypipe/src/camera_config_info.h"

#include "coela_luckypipe/src/gain_utils.h"
#include "coela_luckypipe/src/single_ccd_filters.h"
#include "coela_luckypipe/src/image_cleanup.h"
#include "coela_luckypipe/src/frame_info.h"
#include "coela_analysis/src/psf_generation.h"

#include "getoptpp/getopt_pp.h"

#include <tbb/tick_count.h>
#include <tbb/task_scheduler_init.h>

#include <boost/filesystem.hpp>
using namespace std;

struct SnapshotOptions {
    int n_threads;
    int n_frames;
    string camconf_path;
    string dataset_info_path;
};

SnapshotOptions default_options()
{
    SnapshotOptions s;
    s.n_threads = 6;
    s.n_frames = 100;
    return s;
}

void print_usage_and_exit()
{
    SnapshotOptions s = default_options();
    cout<<"Usage: lucky_snapshot CcdDatasetInfo.txt \n\n"
        <<"[-n --nthreads (number)]      --- Use n processor threads (default: "
        << s.n_threads << ")"<<endl
        <<"[-m --nframes (number)]      ---Create snapshot from m frames; default:"
        << s.n_frames<< ")"<<endl;
    exit(0);
}

SnapshotOptions load_options_from_command_line(int argc, char ** argv)
{
    if (argc<2) { print_usage_and_exit(); }
    SnapshotOptions opts = default_options();
    opts.dataset_info_path =argv[1];

    using namespace GetOpt;
    GetOpt_pp input(argc, argv);
    input >> Option('n', "nthreads", opts.n_threads, opts.n_threads);
    input >> Option('m', "nframes", opts.n_frames, opts.n_frames);

    return opts;
}

using namespace std;
using namespace coela;
using namespace single_CCD_filters;
using tbb::tick_count;

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

    SnapshotOptions opts = load_options_from_command_line(argc, argv);
    CcdDatasetInfo dataset_inf(opts.dataset_info_path);
    CameraConfigInfo camconf(dataset_inf.default_camera_config_file);

    vector<FrameInfo> frame_vec(
        FrameInfo::get_frames_vec(dataset_inf.CCD_inputdir,
                                  dataset_inf.in_filestem,
                                  dataset_inf.extension,
                                  dataset_inf.ccd_id,
                                  dataset_inf.dataset_output_base_dir
                                 ));

    CcdCalibrationInfo ccd_calibration =
        camconf.get_calibration_info_for_CCD_id(dataset_inf.ccd_id);

    PixelRange crop_box = ccd_calibration.cropped_PixelRange;

    PixelArray2d<float> bias_frm(crop_box.x_dim(), crop_box.y_dim(), 0.0);

    SequentialFileBufferFilter<> input_filter(frame_vec, opts.n_frames);
    DecompressFilter decompressor;
    FrameCountDisplayFilter count_filter(true);
    FrameCropFilter crop_filter(crop_box); //Use these two if we have a good background region

    ColHistogramGather col_hist_filter(crop_box,
                                         50, crop_box.y_dim());
    col_hist_filter.set_hist_min_value(-300);

    FrameSummation sum_filter(bias_frm.range(),
                               ccd_calibration.crop_region);

    SerialDecommissionFilter end_filter;

    cout<<"Initializing scheduler with " << opts.n_threads <<" threads."<<endl;

    tbb::task_scheduler_init scheduler(opts.n_threads);
    tbb::pipeline pipeline;

    pipeline.add_filter(input_filter);
    pipeline.add_filter(decompressor);
    pipeline.add_filter(count_filter);
    pipeline.add_filter(crop_filter);
    pipeline.add_filter(col_hist_filter);
    pipeline.add_filter(sum_filter);
    pipeline.add_filter(end_filter);

    cout<<"Gathering histograms for column bias frame from first "<< opts.n_frames
        <<" frames..."<<endl;

    tick_count t0;
    t0 = tick_count::now();

    pipeline.run(coela::single_CCD_filters::lucky_n_tokens);

    tick_count t1 = tick_count::now();
    double time =(t1-t0).seconds();

    const vector<HistogramContainer14bit>& col_hists = col_hist_filter.get_hists();
    cout<<"\n";
    cout<<"Collected histograms of all columns in " <<opts.n_frames
        << " in "<<time<<" seconds or "<< time/ opts.n_frames
        <<" secs per frame"<<endl;

    for (size_t x=0; x!=col_hists.size(); ++x) {
        cout<<"\r Fitting col " <<x+1 <<"\t\t"<<flush;
//        col_hists[x].write_to_file( col_hists_folder+"/col"+string_utils::itoa(x)+".txt");
        double dark_estimate =
            image_cleanup::determine_histogram_bias_pedestal_via_thresholded_centroid(col_hists[x],
                    0.8);
//        double dark_estimate = gain_utils::fit_readout_gaussian(col_hists[x].convert_to_value_count_map(), false).front();
        pixel_array_routines::set_col_to_value(bias_frm, x+1, (float)dark_estimate);
    }
    cout<<"\n";

    CcdImage<double> raw_sum = sum_filter.Avg();
    string output_dir = dataset_inf.dataset_output_base_dir+"snapshot/";
    boost::filesystem::create_directories(output_dir);

    string filestem = dataset_inf.out_filestem +
                      "_CCD"+string_utils::itoa(dataset_inf.ccd_id) +
                      "_"+string_utils::itoa(opts.n_frames)+"_frm_snapshot";

//    raw_sum.write_to_file(output_dir+ filestem + "raw.fits");
    CcdImage<double> coldb = raw_sum;
    coldb.pix -= PixelArray2d<double>(bias_frm);
    coldb.write_to_file(output_dir+ filestem + ".fits");

    return 0;
}


#ifndef MP2_HPP_INCLUDED
#define MP2_HPP_INCLUDED 1

#include <stdlib.h>
#include <iostream>
#include <vector>
#include <armadillo>
#include "Molecule.hpp"
#include "Integrals.hpp"
#include "RHF.hpp"

namespace willow { namespace qcmol {

class MP2 : public RHF {
public:

  MP2 (const std::vector<libint2::Atom>& atoms,
       const libint2::BasisSet& bs,
       const Integrals& ints,
       const int qm_chg = 0,
       const bool l_print = true,
       const std::vector<QAtom>& atoms_Q = std::vector<QAtom> ());

  double mp2_energy () { return emp2;};
  double rhf_energy () { return erhf;};

protected:
  int nocc;
  int nvir;
  int ncore;
  int nmo;
  int nbf;

  int iocc1;
  int iocc2;
  int ivir1;
  int ivir2;

  int naocc;
  int navir;

  double fss, fos;

  // Eigen Values and Vectors
  arma::vec eval;
  arma::mat Cmat;

  void compute_default (const std::vector<libint2::Atom>& atoms,
			const libint2::BasisSet& bs,
			const Integrals& ints,
			const int qm_chg = 0,
			const bool l_print = true,
			const std::vector<QAtom>& atoms_Q = std::vector<QAtom> ());

  void compute_direct  (const std::vector<libint2::Atom>& atoms,
			const libint2::BasisSet& bs,
			const Integrals& ints,
			const int qm_chg = 0,
			const bool l_print = true,
			const std::vector<QAtom>& atoms_Q = std::vector<QAtom> ());

  arma::mat   mo_half_ints (const double* ao_tei);

  arma::mat   mo_iajb_ints (const arma::mat& mo_hf_int);

  arma::mat   mo_half_ints_direct (const int im,
				   const libint2::BasisSet& bs,
				   const arma::mat& Schwartz);

  arma::mat   mo_iajb_ints_direct (const arma::mat& mo_hf_int);
  double emp2;
  double erhf;

};

}  } // namespace willow::qcmol

#endif


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

#include <Eigen/Dense>
#include <iostream>
#include <sstream>
#include <vector>
#include "Lookup/lookup.hpp"
#include "Utils/all_utils.hpp"
#include "layer.hpp"

#ifndef NETKET_FFNN_HPP
#define NETKET_FFNN_HPP

namespace netket {

template <typename T>
class FFNN : public AbstractMachine<T> {
  using VectorType = typename AbstractMachine<T>::VectorType;
  using MatrixType = typename AbstractMachine<T>::MatrixType;
  using Ptype = std::unique_ptr<AbstractLayer<T>>;

  std::vector<Ptype> layers_;  // Pointers to hidden layers

  std::vector<int> layersizes_;
  int depth_;
  int nlayer_;
  int npar_;
  int nv_;
  std::vector<VectorType> din_;

  std::vector<std::vector<int>> changed_nodes_;
  std::vector<VectorType> new_output_;
  typename AbstractMachine<T>::LookupType ltnew_;

  const Hilbert &hilbert_;

  const Graph &graph_;

 public:
  using StateType = typename AbstractMachine<T>::StateType;
  using LookupType = typename AbstractMachine<T>::LookupType;

  // constructor
  explicit FFNN(const Graph &graph, const Hilbert &hilbert, const json &pars)
      : nv_(hilbert.Size()), hilbert_(hilbert), graph_(graph) {
    Init(pars);
  }

  void Init(const json &pars) {
    json layers_par;
    if (FieldExists(pars["Machine"], "Layers")) {
      layers_par = pars["Machine"]["Layers"];
      nlayer_ = layers_par.size();
    } else {
      throw InvalidInputError("Field (Layers) not defined for Machine (FFNN)");
    }

    std::string buffer = "";
    // Initialise Layers
    layersizes_.push_back(nv_);
    for (int i = 0; i < nlayer_; ++i) {
      InfoMessage(buffer) << "# Layer " << i + 1 << " : ";

      layers_.push_back(Ptype(new Layer<T>(graph_, layers_par[i])));

      layersizes_.push_back(layers_.back()->Noutput());

      if (layersizes_[i] != layers_.back()->Ninput()) {
        throw InvalidInputError("input/output layer sizes do not match");
      }
    }

    // Check that final layer has only 1 unit otherwise add unit identity layer
    if (layersizes_.back() != 1) {
      nlayer_ += 1;

      InfoMessage(buffer) << "# Layer " << nlayer_ << " : ";

      layers_.push_back(
          Ptype(new FullyConnected<Identity, T>(layersizes_.back(), 1)));

      layersizes_.push_back(1);
    }
    depth_ = layersizes_.size();

    din_.resize(depth_);
    din_.back().resize(1);
    din_.back()(0) = 1.0;

    npar_ = 0;
    for (int i = 0; i < nlayer_; ++i) {
      npar_ += layers_[i]->Npar();
    }

    for (int i = 0; i < nlayer_; ++i) {
      ltnew_.AddVector(layersizes_[i + 1]);
      ltnew_.AddVV(1);
    }

    changed_nodes_.resize(nlayer_);
    new_output_.resize(nlayer_);

    InfoMessage(buffer) << "# FFNN Initizialized with " << nlayer_
                        << " Layers: ";
    for (int i = 0; i < depth_ - 1; ++i) {
      InfoMessage(buffer) << layersizes_[i] << " -> ";
    }
    InfoMessage(buffer) << layersizes_[depth_ - 1];
    InfoMessage(buffer) << std::endl;
    InfoMessage(buffer) << "# Total Number of Parameters = " << npar_
                        << std::endl;
  }

  void from_json(const json &pars) override {
    json layers_par;
    if (FieldExists(pars["Machine"], "Layers")) {
      layers_par = pars["Machine"]["Layers"];
      nlayer_ = layers_par.size();
    } else {
      throw InvalidInputError(
          "Field (Layers) not defined for Machine (FFNN) in initfile");
    }

    for (int i = 0; i < nlayer_; ++i) {
      layers_[i]->from_json(layers_par[i]);
    }
  }

  int Nvisible() const override { return layersizes_[0]; }

  int Npar() const override { return npar_; }

  VectorType GetParameters() override {
    VectorType pars(npar_);
    int start_idx = 0;
    for (auto const &layer : layers_) {
      layer->GetParameters(pars, start_idx);
      start_idx += layer->Npar();
    }
    return pars;
  }

  void SetParameters(const VectorType &pars) override {
    int start_idx = 0;
    for (auto const &layer : layers_) {
      layer->SetParameters(pars, start_idx);
      start_idx += layer->Npar();
    }
  }

  void InitRandomPars(int seed, double sigma) override {
    for (auto const &layer : layers_) {
      layer->InitRandomPars(seed, sigma);
    }
  }

  void InitLookup(const Eigen::VectorXd &v, LookupType &lt) override {
    if (lt.VVSize() == 0) {
      lt.AddVV(1);                   // contains the output of layer 0
      lt.AddVector(layersizes_[1]);  // contains the lookup of layer 0
      layers_[0]->InitLookup(v, lt.VV(0), lt.V(0));
      for (int i = 1; i < nlayer_; ++i) {
        lt.AddVV(1);                       // contains the output of layer i
        lt.AddVector(layersizes_[i + 1]);  // contains the lookup of layer i
        layers_[i]->InitLookup(lt.V(i - 1), lt.VV(i), lt.V(i));
      }
    } else {
      assert((int(lt.VectorSize()) == nlayer_) &&
             (int(lt.VVSize()) == nlayer_));
      layers_[0]->InitLookup(v, lt.VV(0), lt.V(0));
      for (int i = 1; i < nlayer_; ++i) {
        layers_[i]->InitLookup(lt.V(i - 1), lt.VV(i), lt.V(i));
      }
    }
  }

  void UpdateLookup(const Eigen::VectorXd &v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    layers_[0]->UpdateLookup(v, tochange, newconf, lt.VV(0), lt.V(0),
                             changed_nodes_[0], new_output_[0]);
    for (int i = 1; i < nlayer_; ++i) {
      layers_[i]->UpdateLookup(lt.V(i - 1), changed_nodes_[i - 1],
                               new_output_[i - 1], lt.VV(i), lt.V(i),
                               changed_nodes_[i], new_output_[i]);
      UpdateOutput(lt.V(i - 1), changed_nodes_[i - 1], new_output_[i - 1]);
    }
    UpdateOutput(lt.V(nlayer_ - 1), changed_nodes_[nlayer_ - 1],
                 new_output_[nlayer_ - 1]);
  }

  void UpdateOutput(VectorType &v, const std::vector<int> &tochange,
                    VectorType &newconf) {
    int num_of_changes = tochange.size();
    if (num_of_changes == v.size()) {
      assert(int(newconf.size()) == num_of_changes);
      v.swap(newconf);  // this is done for efficiency
    } else {
      for (int s = 0; s < num_of_changes; s++) {
        const int sf = tochange[s];
        v(sf) = newconf(s);
      }
    }
  }

  T LogVal(const Eigen::VectorXd &v) override {
    LookupType lt;
    InitLookup(v, lt);
    assert(nlayer_ > 0);
    return (lt.V(nlayer_ - 1))(0);
  }

  T LogVal(const Eigen::VectorXd & /*v*/, const LookupType &lt) override {
    assert(nlayer_ > 0);
    return (lt.V(nlayer_ - 1))(0);
  }

  VectorType DerLog(const Eigen::VectorXd &v) override {
    VectorType der(npar_);
    LookupType ltnew;
    InitLookup(v, ltnew);
    DerLog(v, der, ltnew);
    return der;
  }

  void DerLog(const Eigen::VectorXd &v, VectorType &der, const LookupType &lt) {
    int start_idx = npar_;
    // Backpropagation
    if (nlayer_ > 1) {
      start_idx -= layers_[nlayer_ - 1]->Npar();
      // Last Layer
      layers_[nlayer_ - 1]->Backprop(lt.V(nlayer_ - 2), lt.V(nlayer_ - 1),
                                     lt.VV(nlayer_ - 1), din_.back(),
                                     din_[nlayer_ - 1], der, start_idx);
      // Middle Layers
      for (int i = nlayer_ - 2; i > 0; --i) {
        start_idx -= layers_[i]->Npar();
        layers_[i]->Backprop(lt.V(i - 1), lt.V(i), lt.VV(i), din_[i + 1],
                             din_[i], der, start_idx);
      }
      // First Layer
      layers_[0]->Backprop(v, lt.V(0), lt.VV(0), din_[1], din_[0], der, 0);
    } else {
      // Only 1 layer
      layers_[0]->Backprop(v, lt.V(0), lt.VV(0), din_.back(), din_[0], der, 0);
    }
  }

  VectorType LogValDiff(
      const Eigen::VectorXd &v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    const int nconn = tochange.size();
    VectorType logvaldiffs = VectorType::Zero(nconn);
    LookupType lt;
    InitLookup(v, lt);
    T current_val = LogVal(v, lt);

    for (int k = 0; k < nconn; ++k) {
      logvaldiffs(k) = 0;
      if (tochange[k].size() != 0) {
        LookupType ltnew = lt;
        UpdateLookup(v, tochange[k], newconf[k], ltnew);

        logvaldiffs(k) += LogVal(v, ltnew) - current_val;
      }
    }
    return logvaldiffs;
  }

  T LogValDiff(const Eigen::VectorXd &v, const std::vector<int> &tochange,
               const std::vector<double> &newconf,
               const LookupType &lt) override {
    if (tochange.size() != 0) {
      LookupType ltnew = lt;
      UpdateLookup(v, tochange, newconf, ltnew);

      return LogVal(v, ltnew) - LogVal(v, lt);
    } else {
      return 0.0;
    }
  }

  void to_json(json &j) const override {
    j["Machine"]["Name"] = "FFNN";
    j["Machine"]["Layers"] = {};
    for (int i = 0; i < nlayer_; ++i) {
      layers_[i]->to_json(j);
    }
  }
};  // namespace netket

}  // namespace netket

#endif


// next_permutation example
#include <iostream>     // std::cout
#include <algorithm>    // std::next_permutation, std::sort
#include <vector>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include <limits>
#include <fstream>
#include <set>
#include <utility>
#include <regex>
using namespace std;

int main (int argc , char *argv[]) {
  /*string mypaths[] = {"1\n30","3\n33","8\n31","11\n34","9\n28"};
  //vector <pair < int,int > > paths ;
  std::sort (mypaths,mypaths+5);
  fstream myfile;
  fstream myfile_output;
  fstream myfile_trunc;
  //myfile_output.open("astar_output.txt",ios::out);//|ios::trunc);
  myfile.open("waypoints.txt",ios::out);//|ios::trunc);
int c;
  std::cout << "The 5! possible permutations with 5 elements:\n";
  do {
    //myfile.open("waypoints.txt",ios::out);//|ios::trunc);

    myfile << mypaths[0] ;
    myfile << "\n";
    myfile << "\n";
    myfile << mypaths[1] ;
    myfile << "\n";
    myfile << "\n";
    myfile << mypaths[2] ;
    myfile << "\n";
    myfile << "\n";
    myfile << mypaths[3] ;
    myfile << "\n";
    myfile << "\n";
    myfile << mypaths[4] ;
    myfile << "\n";
    myfile << "\n";

    myfile.close();
c++;
    //myfile_trunc.open("waypoints.txt",ios::out|ios::trunc);

    //myfile_trunc.close();

  }while ( std::next_permutation(mypaths,mypaths+5) );
  cout << c<<endl;;
    //myfile.close();
    //myfile_trunc.close(); .erase(std::remove(input.begin(), input.end(), '\n'), input.end());
    string prova ="1\n2";
    regex new_line ("\n+");

  auto result = std::regex_replace(prova, new_line, "");
  //prova.erase(remove(prova.begin(),prova.end(),"\n"),prova.end());
  cout << "   "<< result <<endl;

  vector < vector < int > > prova2;*/
  vector < int > test;
  //vector < int > test2;
  test.push_back(51);
  test.push_back(53);
  test.push_back(57);
  test.push_back(60);
  test.push_back(64);
  test.push_back(70);

  float media = (51+53+57+60+64+70)/6;
  float sigma = 0;
  float partial_sigma = 0;
  /*test2.push_back(3);
  prova2.push_back(test);
  prova2.push_back(test2);

  prova2.clear();

  test.clear();
  if(prova2.empty()) cout << "ocnfev" << endl;*/
  //float x = atof(argv[1]);
  for(int i=0;i<test.size();i++){
    cout << i <<endl;
    partial_sigma = test[i]-media;
    sigma = sigma + pow(partial_sigma,2);
  }
  sigma = sqrt(sigma/test.size());
  cout << sigma << endl;
}


#ifndef MLT_MODELS_TRANSFORMERS_TRANSFORMER_HPP
#define MLT_MODELS_TRANSFORMERS_TRANSFORMER_HPP

#include <type_traits>

#include <Eigen/Core>

#include "../base.hpp"

namespace mlt {
namespace models {
namespace transformers {
	template <class ConcreteType>
	class Transformer : public Model<ConcreteType> {
	public:
		using Result = MatrixXd;
		template <class FitInput, class FitTarget>
		Self& fit(FitInput input, FitTarget, bool cold_start = true) {
			return _self().fit(forward<FitInput>(input), cold_start);
		}

		template <class FitInput, class FitTarget>
		Result fit_transform(FitInput&& input, FitTarget, bool cold_start = true) {
			return _self().fit(forward<FitInput>(input), cold_start).transform(input);
		}

	protected:
		Transformer() = default;
		Transformer(const Transformer&) = default;
		Transformer(Transformer&&) = default;
		Transformer& operator=(const Transformer&) = default;
		~Transformer() = default;
	};
}
}
}
#endif


#include "naivebayes.h"
#include "utilities.h"
#include <boost/foreach.hpp>

using namespace std;

NaiveBayes::NaiveBayes(const HyperParameters &hp) : m_hp( hp )
{

}

NaiveBayes::~NaiveBayes()
{
}

void NaiveBayes::initialize(const int numSamples)
{
    m_confidences.resize(numSamples, m_hp.numClasses);
    m_predictions.resize(numSamples);

    m_inBagSamples.clear();
    m_outOfBagSamples.clear();
}

void NaiveBayes::subSample(const int numSamples)
{
    if (m_hp.useSubSamplingWithReplacement)
    {
        m_inBagSamples = subSampleWithReplacement(numSamples);
        m_outOfBagSamples = setDiff(m_inBagSamples, numSamples);
    }
    else
    {
        subSampleWithoutReplacement(numSamples, static_cast<int>(floor(numSamples * m_hp.bagRatio)),
                                    m_inBagSamples, m_outOfBagSamples);
    }
}

void NaiveBayes::finalize(const matrix<float>& data,
                    matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                    std::vector<int>& forestOutOfBagVoteNum)
{

    // Fill the confidence of the forest
    for (int nSamp = 0; nSamp < m_hp.numLabeled; nSamp++) {
        if (m_hp.useSoftVoting) {
            for (int nClass = 0; nClass < m_hp.numClasses; nClass++) {
                forestConfidences(nSamp,nClass) += m_confidences(nSamp,nClass);
            }
        }
        else {
            forestConfidences(nSamp, m_predictions[nSamp])++;
        }
    }

     // Fill the out of bag confidences and vote count
    BOOST_FOREACH(int n, m_outOfBagSamples) {
        forestOutOfBagVoteNum[n]++;

        if (m_hp.useSoftVoting) {
            for (int nClass = 0; nClass < m_hp.numClasses; nClass++) {
                forestOutOfBagConfidences(n,nClass) += m_confidences(n,nClass);
            }
        }
        else {
            forestOutOfBagConfidences(n, m_predictions[n])++;
        }
    }

    // Clean up
    //clean();
}

void NaiveBayes::clean() {
  m_confidences.clear();
}

double NaiveBayes::computeError(const std::vector<int>& labels)
{
    int error = 0;
    for(int sample = 0; sample < (int)labels.size(); sample++) {
        error += (m_predictions[sample] != labels[sample]) ? 1 : 0;
    }
    return static_cast<double>(error)/static_cast<double>(labels.size());
}