/*
// Copyright (c) 2000-2009, Texas Engineering Experiment Station (TEES), a
// component of the Texas A&M University System.

// All rights reserved.

// The information and source code contained herein is the exclusive
// property of TEES and may not be disclosed, examined or reproduced
// in whole or in part without explicit written authorization from TEES.
*/

#include <limits> //currently needed by generate_graph
#include <stapl/containers/sequential/graph/graph.h>
#include <stapl/containers/sequential/graph/profile/bgl_profilers.h>
#include <stapl/containers/sequential/graph/profile/profile_utils.h>

using namespace stapl::sequential;

#include <stapl/containers/sequential/graph/algorithms/dijkstra.h>
#include <stapl/containers/sequential/graph/profile/stapl_graph_profilers.h>
#include <boost/graph/adjacency_list.hpp>
#include <stapl/containers/sequential/graph/algorithms/graph_generators.h>

using namespace boost;
using namespace stapl;
using stapl::UNDIRECTED;
using stapl::DIRECTED;
using stapl::NONMULTIEDGES;
using stapl::MULTIEDGES;
using std::cout;
using std::cerr;
using std::endl;

void print_help_message() {
  cout<<"The program must be executed with at least 1 integer argument.\n";
  cout<<"./<executable> T [optional args]\n";
  cout<<"T determines the algorithms tested, graph directedness, and ";
  cout<<"algorithm data location.\n";
  cout<<"Note that, in general, you want to use the optional args.\n";

  cout<<"Test types:\nTests 1-4 are for Directed graphs.\n";
  cout<<"1 --- BFS/DFS with data stored internally.\n";
  cout<<"2 --- BFS/DFS with data stored externally.\n";
  cout<<"3 --- Dijkstra with data stored internally.\n";
  cout<<"4 --- Dijkstra with data stored externally.\n\n";
  cout<<"Tests 5-8 are for Undirected graphs.\n";
  cout<<"5 --- BFS/DFS with data stored internally.\n";
  cout<<"6 --- BFS/DFS with data stored externally.\n";
  cout<<"7 --- Dijkstra with data stored internally.\n";
  cout<<"8 --- Dijkstra with data stored externally.\n";

  profile_config::print_help_message();
  return;
}

//the propertize() methods assume the graphs have size_t properties.
template<typename BGL_GRAPH, typename STAPL_GRAPH>
void dijkstra_profiler_suite(std::string filename, int argc, char** argv) {
  typedef typename BGL_GRAPH::vertex_descriptor   bgl_vd_t;
  typedef typename STAPL_GRAPH::vertex_descriptor stapl_vd_t;
  typedef typename std::vector<bgl_vd_t>          bgl_verts_t;
  typedef typename std::vector<stapl_vd_t>        stapl_verts_t;

  size_t seed = time(NULL);
  cout<<"Propertizing the graphs with seed: "<<seed<<endl;

  BGL_GRAPH* bgptr = new BGL_GRAPH;
  bgl_verts_t bgl_verts = read_from_file_bgl(*bgptr, filename);
  propertize_bgl(*bgptr,seed);

  dijkstra_profiler_bgl<BGL_GRAPH> dbgl("test",bgptr,argc,argv);
  dbgl.collect_profile();
  dbgl.report();

  bgptr->clear();
  delete bgptr;

  STAPL_GRAPH* sgptr = new STAPL_GRAPH;
  stapl_verts_t stapl_verts = read_from_file(*sgptr, filename);
  propertize(*sgptr,seed);

  dijkstra_profiler<STAPL_GRAPH> dstapl("test",sgptr,argc,argv);
  dstapl.collect_profile();
  dstapl.report();

  sgptr->clear();
  delete sgptr;
}

template<typename BGL_GRAPH, typename STAPL_GRAPH>
void dsssp_internal_profiler_suite(std::string filename, int argc, char** argv) {
  typedef typename BGL_GRAPH::vertex_descriptor     bgl_vd_t;
  typedef typename STAPL_GRAPH::vertex_descriptor   stapl_vd_t;
  typedef typename std::vector<bgl_vd_t>        bgl_verts_t;
  typedef typename std::vector<stapl_vd_t>      stapl_verts_t;

  size_t seed = time(NULL);
  cout<<"Propertizing the graphs with seed: "<<seed<<endl;

  BGL_GRAPH* bgptr = new BGL_GRAPH;
  read_from_file_bgl(*bgptr, filename);
  propertize_bgl(*bgptr,seed);

  dsssp_in_profiler_bgl<BGL_GRAPH> dbgl("test",bgptr,argc,argv);
  dbgl.collect_profile();
  dbgl.report();

  bgptr->clear();
  delete bgptr;

  STAPL_GRAPH* sgptr = new STAPL_GRAPH;
  read_from_file(*sgptr, filename);
  propertize(*sgptr,seed);

  dsssp_in_profiler<STAPL_GRAPH> dstapl("test",sgptr,argc,argv);
  dstapl.collect_profile();
  dstapl.report();

  sgptr->clear();
  delete sgptr;
}

template<typename BGL_GRAPH, typename STAPL_GRAPH>
void algo_profiler_suite(std::string filename, int argc, char** argv) {
  BGL_GRAPH* bgptr = new BGL_GRAPH;
  read_from_file_bgl(*bgptr, filename);

  bfs_profiler_bgl<BGL_GRAPH> bfsbgl("test",bgptr,argc,argv);
  bfsbgl.collect_profile();
  bfsbgl.report();

  dfs_profiler_bgl<BGL_GRAPH> dfsbgl("test",bgptr,argc,argv);
  dfsbgl.collect_profile();
  dfsbgl.report();

  bgptr->clear();
  delete bgptr;

  STAPL_GRAPH* sgptr = new STAPL_GRAPH;
  read_from_file(*sgptr, filename);

  bfs_profiler<STAPL_GRAPH> bfsstapl("test",sgptr,argc,argv);
  bfsstapl.collect_profile();
  bfsstapl.report();

  dfs_profiler<STAPL_GRAPH> dfsstapl("test",sgptr,argc,argv);
  dfsstapl.collect_profile();
  dfsstapl.report();

  sgptr->clear();
  delete sgptr;
}

template<typename BGL_GRAPH, typename STAPL_GRAPH>
void algo_internal_profiler_suite(std::string filename, int argc, char** argv) {
  BGL_GRAPH* bgptr = new BGL_GRAPH;
  read_from_file_bgl(*bgptr, filename);

  bfs_internal_profiler_bgl<BGL_GRAPH> bfsbgl("test",bgptr,argc,argv);
  bfsbgl.collect_profile();
  bfsbgl.report();

  dfs_internal_profiler_bgl<BGL_GRAPH> dfsbgl("test",bgptr,argc,argv);
  dfsbgl.collect_profile();
  dfsbgl.report();

  bgptr->clear();
  delete bgptr;

  STAPL_GRAPH* sgptr = new STAPL_GRAPH;
  read_from_file(*sgptr, filename);

  bfs_internal_profiler<STAPL_GRAPH> bfsstapl("test",sgptr,argc,argv);
  bfsstapl.collect_profile();
  bfsstapl.report();

  dfs_internal_profiler<STAPL_GRAPH> dfsstapl("test",sgptr,argc,argv);
  dfsstapl.collect_profile();
  dfsstapl.report();

  sgptr->clear();
  delete sgptr;
}

template<typename BGL_GRAPH, typename STAPL_GRAPH>
void traversal_profiler_suite(std::string filename, int argc, char** argv) {
  BGL_GRAPH* bgptr = new BGL_GRAPH;
  read_from_file_bgl(*bgptr, filename);

  traversal_profiler_bgl<BGL_GRAPH> tpbgl("test",bgptr,argc,argv);
  tpbgl.collect_profile();
  tpbgl.report();

  bgptr->clear();
  delete bgptr;

  STAPL_GRAPH* sgptr = new STAPL_GRAPH;
  read_from_file(*sgptr, filename);

  traversal_profiler<STAPL_GRAPH> tpssgl("test",sgptr,argc,argv);
  tpssgl.collect_profile();
  tpssgl.report();

  sgptr->clear();
  delete sgptr;
}

/**
 * Configure and run experiments on BGL's and SSGL's implementations of
 * BFS, DFS, and Dijkstra's algorithm.
 *
 * A graph is pre-generated and then stored in external memory. This is
 * done so that we can guarantee that the graph build for both BGL and
 * SSGL are reading from the same input, which helps us ensure that the
 * algorithm profilers are always operating on the same input graph.
 *
 * For information on each test type (case), just read the cases
 * indvidually or execute the binary without any arguments.
 *
 * Furthermore, the graphs tend to become very large, so storing a
 * collapsed/blueprint graph in internal memory would be prohibitive.
 * However, if this ever becomes necessary, all that needs to be done
 * is to have the config object store the graph instead of using
 * write_to_file and the profiler suite methods defined above need to
 * take the config object (by reference!) and use it to rebuild the
 * graph instead of reading from external memory.
 */
int main(int argc,char** argv) {
  size_t T=0;     //determines graph attributes and algo. suite
  if(argc >= 2) T=atoi(argv[1]);
  else {
    print_help_message();
    return 0;
  }

  //Build the graph
  profile_config config;
  std::string filename = "graph.txt";
  if((T>0 && T<11) || T==1337) {
    config.configure(argc,argv);
    stapl::sequential::graph<DIRECTED,MULTIEDGES> g;
    build_graph(g,config);
    write_to_file(g,filename);
  }
  else {
    print_help_message();
    return 0;
  }

  cout<<"Benchmark: STAPL Graph vs. BGL"<<endl<<endl;
  //==================================================================
  // Begin test cases
  //==================================================================
  if(T==1) {
    typedef boost::property<boost::vertex_color_t,
                            boost::default_color_type>  bgl_vp_t;
    typedef adjacency_list<vecS, vecS, directedS,
                           bgl_vp_t>                    BGL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES,int>       STAPL_TYPE;

    cout<<"Algo. Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nProperties stored internally.\n\n";
    algo_internal_profiler_suite<BGL_TYPE,STAPL_TYPE>(filename,argc,argv);
  }

  if(T==2) {
    typedef adjacency_list<vecS, vecS, directedS> BGL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES>     STAPL_TYPE;

    cout<<"Algo. Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nProperties stored externally.\n\n";
    algo_profiler_suite<BGL_TYPE,STAPL_TYPE>(filename,argc,argv);
  }

  if(T==3) {
    //The vertex_name_t property is to store the vertex's parent given by
    //Dijkstra's algo. It has nothing to do with any kind of naming.
    typedef property<vertex_color_t, default_color_type>    bgl_vp_t3;
    typedef property<vertex_name_t, size_t, bgl_vp_t3>      bgl_vp_t2;
    typedef property<vertex_distance_t, size_t, bgl_vp_t2>  bgl_vp_t;
    typedef property<edge_weight_t, size_t>                 bgl_ep_t;
    typedef adjacency_list<vecS, vecS, directedS,
                           bgl_vp_t, bgl_ep_t>              BGL_TYPE;

    typedef size_t                                          stapl_ep_t;
    typedef stapl::dijkstra_property<size_t,stapl_ep_t,int> stapl_vp_t;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES,
                                     stapl_vp_t, stapl_ep_t>            STAPL_TYPE;

    cout<<"Dijkstra Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nProperties stored internally.\n\n";
    dsssp_internal_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }

  if(T==4) {
    typedef adjacency_list<vecS, vecS, directedS, boost::no_property,
                           property<edge_weight_t, size_t> > BGL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES,
                                     stapl::properties::no_property,
                                     size_t>  STAPL_TYPE;

    cout<<"Dijkstra Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nProperties stored externally.\n\n";
    dijkstra_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }

  if(T==5) {
    typedef boost::property<boost::vertex_color_t,
                            boost::default_color_type>  bgl_vp_t;
    typedef adjacency_list<vecS, vecS, undirectedS,
                           bgl_vp_t>                    BGL_TYPE;
    typedef stapl::sequential::graph<UNDIRECTED,MULTIEDGES,int>     STAPL_TYPE;

    cout<<"Algo. Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nUndirected Edges\nProperties stored internally.\n\n";
    algo_internal_profiler_suite<BGL_TYPE,STAPL_TYPE>(filename,argc,argv);
  }

  if(T==6) {
    typedef adjacency_list<vecS, vecS, undirectedS> BGL_TYPE;
    typedef stapl::sequential::graph<UNDIRECTED,MULTIEDGES>     STAPL_TYPE;

    cout<<"Algo. Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nUndirected Edges\nProperties stored externally.\n\n";
    algo_profiler_suite<BGL_TYPE,STAPL_TYPE>(filename,argc,argv);
  }

  if(T==7) {
    //The vertex_name_t property is to store the vertex's parent given by
    //Dijkstra's algo. It has nothing to do with any kind of naming.
    typedef property<vertex_color_t, default_color_type>    bgl_vp_t3;
    typedef property<vertex_name_t, size_t, bgl_vp_t3>      bgl_vp_t2;
    typedef property<vertex_distance_t, size_t, bgl_vp_t2>  bgl_vp_t;
    typedef property<edge_weight_t, size_t>                 bgl_ep_t;
    typedef adjacency_list<vecS, vecS, undirectedS,
                           bgl_vp_t, bgl_ep_t>              BGL_TYPE;

    typedef size_t                                          stapl_ep_t;
    typedef stapl::dijkstra_property<size_t,stapl_ep_t,int> stapl_vp_t;
    typedef stapl::sequential::graph<UNDIRECTED,MULTIEDGES,
                                     stapl_vp_t, stapl_ep_t>            STAPL_TYPE;

    cout<<"Dijkstra Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nUndirected Edges\nProperties stored internally.\n\n";
    dsssp_internal_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }

  if(T==8) {
    typedef adjacency_list<vecS, vecS, undirectedS, boost::no_property,
                           property<edge_weight_t, size_t> > BGL_TYPE;
    typedef stapl::sequential::graph<UNDIRECTED,MULTIEDGES,
                                     stapl::properties::no_property,
                                     size_t>  STAPL_TYPE;

    cout<<"Dijkstra Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nUndirected Edges\nProperties stored externally.\n\n";
    dijkstra_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }

  if(T==9) {
    typedef adjacency_list<vecS, vecS, directedS> BGL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES>     STAPL_TYPE;

    cout<<"Traversal Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nNo properties.\n\n";
    traversal_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }

  if(T==10) {
    typedef property<vertex_name_t, int>          vp_t;
    typedef property<edge_weight_t, int>          ep_t;
    typedef adjacency_list<vecS, vecS, directedS, vp_t, ep_t> BGL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES,int,int> STAPL_TYPE;

    cout<<"Traversal Profiler Suite:\nVertex Storage: Vector\n";
    cout<<"Edge Storage: Vector\nDirected Edges\nInteger properties.\n\n";
    traversal_profiler_suite<BGL_TYPE, STAPL_TYPE>(filename,argc,argv);
  }
  //==================================================================
  // Smart vector storage in SSGL
  //==================================================================
  if(T==42) {
    typedef boost::property<boost::vertex_color_t,
                            boost::default_color_type>  bgl_vp_t;
    typedef adjacency_list<vecS, vecS, directedS,
                           bgl_vp_t>                    BGL_TYPE;

    typedef stapl::sequential::adj_graph_traits_vector_storage<
      DIRECTED, MULTIEDGES, int, stapl::properties::no_property> vector_trait;
    typedef stapl::sequential::graph<DIRECTED,
                                     MULTIEDGES,
                                     int,
                                     stapl::properties::no_property,
                                     vector_trait> STAPL_TYPE;
    typedef stapl::sequential::graph<DIRECTED,MULTIEDGES> BUILDER_T;

    cout<<"Profiler Suite:\nEdge Storage: Vector\nVertex Storage: ";
    cout<<"Vector/Smart Vector\nDirected Edges\n\n";
    algo_internal_profiler_suite<BGL_TYPE,STAPL_TYPE>(filename,argc,argv);
  }

  cout<<"That's all folks!"<<endl;

  return 0;
}


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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)
//=======================================================================
#include <iostream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <vector>

#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>

using namespace boost;

// a class to hold the coordinates of the straight line embedding
struct coord_t
{
    std::size_t x;
    std::size_t y;
};

int main(int argc, char** argv)
{
    typedef adjacency_list< vecS, vecS, undirectedS,
        property< vertex_index_t, int > >
        graph;

    // Define the storage type for the planar embedding
    typedef std::vector< std::vector< graph_traits< graph >::edge_descriptor > >
        embedding_storage_t;
    typedef boost::iterator_property_map< embedding_storage_t::iterator,
        property_map< graph, vertex_index_t >::type >
        embedding_t;

    // Create the graph - a maximal planar graph on 7 vertices. The functions
    // planar_canonical_ordering and chrobak_payne_straight_line_drawing both
    // require a maximal planar graph. If you start with a graph that isn't
    // maximal planar (or you're not sure), you can use the functions
    // make_connected, make_biconnected_planar, and make_maximal planar in
    // sequence to add a set of edges to any undirected planar graph to make
    // it maximal planar.

    graph g(7);
    add_edge(0, 1, g);
    add_edge(1, 2, g);
    add_edge(2, 3, g);
    add_edge(3, 0, g);
    add_edge(3, 4, g);
    add_edge(4, 5, g);
    add_edge(5, 6, g);
    add_edge(6, 3, g);
    add_edge(0, 4, g);
    add_edge(1, 3, g);
    add_edge(3, 5, g);
    add_edge(2, 6, g);
    add_edge(1, 4, g);
    add_edge(1, 5, g);
    add_edge(1, 6, g);

    // Create the planar embedding
    embedding_storage_t embedding_storage(num_vertices(g));
    embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

    boyer_myrvold_planarity_test(boyer_myrvold_params::graph = g,
        boyer_myrvold_params::embedding = embedding);

    // Find a canonical ordering
    std::vector< graph_traits< graph >::vertex_descriptor > ordering;
    planar_canonical_ordering(g, embedding, std::back_inserter(ordering));

    // Set up a property map to hold the mapping from vertices to coord_t's
    typedef std::vector< coord_t > straight_line_drawing_storage_t;
    typedef boost::iterator_property_map<
        straight_line_drawing_storage_t::iterator,
        property_map< graph, vertex_index_t >::type >
        straight_line_drawing_t;

    straight_line_drawing_storage_t straight_line_drawing_storage(
        num_vertices(g));
    straight_line_drawing_t straight_line_drawing(
        straight_line_drawing_storage.begin(), get(vertex_index, g));

    // Compute the straight line drawing
    chrobak_payne_straight_line_drawing(
        g, embedding, ordering.begin(), ordering.end(), straight_line_drawing);

    std::cout << "The straight line drawing is: " << std::endl;
    graph_traits< graph >::vertex_iterator vi, vi_end;
    for (boost::tie(vi, vi_end) = vertices(g); vi != vi_end; ++vi)
    {
        coord_t coord(get(straight_line_drawing, *vi));
        std::cout << *vi << " -> (" << coord.x << ", " << coord.y << ")"
                  << std::endl;
    }

    // Verify that the drawing is actually a plane drawing
    if (is_straight_line_drawing(g, straight_line_drawing))
        std::cout << "Is a plane drawing." << std::endl;
    else
        std::cout << "Is not a plane drawing." << std::endl;

    return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 1> test_int_6_1(const Eigen::Matrix<int, 6, 1> & M)
{
	return M;
}
void export_int_6_1()
{
	boost::python::def("test_int_6_1",test_int_6_1);
}


// STL
#include <sstream>

// ROS
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Empty.h>
#include <tf/transform_broadcaster.h>
#include <pcl_ros/transforms.h>

// Eigen
#include <Eigen/Eigen>

// Variables
bool hasExtrinsic;
Eigen::Matrix4f Teig = Eigen::Matrix4f::Identity(); // Scaled to [m]
ros::Publisher pub; // Publisher for aligned point cloud

/*
 * Callback function does the following:
 *  - Reads the unaligned point cloud from depth_registered/points
 *  - Knows the extrinsic calibration transformation matrix Teig
 *  - Applies Teig to the points and publishes them through depth_registered/points_extrinsic
 */
void callback(sensor_msgs::PointCloud2::ConstPtr cloud) {
   if(hasExtrinsic) {
      sensor_msgs::PointCloud2Ptr output(new sensor_msgs::PointCloud2);
      pcl_ros::transformPointCloud(Teig, *cloud, *output);
      pub.publish(output);
   } else {
      pub.publish(cloud);
   }
}

/*
 * Main entry point
 */
int main(int argc, char** argv){
   // Init
   ros::init(argc, argv, "");
   ros::NodeHandle n("~");

   // Namespace name is expected to be equal to the target frame name
   const std::string& ns = n.getNamespace();

   // Get arguments
   std::string source;
   int rate;
   if(!n.getParam(ns+"/source", source))
      source = "/world";
   if(!n.getParam(ns+"/rate", rate))
      rate = 30;

   // Setup publisher of aligned point clouds
   const int queuesize = 10;
   ros::Subscriber sub = n.subscribe<sensor_msgs::PointCloud2>("/"+ns+"/depth_registered/points", queuesize, callback);
   pub = n.advertise<sensor_msgs::PointCloud2>("/"+ns+"/depth_registered/points_extrinsic", queuesize);

   // Load extrinsics if available
   hasExtrinsic = n.hasParam(ns+"/position/data");
   if(hasExtrinsic) {
      XmlRpc::XmlRpcValue xmldata;
      n.getParam(ns+"/position/data", xmldata);
      ROS_ASSERT(xmldata.getType() == XmlRpc::XmlRpcValue::TypeArray);
      ROS_ASSERT(xmldata.size() == 1);
      const std::string sdata = xmldata[0];

      // Read rotation and translation
      std::istringstream iss(sdata);
      tf::Matrix3x3 R;
      tf::Vector3 t;
      // Row-wise
      for(int i = 0; i < 3; ++i) {
         tf::Vector3& Ri = R[i];
         iss >> Ri.m_floats[0] >> Ri.m_floats[1] >> Ri.m_floats[2] >> t[i];
      }

      // Create transformation
      tf::Transform T;
      T.setBasis(R);
      T.setOrigin(t);

      // Convert extrinsics to Eigen format [m]
      /*
      Teig << R[0][0],R[0][1],R[0][2],0.001f*t[0],\
            R[1][0],R[1][1],R[1][2],0.001f*t[1],\
            R[2][0],R[2][1],R[2][2],0.001f*t[2],\
            0,0,0,1;
      */
      Teig << R[0][0],R[0][1],R[0][2],t[0],\
            R[1][0],R[1][1],R[1][2],t[1],\
            R[2][0],R[2][1],R[2][2],t[2],\
            0,0,0,1;

      // Create broadcaster
      tf::TransformBroadcaster br;

      // Setup rate
      ros::Rate r(rate);

      // Start
      while(ros::ok()) {
         br.sendTransform(tf::StampedTransform(T, ros::Time::now(), source, ns));
         ros::spinOnce();
         r.sleep();
      }
   } else {
      ROS_ERROR_STREAM("No extrinsics available for \"" << ns << "\"!");
      ros::spin();
   }

   return 0;
};


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_HYPOT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_HYPOT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
    /*!
      @brief  hypot generic tag

      Represents the hypot function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct hypot_ : ext::elementwise_<hypot_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<hypot_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_hypot_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::hypot_, Site> dispatching_hypot_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::hypot_, Site>();
    }
    template<class... Args>
    struct impl_hypot_;
  }
  /*!
    Computes \f$(x^2 + y^2)^{1/2}\f$

    @par semantic:
    For any given value @c x,  @c y of floating type @c T:

    @code
    T r = hypot(x, y);
    @endcode

    The code is similar to:

    @code
    T r = sqrt(sqr(x)+sqr(y));
    @endcode

    Provision are made to avoid overflow as possible and to compute
    @c hypot accurately.
    If these considerations can be put aside use
    @c fast_hypot.

    @param  a0
    @param  a1

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::hypot_, hypot, 2)
} }

#endif


#ifndef MATHPARSER_HPP
#define MATHPARSER_HPP

/*=============================================================================
    Copyright (c) 2004 Angus Leeming

    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)
=============================================================================*/
#if defined PHOENIX_LIMIT && PHOENIX_LIMIT < 6
#undef PHOENIX_LIMIT
#endif

#ifndef PHOENIX_LIMIT
#define PHOENIX_LIMIT 6
#endif

#define YAC_SINGLE_COMPILATION_UNIT

#include "external/yac/yac_gnuplot_grammar.hpp"
#include "external/yac/yac_skip_grammar.hpp"
#include "external/yac/yac_virtual_machine.hpp"

#include <boost/spirit/iterator/file_iterator.hpp>
#include <boost/math/special_functions/acosh.hpp>
#include <boost/math/special_functions/asinh.hpp>
#include <boost/math/special_functions/atanh.hpp>

#include <iostream>
#include <sstream>
#include <string>

using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::string;

namespace spirit = boost::spirit;

namespace {

int    bitwise_and(int a, int b) { return a & b; }
int    bitwise_or(int a, int b) { return a | b; }
int    bitwise_xor(int a, int b) { return a ^ b; }
bool   logical_and(bool a, bool b) { return a && b; }
double logical_not(double a) { return !a; }
double equal(double a, double b) { return a == b; }
double not_equal(double a, double b) { return a != b; }
double less(double a, double b) { return a < b; }
double greater(double a, double b) { return a > b; }
double less_equal(double a, double b) { return a <= b; }
double greater_equal(double a, double b) { return a >= b; }
int    shift_left(int a, int b) { return a << b; }
int    shift_right(int a, int b) { return a >> b; }
double add(double a, double b) { return a + b; }
double subtract(double a, double b) { return a - b; }
double multiply(double a, double b) { return a * b; }
double divide(double a, double b) { return a / b; }
int    mod(int a, int b) { return a % b; }
double negate(double a) { return -a; }
double sgn(double a) { return a >= 0.0 ? 1.0 : -1.0; }

typedef int(* ifunc2_ptr_t)(int, int);
typedef bool(* bfunc2_ptr_t)(bool, bool);
typedef double(* dfunc2_ptr_t)(double, double);
typedef double(* dfunc1_ptr_t)(double);

} // namespace anon

namespace yac {

boost::shared_ptr<function> make_func(string const & name, dfunc1_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function1<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, dfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<double>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, bfunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<bool>(name, ptr));
}

boost::shared_ptr<function> make_func(string const & name, ifunc2_ptr_t ptr)
{
    return boost::shared_ptr<function>(new function2<int>(name, ptr));
}

class controller {
private:
	  virtual_machine vm;
	  std::vector<RealType> x_vector, y_vector;
public:
  RealType time;
  controller():
    time(0.)
    {
      std::vector<RealType> X;
      std::vector<RealType> Y;
      X.push_back(0.);
      Y.push_back(0.);
      x_vector = X;
      y_vector = Y;

      char const * const keywords[] = {
	"x", "y",
	"return",
	"pi",
	"abs", "acos", "acosh", "asin", "asinh", "atan",
	"atanh", "besj0", "besj1", "besy0", "besy1", "ceil",
	"cos", "cosh",
#if defined(__GNUC__)
	"erf", "erfc",
#endif
	"exp", "floor", "log", "log10", "sgn", "sin", "sinh",
	"sqrt", "tan", "tanh", "atan2",
      };
      std::size_t const keywords_size =
	sizeof(keywords) / sizeof(keywords[0]);

      for (std::size_t i = 0; i != keywords_size; ++i)
	name_grammar::reserved_keywords.add(keywords[i]);

      vm.global_vars.add
	("pi", 3.1415926536);

      vm.funcs.add
	("bitwise_and##2",   make_func("bitwise_and#",   &::bitwise_and))
	("bitwise_or##2",    make_func("bitwise_or#",    &::bitwise_or))
	("bitwise_xor##2",   make_func("bitwise_xor#",   &::bitwise_xor))
	("logical_and##2",   make_func("logical_and#",   &::logical_and))
	("logical_not##1",   make_func("logical_not#",   &::logical_not))
	("equal##2",         make_func("equal#",         &::equal))
	("not_equal##2",     make_func("not_equal#",     &::not_equal))
	("less##2",          make_func("less#",          &::less))
	("greater##2",       make_func("greater#",       &::greater))
	("less_equal##2",    make_func("less_equal#",    &::less_equal))
	("greater_equal##2", make_func("greater_equal#", &::greater_equal))
	("shift_left##2",    make_func("shift_left#",    &::shift_left))
	("shift_right##2",   make_func("shift_right#",   &::shift_right))
	("add##2",           make_func("add#",           &::add))
	("subtract##2",      make_func("subtract#",      &::subtract))
	("multiply##2",      make_func("multiply#",      &::multiply))
	("divide##2",        make_func("divide#",        &::divide))
	("mod##2",           make_func("mod#",           &::mod))
	("negate##1",        make_func("negate#",        &::negate))
	("abs#1",   make_func("abs",   &std::abs))
	("acos#1",  make_func("acos",  &std::acos))
	("asin#1",  make_func("asin",  &std::asin))
	("atan#1",  make_func("atan",  &std::atan))
	("acosh#1", make_func("acosh", &boost::math::acosh))
	("asinh#1", make_func("asinh", &boost::math::asinh))
	("atanh#1", make_func("atanh", &boost::math::atanh))
	("besj0#1", make_func("besj0", &j0))
	("besj1#1", make_func("besj1", &j1))
	("besy0#1", make_func("besy0", &y0))
	("besy1#1", make_func("besy1", &y1))
	("ceil#1",  make_func("ceil",  &std::ceil))
	("cos#1",   make_func("cos",   &std::cos))
	("cosh#1",  make_func("cosh",  &std::cosh))
#if defined(__GNUC__)
	("erf#1",   make_func("erf",   &erf))
	("erfc#1",  make_func("erfc",  &erfc))
#endif
	("exp#1",   make_func("exp",   &std::exp))
	("floor#1", make_func("floor", &std::floor))
	("log#1",   make_func("log",   &std::log))
	("log10#1", make_func("log10", &std::log10))
	("sgn#1",   make_func("sgn",   &sgn))
	("sin#1",   make_func("sin",   &std::sin))
	("sinh#1",  make_func("sinh",  &std::sinh))
	("sqrt#1",  make_func("sqrt",  &std::sqrt))
	("tan#1",   make_func("tan",   &std::tan))
	("tanh#1",  make_func("tanh",  &std::tanh))
	("atan2#2", make_func("atan2", &std::atan2));
    }

  controller(std::vector<RealType> &X, std::vector<RealType> &Y, RealType &time_)
    : x_vector(X), y_vector(Y), time(time_)
  {
        char const * const keywords[] = {
	  "x", "y", "t",
	  "return",
	  "pi",
            "abs", "acos", "acosh", "asin", "asinh", "atan",
            "atanh", "besj0", "besj1", "besy0", "besy1", "ceil",
            "cos", "cosh",
#if defined(__GNUC__)
            "erf", "erfc",
#endif
            "exp", "floor", "log", "log10", "sgn", "sin", "sinh",
            "sqrt", "tan", "tanh", "atan2",
        };
        std::size_t const keywords_size =
            sizeof(keywords) / sizeof(keywords[0]);

        for (std::size_t i = 0; i != keywords_size; ++i)
                name_grammar::reserved_keywords.add(keywords[i]);

        vm.global_vars.add
	  ("pi", 3.1415926536);

        vm.funcs.add
            ("bitwise_and##2",   make_func("bitwise_and#",   &::bitwise_and))
            ("bitwise_or##2",    make_func("bitwise_or#",    &::bitwise_or))
            ("bitwise_xor##2",   make_func("bitwise_xor#",   &::bitwise_xor))
            ("logical_and##2",   make_func("logical_and#",   &::logical_and))
            ("logical_not##1",   make_func("logical_not#",   &::logical_not))
            ("equal##2",         make_func("equal#",         &::equal))
            ("not_equal##2",     make_func("not_equal#",     &::not_equal))
            ("less##2",          make_func("less#",          &::less))
            ("greater##2",       make_func("greater#",       &::greater))
            ("less_equal##2",    make_func("less_equal#",    &::less_equal))
            ("greater_equal##2", make_func("greater_equal#", &::greater_equal))
            ("shift_left##2",    make_func("shift_left#",    &::shift_left))
            ("shift_right##2",   make_func("shift_right#",   &::shift_right))
            ("add##2",           make_func("add#",           &::add))
            ("subtract##2",      make_func("subtract#",      &::subtract))
            ("multiply##2",      make_func("multiply#",      &::multiply))
            ("divide##2",        make_func("divide#",        &::divide))
            ("mod##2",           make_func("mod#",           &::mod))
            ("negate##1",        make_func("negate#",        &::negate))
            ("abs#1",   make_func("abs",   &std::abs))
            ("acos#1",  make_func("acos",  &std::acos))
            ("asin#1",  make_func("asin",  &std::asin))
            ("atan#1",  make_func("atan",  &std::atan))
            ("acosh#1", make_func("acosh", &boost::math::acosh))
            ("asinh#1", make_func("asinh", &boost::math::asinh))
            ("atanh#1", make_func("atanh", &boost::math::atanh))
            ("besj0#1", make_func("besj0", &j0))
            ("besj1#1", make_func("besj1", &j1))
            ("besy0#1", make_func("besy0", &y0))
            ("besy1#1", make_func("besy1", &y1))
            ("ceil#1",  make_func("ceil",  &std::ceil))
            ("cos#1",   make_func("cos",   &std::cos))
            ("cosh#1",  make_func("cosh",  &std::cosh))
#if defined(__GNUC__)
            ("erf#1",   make_func("erf",   &erf))
            ("erfc#1",  make_func("erfc",  &erfc))
#endif
            ("exp#1",   make_func("exp",   &std::exp))
            ("floor#1", make_func("floor", &std::floor))
            ("log#1",   make_func("log",   &std::log))
            ("log10#1", make_func("log10", &std::log10))
            ("sgn#1",   make_func("sgn",   &sgn))
            ("sin#1",   make_func("sin",   &std::sin))
            ("sinh#1",  make_func("sinh",  &std::sinh))
            ("sqrt#1",  make_func("sqrt",  &std::sqrt))
            ("tan#1",   make_func("tan",   &std::tan))
            ("tanh#1",  make_func("tanh",  &std::tanh))
            ("atan2#2", make_func("atan2", &std::atan2));
    }

  void updateReservedVariables( std::vector<RealType> &X,
				std::vector<RealType> &Y )
  {
    x_vector = X;
    y_vector = Y;
  }

  void updateTime( RealType& t ) {
    time = t;
  }

    template <typename ItT>
    std::vector<RealType> parse( ItT first, ItT last )
  {
        using phoenix::arg1;
        using phoenix::var;

        typedef spirit::parse_info<ItT> parse_info_t;

        gnuplot_grammar calculator(vm.funcs, vm.global_vars);

        parse_info_t info = spirit::parse(first, last,
                                          calculator[var(vm.stk) = arg1],
                                          skip_grammar());

        if (info.full) {
	  std::vector<RealType> result = vectorizedEval(vm.stk,
						      x_vector,
						      y_vector, time );
	  return result;
        } else {
            cerr << "-------------------------\n"
                 << "Math parsing failed\n"
                 << "-------------------------\n";
	    return std::vector<RealType> ( x_vector.size(), 0.0 );
        }
    }
};

} // namespace yac

#endif // MATHPARSER_HPP


/** @file pipeline_benchmark.cpp
 *  @brief Test pipeline performance
 */

#include <armadillo>
#include <benchmark/benchmark.h>
#include <load_data.h>
#include <load_json_config.h>
#include <stp.h>

// Cmake variable
#ifndef _PIPELINE_TESTPATH
#define _PIPELINE_TESTPATH 0
#endif

std::string data_path(_PIPELINE_DATAPATH);
std::string input_npz("simdata_nstep10.npz");
std::string config_path(_PIPELINE_CONFIGPATH);
std::string config_file_exact("fastimg_exact_config.json");
std::string config_file_oversampling("fastimg_oversampling_config.json");

stp::SourceFindImage run_pipeline(arma::mat& uvw_lambda, arma::cx_mat& residual_vis, arma::mat& vis_weights, ConfigurationFile& cfg)
{
    stp::PSWF kernel_func(cfg.img_pars.kernel_support);
    std::pair<arma::Mat<real_t>, arma::Mat<real_t>> result = stp::image_visibilities(kernel_func, residual_vis, vis_weights, uvw_lambda, cfg.img_pars);
    result.second.reset();

    return stp::SourceFindImage(std::move(result.first), cfg.detection_n_sigma, cfg.analysis_n_sigma, cfg.estimate_rms,
        cfg.find_negative_sources, cfg.sigma_clip_iters, cfg.median_method, cfg.gaussian_fitting, cfg.ccl_4connectivity,
        cfg.generate_labelmap, cfg.source_min_area, cfg.ceres_diffmethod, cfg.ceres_solvertype);
}

static void pipeline_kernel_exact_benchmark(benchmark::State& state)
{
    int image_size = state.range(0);

    //Load simulated data from input_npz
    arma::mat input_uvw = load_npy_double_array<double>(data_path + input_npz, "uvw_lambda");
    arma::cx_mat input_vis = load_npy_complex_array<double>(data_path + input_npz, "vis");
    arma::mat input_snr_weights = load_npy_double_array<double>(data_path + input_npz, "snr_weights");
    arma::mat skymodel = load_npy_double_array<double>(data_path + input_npz, "skymodel");

    // Generate model visibilities from the skymodel and UVW-baselines
    arma::cx_mat input_model = stp::generate_visibilities_from_local_skymodel(skymodel, input_uvw);

    // Subtract model-generated visibilities from incoming data
    arma::cx_mat residual_vis = input_vis - input_model;

    // Load all configurations from json configuration file
    ConfigurationFile cfg(config_path + config_file_exact);
    cfg.img_pars.image_size = image_size;

    for (auto _ : state) {
        benchmark::DoNotOptimize(run_pipeline(input_uvw, residual_vis, input_snr_weights, cfg));
    }
}

static void pipeline_kernel_oversampling_benchmark(benchmark::State& state)
{
    int image_size = state.range(0);

    //Load simulated data from input_npz
    arma::mat input_uvw = load_npy_double_array<double>(data_path + input_npz, "uvw_lambda");
    arma::cx_mat input_vis = load_npy_complex_array<double>(data_path + input_npz, "vis");
    arma::mat input_snr_weights = load_npy_double_array<double>(data_path + input_npz, "snr_weights");
    arma::mat skymodel = load_npy_double_array<double>(data_path + input_npz, "skymodel");

    // Generate model visibilities from the skymodel and UVW-baselines
    arma::cx_mat input_model = stp::generate_visibilities_from_local_skymodel(skymodel, input_uvw);

    // Load all configurations from json configuration file
    ConfigurationFile cfg(config_path + config_file_oversampling);
    cfg.img_pars.image_size = image_size;

    // Subtract model-generated visibilities from incoming data
    arma::cx_mat residual_vis = input_vis - input_model;

    for (auto _ : state) {
        benchmark::DoNotOptimize(run_pipeline(input_uvw, residual_vis, input_snr_weights, cfg));
    }
}

BENCHMARK(pipeline_kernel_oversampling_benchmark)
    ->RangeMultiplier(2)
    ->Range(1 << 10, 1 << 16)
    ->Unit(benchmark::kMillisecond);

BENCHMARK(pipeline_kernel_exact_benchmark)
    ->RangeMultiplier(2)
    ->Range(1 << 10, 1 << 16)
    ->Unit(benchmark::kMillisecond);

BENCHMARK_MAIN();


#define _CRT_SECURE_NO_WARNINGS

#include "MeshLoader.h"
#include "SignedDistance.h"
#include "Visualize.h"
#include <boost/gil/extension/io/pnm.hpp>
#include <iostream>

using namespace std;
using namespace MeshSdf;

// NOTE: argv[1] should contain path to mesh in obj format.
int main(int argc, char* argv[])
{
	if (argc <= 1) {
		std::cout << "Usage: Demo path/to/mesh z_value x_pixels";
		exit(-1);
	}

	std::cout << "Loading mesh: " << argv[1] << '\n';
	auto mesh = LoadObj(argv[1]);

	std::cout << "Converting mesh to sdf...\n";
	SignedDistance df{ mesh };
	//UnsignedDistance df{ mesh.verts, mesh.tris };

	auto const DepthPercent = std::stoi(argv[2]);
	auto boundBox = Scale(BoundingBox(mesh), 1.3f);
	auto aspectRatio = (boundBox.ymax - boundBox.ymin) / (boundBox.xmax - boundBox.xmin);
	auto const ImWidth = std::stoi(argv[3]);
	auto const ImHeight = static_cast<int>(aspectRatio * ImWidth);
	std::cout << "Computing SDF slice for z=" << DepthPercent << ", image size=" << ImHeight << "x" << ImWidth << '\n';
	auto imgSlice = ColorCodedSlice(df, ImHeight, ImWidth, boundBox, DepthPercent);

	std::cout << "Writing to file...\n";
	write_view("./sdf_slice.pnm", const_view(imgSlice), boost::gil::pnm_tag{});
}


/*
 *              Copyright Andrey Semashev 2016.
 * 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)
 */
/*!
 * \file   murmur3.hpp
 * \author Andrey Semashev
 * \date   16.01.2016
 *
 * \brief  This header is the Boost.Log library implementation, see the library documentation
 *         at http://www.boost.org/doc/libs/release/libs/log/doc/html/index.html.
 *
 * This file implements MurmurHash3 hash function implementation. See https://en.wikipedia.org/wiki/MurmurHash.
 */

#ifndef BOOST_LOG_MURMUR3_HPP_INCLUDED_
#define BOOST_LOG_MURMUR3_HPP_INCLUDED_

#include <boost/log/detail/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/log/detail/header.hpp>

namespace boost {

BOOST_LOG_OPEN_NAMESPACE

namespace aux {

//! 32-bit MurmurHash3 algorithm implementation (https://en.wikipedia.org/wiki/MurmurHash)
class murmur3_32
{
private:
    uint32_t m_state;
    uint32_t m_len;

    static BOOST_CONSTEXPR_OR_CONST uint32_t c1 = 0xcc9e2d51;
    static BOOST_CONSTEXPR_OR_CONST uint32_t c2 = 0x1b873593;
    static BOOST_CONSTEXPR_OR_CONST uint32_t r1 = 15;
    static BOOST_CONSTEXPR_OR_CONST uint32_t r2 = 13;
    static BOOST_CONSTEXPR_OR_CONST uint32_t m = 5;
    static BOOST_CONSTEXPR_OR_CONST uint32_t n = 0xe6546b64;

public:
    explicit BOOST_CONSTEXPR murmur3_32(uint32_t seed) BOOST_NOEXCEPT : m_state(seed), m_len(0u)
    {
    }

    //! Mixing stage of the 32-bit MurmurHash3 algorithm
    void mix(uint32_t value) BOOST_NOEXCEPT
    {
        value *= c1;
        value = (value << r1) | (value >> (32u - r1));
        value *= c2;

        uint32_t h = m_state ^ value;
        m_state = ((h << r2) | (h >> (32u - r2))) * m + n;
        m_len += 4u;
    }

    //! Finalization stage of the 32-bit MurmurHash3 algorithm
    uint32_t finalize() BOOST_NOEXCEPT
    {
        uint32_t h = m_state ^ m_len;
        h ^= h >> 16u;
        h *= 0x85ebca6bu;
        h ^= h >> 13u;
        h *= 0xc2b2ae35u;
        h ^= h >> 16u;
        m_state = h;
        return h;
    }
};

} // namespace aux

BOOST_LOG_CLOSE_NAMESPACE // namespace log

} // namespace boost

#include <boost/log/detail/footer.hpp>

#endif // BOOST_LOG_MURMUR3_HPP_INCLUDED_


#include "Scheme.h"

#include <NTL/RR.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <cmath>

#include "EvaluatorUtils.h"
#include "NumUtils.h"
#include "Params.h"
#include "PubKey.h"
#include "Ring2Utils.h"

using namespace std;
using namespace NTL;

CZZ* Scheme::groupidx(CZZ*& vals, long slots) {
	CZZ* res = new CZZ[slots * 2];
	long logslots = log2(slots);
	for (long i = 0; i < slots; ++i) {
		long idx = (params.rotGroup[i] % (4 * slots) - 1) / 2;
		res[idx] = vals[i];
		res[2 * slots - idx - 1] = vals[i].conjugate();
	}
	return res;
}

CZZ* Scheme::groupidx(CZZ& val) {
	CZZ* res = new CZZ[2];
	res[0] = val;
	res[1] = val.conjugate();
	return res;
}

CZZ* Scheme::degroupidx(CZZ*& vals, long slots) {
	long logslots = log2(slots);
	CZZ* res = new CZZ[slots];
	for (long i = 0; i < slots; ++i) {
		long idx = (params.rotGroup[i] % (4 * slots) - 1) / 2;
		res[i] = vals[idx];
	}
	return res;
}

//-----------------------------------------

Message Scheme::encodeWithBits(CZZ*& gvals, long cbits, long slots) {
	ZZX mx;
	mx.SetLength(params.N);
	ZZ mod = power2_ZZ(cbits);
	long idx = 0;
	long doubleslots = slots << 1;
	long logDoubleslots = log2(slots) + 1;
	long gap = (params.N >> logDoubleslots);
	NumUtils::fftSpecialInv(gvals, doubleslots, aux);
	for (long i = 0; i < doubleslots; ++i) {
		mx.rep[idx] = gvals[i].r;
		idx += gap;
	}
	return Message(mx, mod, cbits, slots);
}

Message Scheme::encode(CZZ*& gvals, long slots) {
	ZZX mx;
	mx.SetLength(params.N);
	long idx = 0;
	long doubleslots = slots << 1;
	long logDoubleslots = log2(slots) + 1;
	long gap = (params.N >> logDoubleslots);

	NumUtils::fftSpecialInv(gvals, doubleslots, aux);

	for (long i = 0; i < doubleslots; ++i) {
		mx.rep[idx] = gvals[i].r;
		idx += gap;
	}
	return Message(mx, params.q, params.logq, slots);
}

Cipher Scheme::encryptMsg(Message& msg) {
	ZZX ax, bx, vx, eax, ebx;
	NumUtils::sampleZO(vx, params.N);
	Ring2Utils::mult(ax, vx, publicKey.ax, msg.mod, params.N);
	NumUtils::sampleGauss(eax, params.N, params.sigma);
	Ring2Utils::addAndEqual(ax, eax, msg.mod, params.N);

	Ring2Utils::mult(bx, vx, publicKey.bx, msg.mod, params.N);
	NumUtils::sampleGauss(ebx, params.N, params.sigma);
	Ring2Utils::addAndEqual(bx, ebx, msg.mod, params.N);

	Ring2Utils::addAndEqual(bx, msg.mx, msg.mod, params.N);

	return Cipher(ax, bx, msg.mod, msg.cbits, msg.slots);
}

Cipher Scheme::encryptWithBits(CZZ*& vals, long cbits, long slots) {
	CZZ* gvals = groupidx(vals, slots);
	Message msg = encodeWithBits(gvals, cbits, slots);
	delete[] gvals;
	return encryptMsg(msg);
}

Cipher Scheme::encrypt(CZZ*& vals, long slots) {
	CZZ* gvals = groupidx(vals, slots);
	Message msg = encode(gvals, slots);
	delete[] gvals;
	return encryptMsg(msg);
}

Cipher Scheme::encryptSingleWithBits(CZZ& val, long cbits) {
	CZZ* gvals = groupidx(val);
	Message msg = encodeWithBits(gvals, cbits, 1);
	delete[] gvals;
	return encryptMsg(msg);
}

Cipher Scheme::encryptSingle(CZZ& val) {
	CZZ* gvals = groupidx(val);
	Message msg = encode(gvals, 1);
	delete[] gvals;
	return encryptMsg(msg);
}

//-----------------------------------------

Message Scheme::decryptMsg(SecKey& secretKey, Cipher& cipher) {
	ZZX mx;
	Ring2Utils::mult(mx, cipher.ax, secretKey.sx, cipher.mod, params.N);
	Ring2Utils::addAndEqual(mx, cipher.bx, cipher.mod, params.N);
	return Message(mx, cipher.mod, cipher.cbits, cipher.slots);
}

CZZ* Scheme::decode(Message& msg) {
	long doubleslots = msg.slots * 2;
	CZZ* fftinv = new CZZ[doubleslots];

	long idx = 0;
	long gap = params.N / doubleslots;
	for (long i = 0; i < doubleslots; ++i) {
		ZZ tmp = msg.mx.rep[idx] % msg.mod;
		if(NumBits(tmp) == msg.cbits) tmp -= msg.mod;
		fftinv[i] = CZZ(tmp, ZZ(0));
		idx += gap;
	}
	NumUtils::fftSpecial(fftinv, doubleslots, aux);
	return fftinv;
}

CZZ* Scheme::decrypt(SecKey& secretKey, Cipher& cipher) {
	Message msg = decryptMsg(secretKey, cipher);
	CZZ* gvals = decode(msg);
	CZZ* res = degroupidx(gvals, msg.slots);
	delete[] gvals;
	return res;
}

CZZ Scheme::decryptSingle(SecKey& secretKey, Cipher& cipher) {
	Message msg = decryptMsg(secretKey, cipher);
	CZZ* gvals = decode(msg);
	CZZ res = gvals[0];
	delete[] gvals;
	return res;
}

//-----------------------------------------

Cipher Scheme::negate(Cipher& cipher){
    ZZX ax, bx;

    Ring2Utils::multByConst(ax, cipher.ax, to_ZZ("-1"), cipher.mod, params.N);
    Ring2Utils::multByConst(bx, cipher.bx, to_ZZ("-1"), cipher.mod, params.N);

    return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::negateAndEqual(Cipher& cipher){
    Ring2Utils::multByConstAndEqual(cipher.ax, to_ZZ("-1"), cipher.mod, params.N);
    Ring2Utils::multByConstAndEqual(cipher.bx, to_ZZ("-1"), cipher.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::add(Cipher& cipher1, Cipher& cipher2) {
	ZZX ax, bx;

	Ring2Utils::add(ax, cipher1.ax, cipher2.ax, cipher1.mod, params.N);
	Ring2Utils::add(bx, cipher1.bx, cipher2.bx, cipher1.mod, params.N);

	return Cipher(ax, bx, cipher1.mod, cipher1.cbits, cipher1.slots);
}

void Scheme::addAndEqual(Cipher& cipher1, Cipher& cipher2) {
	Ring2Utils::addAndEqual(cipher1.ax, cipher2.ax, cipher1.mod, params.N);
	Ring2Utils::addAndEqual(cipher1.bx, cipher2.bx, cipher1.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::addConst(Cipher& cipher, ZZ& cnst) {
	ZZX ax = cipher.ax;
	ZZX bx = cipher.bx;

	AddMod(bx.rep[0], cipher.bx.rep[0], cnst, cipher.mod);
	return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::addConstAndEqual(Cipher& cipher, ZZ& cnst) {
	ZZ mod = power2_ZZ(cipher.cbits);
	AddMod(cipher.bx.rep[0], cipher.bx.rep[0], cnst, mod);
}

//-----------------------------------------

Cipher Scheme::addByPoly(Cipher& cipher, Message& msg) {
    ZZX axres, bxres;

    //Ring2Utils::add(axres, cipher.ax, msg.mx, cipher.mod, params.N);
    Ring2Utils::add(bxres, cipher.bx, msg.mx, cipher.mod, params.N);
    return Cipher(cipher.ax, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::addByPolyAndEqual(Cipher& cipher, Message& msg) {
    //Ring2Utils::addAndEqual(cipher.ax, msg.mx, cipher.mod, params.N);
    Ring2Utils::addAndEqual(cipher.bx, msg.mx, cipher.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::sub(Cipher& cipher1, Cipher& cipher2) {
	ZZX ax, bx;

	Ring2Utils::sub(ax, cipher1.ax, cipher2.ax, cipher1.mod, params.N);
	Ring2Utils::sub(bx, cipher1.bx, cipher2.bx, cipher1.mod, params.N);

	return Cipher(ax, bx, cipher1.mod, cipher1.cbits, cipher1.slots);
}

void Scheme::subAndEqual(Cipher& cipher1, Cipher& cipher2) {
	Ring2Utils::subAndEqual(cipher1.ax, cipher2.ax, cipher1.mod, params.N);
	Ring2Utils::subAndEqual(cipher1.bx, cipher2.bx, cipher1.mod, params.N);
}

void Scheme::subAndEqual2(Cipher& cipher1, Cipher& cipher2) {
	Ring2Utils::subAndEqual2(cipher1.ax, cipher2.ax, cipher1.mod, params.N);
	Ring2Utils::subAndEqual2(cipher1.bx, cipher2.bx, cipher1.mod, params.N);
}

//-----------------------------------------

// cipher - msg  = (b, a) - (mx, 0)
Cipher Scheme::subByPoly(Cipher& cipher, Message& msg) {
    ZZX bxres;
    //Ring2Utils::sub(axres, cipher.ax, msg.mx, cipher.mod, params.N);
    Ring2Utils::sub(bxres, cipher.bx, msg.mx, cipher.mod, params.N);
    return Cipher(cipher.ax, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::subByPolyAndEqual(Cipher& cipher, Message& msg) {

    Ring2Utils::subAndEqual(cipher.bx, msg.mx, cipher.mod, params.N);
}

// msg - cipher  = (mx, 0 ) - (b, a)
Cipher Scheme::subByPoly2(Cipher& cipher, Message& msg) {
    ZZX axres, bxres;

    Ring2Utils::multByConst(axres, cipher.ax, to_ZZ("-1"), cipher.mod, params.N);
    Ring2Utils::sub(bxres, msg.mx, cipher.bx, cipher.mod, params.N);
    return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

//-----------------------------------------

Cipher Scheme::subConst(Cipher& cipher, ZZ& cnst){
    ZZX ax = cipher.ax;
    ZZX bx = cipher.bx;

    SubMod(bx.rep[0], cipher.bx.rep[0], cnst, cipher.mod);
    return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

Cipher Scheme::subConst2(Cipher& cipher, ZZ& cnst){
    Cipher res = negate(cipher);
    addConstAndEqual(res, cnst);

    return res;
}

void Scheme::subConstAndEqual(Cipher& cipher, ZZ& cnst){
    //ZZX ax = cipher.ax;
    //ZZX bx = cipher.bx;

    SubMod(cipher.bx.rep[0], cipher.bx.rep[0], cnst, cipher.mod);
    //return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}
//-----------------------------------------

Cipher Scheme::conjugate(Cipher& cipher) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxconj, bxres, axres;

	Ring2Utils::conjugate(bxconj, cipher.bx, params.N);
	Ring2Utils::conjugate(bxres, cipher.ax, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axConj, Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxConj, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxconj, cipher.mod, params.N);
	return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::conjugateAndEqual(Cipher& cipher) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxconj, bxres, axres;

	Ring2Utils::conjugate(bxconj, cipher.bx, params.N);
	Ring2Utils::conjugate(bxres, cipher.ax, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axConj, Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxConj, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxconj, cipher.mod, params.N);

	cipher.ax = axres;
	cipher.bx = bxres;
}

Cipher Scheme::imult(Cipher& cipher, const long precisionBits) {
	ZZ tmp = EvaluatorUtils::evaluateVal(sqrt(to_RR(2.0)), precisionBits - 1);

	ZZX bxres, axres, axtmp, bxtmp;

	Ring2Utils::multByMonomial(axtmp, cipher.ax, params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(axtmp, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomial(bxtmp, cipher.bx, params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(bxtmp, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomial(axres, cipher.ax, 3 * params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(axres, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomial(bxres, cipher.bx, 3 * params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(bxres, tmp, cipher.mod, params.N);

	Ring2Utils::addAndEqual(axres, axtmp, cipher.mod, params.N);
	Ring2Utils::addAndEqual(bxres, bxtmp, cipher.mod, params.N);

	Cipher res(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
	modSwitchAndEqual(res, precisionBits);
	return res;
}

void Scheme::imultAndEqual(Cipher& cipher, const long precisionBits) {

	ZZ tmp = EvaluatorUtils::evaluateVal(sqrt(to_RR(2.0)), precisionBits - 1);

	ZZX axtmp, bxtmp;

	Ring2Utils::multByMonomial(axtmp, cipher.ax, params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(axtmp, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomial(bxtmp, cipher.bx, params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(bxtmp, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomialAndEqual(cipher.ax, 3 * params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(cipher.ax, tmp, cipher.mod, params.N);

	Ring2Utils::multByMonomialAndEqual(cipher.bx, 3 * params.N / 4, params.N);
	Ring2Utils::multByConstAndEqual(cipher.bx, tmp, cipher.mod, params.N);

	Ring2Utils::addAndEqual(cipher.ax, axtmp, cipher.mod, params.N);
	Ring2Utils::addAndEqual(cipher.bx, bxtmp, cipher.mod, params.N);

	modSwitchAndEqual(cipher, precisionBits);
}

Cipher Scheme::mult(Cipher& cipher1, Cipher& cipher2) {
	ZZ Pmod = cipher1.mod << params.logq;

	ZZX axbx1 = Ring2Utils::add(cipher1.ax, cipher1.bx, cipher1.mod, params.N);
	ZZX axbx2 = Ring2Utils::add(cipher2.ax, cipher2.bx, cipher1.mod, params.N);
	Ring2Utils::multAndEqual(axbx1, axbx2, cipher1.mod, params.N);

	ZZX bxbx = Ring2Utils::mult(cipher1.bx, cipher2.bx, cipher1.mod, params.N);
	ZZX axax = Ring2Utils::mult(cipher1.ax, cipher2.ax, cipher1.mod, params.N);

	ZZX axmult = Ring2Utils::mult(axax, publicKey.axStar, Pmod, params.N);
	ZZX bxmult = Ring2Utils::mult(axax, publicKey.bxStar, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axmult, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxmult, params.logq, params.N);

	Ring2Utils::addAndEqual(axmult, axbx1, cipher1.mod, params.N);
	Ring2Utils::subAndEqual(axmult, bxbx, cipher1.mod, params.N);
	Ring2Utils::subAndEqual(axmult, axax, cipher1.mod, params.N);
	Ring2Utils::addAndEqual(bxmult, bxbx, cipher1.mod, params.N);

	return Cipher(axmult, bxmult, cipher1.mod, cipher1.cbits, cipher1.slots);
}

void Scheme::multAndEqual(Cipher& cipher1, Cipher& cipher2) {
	ZZ Pmod = cipher1.mod << params.logq;

	ZZX axbx1 = Ring2Utils::add(cipher1.ax, cipher1.bx, cipher1.mod, params.N);
	ZZX axbx2 = Ring2Utils::add(cipher2.ax, cipher2.bx, cipher1.mod, params.N);
	Ring2Utils::multAndEqual(axbx1, axbx2, cipher1.mod, params.N);

	ZZX bxbx = Ring2Utils::mult(cipher1.bx, cipher2.bx, cipher1.mod, params.N);
	ZZX axax = Ring2Utils::mult(cipher1.ax, cipher2.ax, cipher1.mod, params.N);

	cipher1.ax = Ring2Utils::mult(axax, publicKey.axStar, Pmod, params.N);
	cipher1.bx = Ring2Utils::mult(axax, publicKey.bxStar, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(cipher1.ax, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(cipher1.bx, params.logq, params.N);

	Ring2Utils::addAndEqual(cipher1.ax, axbx1, cipher1.mod, params.N);
	Ring2Utils::subAndEqual(cipher1.ax, bxbx, cipher1.mod, params.N);
	Ring2Utils::subAndEqual(cipher1.ax, axax, cipher1.mod, params.N);
	Ring2Utils::addAndEqual(cipher1.bx, bxbx, cipher1.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::square(Cipher& cipher) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX axax, axbx, bxbx, bxmult, axmult;

	Ring2Utils::square(bxbx, cipher.bx, cipher.mod, params.N);
	Ring2Utils::mult(axbx, cipher.ax, cipher.bx, cipher.mod, params.N);
	Ring2Utils::addAndEqual(axbx, axbx, cipher.mod, params.N);
	Ring2Utils::square(axax, cipher.ax, cipher.mod, params.N);

	Ring2Utils::mult(axmult, axax, publicKey.axStar, Pmod, params.N);
	Ring2Utils::mult(bxmult, axax, publicKey.bxStar, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axmult, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxmult, params.logq, params.N);

	Ring2Utils::addAndEqual(axmult, axbx, cipher.mod, params.N);
	Ring2Utils::addAndEqual(bxmult, bxbx, cipher.mod, params.N);

	return Cipher(axmult, bxmult, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::squareAndEqual(Cipher& cipher) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxbx, axbx, axax, bxmult, axmult;

	Ring2Utils::square(bxbx, cipher.bx, cipher.mod, params.N);
	Ring2Utils::mult(axbx, cipher.bx, cipher.ax, cipher.mod, params.N);
	Ring2Utils::addAndEqual(axbx, axbx, cipher.mod, params.N);
	Ring2Utils::square(axax, cipher.ax, cipher.mod, params.N);

	Ring2Utils::mult(axmult, axax, publicKey.axStar, Pmod, params.N);
	Ring2Utils::mult(bxmult, axax, publicKey.bxStar, Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axmult, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxmult, params.logq, params.N);

	Ring2Utils::addAndEqual(axmult, axbx, cipher.mod, params.N);
	Ring2Utils::addAndEqual(bxmult, bxbx, cipher.mod, params.N);

	cipher.bx = bxmult;
	cipher.ax = axmult;
}

//-----------------------------------------

Cipher Scheme::multByConst(Cipher& cipher, ZZ& cnst) {
	ZZX ax, bx;
	Ring2Utils::multByConst(ax, cipher.ax, cnst, cipher.mod, params.N);
	Ring2Utils::multByConst(bx, cipher.bx, cnst, cipher.mod, params.N);

	return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::multByConstAndEqual(Cipher& cipher, ZZ& cnst) {
	Ring2Utils::multByConstAndEqual(cipher.ax, cnst, cipher.mod, params.N);
	Ring2Utils::multByConstAndEqual(cipher.bx, cnst, cipher.mod, params.N);
}

Cipher Scheme::multByPoly(Cipher& cipher, ZZX& poly) {
	ZZX axres, bxres;
	Ring2Utils::mult(axres, cipher.ax, poly, cipher.mod, params.N);
	Ring2Utils::mult(bxres, cipher.bx, poly, cipher.mod, params.N);
	return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::multByPolyAndEqual(Cipher& cipher, ZZX& poly) {
	Ring2Utils::multAndEqual(cipher.ax, poly, cipher.mod, params.N);
	Ring2Utils::multAndEqual(cipher.bx, poly, cipher.mod, params.N);
}

Cipher Scheme::multByConstBySlots(Cipher& cipher, CZZ*& cnstvec) {
	CZZ* gcnstvec = groupidx(cnstvec, cipher.slots);
	Message msg = encode(gcnstvec, cipher.slots);
	delete[] gcnstvec;

	ZZX axres, bxres;
	Ring2Utils::mult(axres, cipher.ax, msg.mx, cipher.mod, params.N);
	Ring2Utils::mult(bxres, cipher.bx, msg.mx, cipher.mod, params.N);
	return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::multByConstBySlotsAndEqual(Cipher& cipher, CZZ*& cnstvec) {
	CZZ* gcnstvec = groupidx(cnstvec, cipher.slots);
	Message msg = encode(gcnstvec, cipher.slots);
	delete[] gcnstvec;

	Ring2Utils::multAndEqual(cipher.ax, msg.mx, cipher.mod, params.N);
	Ring2Utils::multAndEqual(cipher.bx, msg.mx, cipher.mod, params.N);
}

Cipher Scheme::multByPoly(Cipher& cipher, Message& msg) {
    ZZX axres, bxres;
    Ring2Utils::mult(axres, cipher.ax, msg.mx, cipher.mod, params.N);
    Ring2Utils::mult(bxres, cipher.bx, msg.mx, cipher.mod, params.N);
    return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::multByPolyAndEqual(Cipher& cipher, Message& msg) {
    Ring2Utils::multAndEqual(cipher.ax, msg.mx, cipher.mod, params.N);
    Ring2Utils::multAndEqual(cipher.bx, msg.mx, cipher.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::multByMonomial(Cipher& cipher, const long degree) {
	ZZX ax, bx;

	Ring2Utils::multByMonomial(ax, cipher.ax, degree, params.N);
	Ring2Utils::multByMonomial(bx, cipher.bx, degree, params.N);

	return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::multByMonomialAndEqual(Cipher& cipher, const long degree) {
	Ring2Utils::multByMonomialAndEqual(cipher.ax, degree, params.N);
	Ring2Utils::multByMonomialAndEqual(cipher.bx, degree, params.N);
}

//-----------------------------------------

Cipher Scheme::leftShift(Cipher& cipher, long bits) {
	ZZX ax, bx;

	Ring2Utils::leftShift(ax, cipher.ax, bits, cipher.mod, params.N);
	Ring2Utils::leftShift(bx, cipher.bx, bits, cipher.mod, params.N);

	return Cipher(ax, bx, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::leftShiftAndEqual(Cipher& cipher, long bits) {
	Ring2Utils::leftShiftAndEqual(cipher.ax, bits, cipher.mod, params.N);
	Ring2Utils::leftShiftAndEqual(cipher.bx, bits, cipher.mod, params.N);
}

void Scheme::doubleAndEqual(Cipher& cipher) {
	Ring2Utils::doubleAndEqual(cipher.ax, cipher.mod, params.N);
	Ring2Utils::doubleAndEqual(cipher.bx, cipher.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::modSwitch(Cipher& cipher, long bitsDown) {
	ZZX ax, bx;

	Ring2Utils::rightShift(ax, cipher.ax, bitsDown, params.N);
	Ring2Utils::rightShift(bx, cipher.bx, bitsDown, params.N);

	long newcbits = cipher.cbits - bitsDown;
	ZZ newmod = cipher.mod >> bitsDown;
	return Cipher(ax, bx, newmod, newcbits, cipher.slots);
}

void Scheme::modSwitchAndEqual(Cipher& cipher, long bitsDown) {
	Ring2Utils::rightShiftAndEqual(cipher.ax, bitsDown, params.N);
	Ring2Utils::rightShiftAndEqual(cipher.bx, bitsDown, params.N);
	cipher.cbits -= bitsDown;
	cipher.mod >>= bitsDown;
}

Cipher Scheme::modEmbed(Cipher& cipher, long bitsDown) {
	ZZ newmod = cipher.mod >> bitsDown;
	long newcbits = cipher.cbits - bitsDown;
	ZZX bx, ax;
	Ring2Utils::mod(ax, cipher.ax, newmod, params.N);
	Ring2Utils::mod(bx, cipher.bx, newmod, params.N);
	return Cipher(ax, bx, newmod, newcbits, cipher.slots);
}

void Scheme::modEmbedAndEqual(Cipher& cipher, long bitsDown) {
	cipher.mod >>= bitsDown;
	cipher.cbits -= bitsDown;
	Ring2Utils::modAndEqual(cipher.ax, cipher.mod, params.N);
	Ring2Utils::modAndEqual(cipher.bx, cipher.mod, params.N);
}

//-----------------------------------------

Cipher Scheme::leftRotateByPo2(Cipher& cipher, long logrotSlots) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxrot, bxres, axres;

	long rotSlots = (1 << logrotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, params.rotGroup[rotSlots], params.q, params.N);
	Ring2Utils::inpower(bxres, cipher.ax, params.rotGroup[rotSlots], params.q, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axLeftRot[logrotSlots], Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxLeftRot[logrotSlots], Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxrot, cipher.mod, params.N);
	return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::leftRotateByPo2AndEqual(Cipher& cipher, long logrotSlots) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxrot, bxres, axres;

	long rotSlots = (1 << logrotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, params.rotGroup[rotSlots], params.q, params.N);
	Ring2Utils::inpower(bxres, cipher.ax, params.rotGroup[rotSlots], params.q, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axLeftRot[logrotSlots], Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxLeftRot[logrotSlots], Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxrot, cipher.mod, params.N);

	cipher.ax = axres;
	cipher.bx = bxres;
}

Cipher Scheme::rightRotateByPo2(Cipher& cipher, long logrotSlots) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxrot, bxres, axres;

	long rotSlots = params.N/2 - (1 << logrotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, params.rotGroup[rotSlots], params.q, params.N);
	Ring2Utils::inpower(bxres, cipher.ax, params.rotGroup[rotSlots], params.q, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axRightRot[logrotSlots], Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxRightRot[logrotSlots], Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxrot, cipher.mod, params.N);
	return Cipher(axres, bxres, cipher.mod, cipher.cbits, cipher.slots);
}

void Scheme::rightRotateByPo2AndEqual(Cipher& cipher, long logrotSlots) {
	ZZ Pmod = cipher.mod << params.logq;

	ZZX bxrot, bxres, axres;

	long rotSlots = params.N/2 - (1 << logrotSlots);

	Ring2Utils::inpower(bxrot, cipher.bx, params.rotGroup[rotSlots], params.q, params.N);
	Ring2Utils::inpower(bxres, cipher.ax, params.rotGroup[rotSlots], params.q, params.N);

	Ring2Utils::mult(axres, bxres, publicKey.axRightRot[logrotSlots], Pmod, params.N);
	Ring2Utils::multAndEqual(bxres, publicKey.bxRightRot[logrotSlots], Pmod, params.N);

	Ring2Utils::rightShiftAndEqual(axres, params.logq, params.N);
	Ring2Utils::rightShiftAndEqual(bxres, params.logq, params.N);

	Ring2Utils::addAndEqual(bxres, bxrot, cipher.mod, params.N);

	cipher.ax = axres;
	cipher.bx = bxres;
}

Cipher Scheme::leftRotate(Cipher& cipher, long rotSlots) {
	Cipher res = cipher;
	leftRotateAndEqual(res, rotSlots);
	return res;
}

void Scheme::leftRotateAndEqual(Cipher& cipher, long rotSlots) {
	long remrotSlots = rotSlots % cipher.slots;
	long logrotSlots = log2(remrotSlots) + 1;
	for (long i = 0; i < logrotSlots; ++i) {
		if(bit(remrotSlots, i)) {
			leftRotateByPo2AndEqual(cipher, i);
		}
	}
}

Cipher Scheme::rightRotate(Cipher& cipher, long rotSlots) {
	Cipher res = cipher;
	rightRotateAndEqual(res, rotSlots);
	return res;
}

void Scheme::rightRotateAndEqual(Cipher& cipher, long rotSlots) {
	long remrotSlots = rotSlots % cipher.slots;
	long logrotSlots = log2(remrotSlots) + 1;
	for (long i = 0; i < logrotSlots; ++i) {
		if(bit(remrotSlots, i)) {
			rightRotateByPo2AndEqual(cipher, i);
		}
	}
}


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* 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)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <boost/random.hpp>

typedef rokko::localized_vector<double> vector_t;

int main(int argc, char *argv[]) {
  int n = 6;
  vector_t v(n);
  boost::mt19937 engine(12345l);
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<> >
    gauss(engine, boost::normal_distribution<>());
  for (int i = 0; i < n; ++i) v(i) = gauss();
  std::cout << "v: " << v << std::endl;
}


#include <iostream>
#include <thread>
#include <fstream>
#include <string>
#include <filesystem>
#include <tuple>
#include <algorithm>
#include <chrono>
#include <regex>

#include <boost/multiprecision/cpp_int.hpp>

using namespace std;

namespace mp = boost::multiprecision;
namespace fs = std::filesystem;

const auto iter = 1 + 28;
const string resultsDirectory = "results";

void saveResult(const mp::cpp_int &power, int num) {
    vector<unsigned char> buffer;
    export_bits(power, back_inserter(buffer), 8);

    fstream file(resultsDirectory + "/" + to_string(num) + ".txt", fstream::out | fstream::trunc);
    for (unsigned char val: buffer) {
        file.put(val);
    }
    file.close();
}

void saveResult(const mp::cpp_int &power, const string &filename) {
    vector<unsigned char> buffer;
    export_bits(power, back_inserter(buffer), 8);

    fstream file(resultsDirectory + "/" + filename + ".txt", fstream::out | fstream::trunc);
    for (unsigned char val: buffer) {
        file.put(val);
    }
    file.close();
}

tuple <mp::cpp_int*, int> loadResults() {
    auto *powers = new mp::cpp_int[iter];
    mp::cpp_int partial;
    vector<int> loadedPowers;
    auto numberToLarge = false;

    for (const auto &entry : fs::directory_iterator(resultsDirectory)) {
        auto filename = fs::path(entry).stem(); // Get filename without path and extension
        int power = atoi(filename.c_str()); // Convert string into integer

        if (power == 0) {
            continue;
        }

        if (power < iter) {
            cout << "Loading " + entry.path().filename().string() << endl;

            vector<unsigned char> buffer;
            fstream file(entry.path(), fstream::in);

            while (!file.eof()) {
                char val;
                file.get(val);
                if (file.eof()) break;
                buffer.push_back(val);
            }
            file.close();

            import_bits(powers[power], buffer.begin(), buffer.end());

            if (powers[power] == 0) {
                continue;
            }
            loadedPowers.push_back(power); // Add read power to loadedPowers
        } else {
            numberToLarge = true;
        }
    }

    cout << "Finished Loading" << endl;
    if (numberToLarge) {
        cout << "Found file with power larger than set possible!" << endl << endl;
    }

    int largestPower = 0;
    sort(loadedPowers.begin(), loadedPowers.end());

    // Find largest power without "gaps" in files, so
    // 0, 1, 2, 3, 5, 6 will return 3
    // We will need all powers in stage two
    for (auto power: loadedPowers) {
        if (power == largestPower + 1){
            largestPower++;
        }
    }

    powers[0] = 9;
    largestPower = max(++largestPower, 1); // Set largestPower to bigger value: 1 or largestPower + 1

    return {powers, largestPower};
}

string prepareTime(chrono::steady_clock::time_point start, chrono::steady_clock::time_point end) {
    auto microseconds = chrono::duration_cast<chrono::microseconds>(end - start).count();
    auto milliseconds = chrono::duration_cast<chrono::milliseconds>(end - start).count();
    auto seconds = chrono::duration_cast<chrono::seconds>(end - start).count();
    auto hours = chrono::duration_cast<chrono::hours>(end - start).count();

    string time;

    if (hours > 0) {
        time += to_string(hours) + "h ";
    }
    if (seconds > 0) {
        time += to_string(seconds % 60) + "s ";
    }
    if (milliseconds > 0) {
        time += to_string(milliseconds % 1000) + "ms ";
    }
    if (microseconds > 0) {
        time += to_string(microseconds % 1000) + "μm";
    }

    if (time.empty()) {
        return "no time";
    } else {
        return time;
    }
}

int main() {
    auto [powers, currentPower] = loadResults();
    thread threads[iter];

    for (; currentPower < iter; ++currentPower) {
        auto startTime = chrono::steady_clock::now();

        powers[currentPower] = pow(powers[currentPower - 1], 2);

        auto endTime = chrono::steady_clock::now();
        cout << "Calculated 9^" << mp::pow(mp::cpp_int(2), currentPower) << ", in " << prepareTime(startTime, endTime) << ", step " << currentPower << endl; // For logging progress

        // thread() can't find rsquare prismight function by its own (there are two),
        // so I point it to right address manually
        auto functionAddress = static_cast<void(*)(const mp::cpp_int&, int)>(saveResult);
        threads[currentPower] = thread(functionAddress, powers[currentPower], currentPower);
    }

    // Stage two: multiply following elements from array:
    // int toMultiply[] =  {28, 26, 25, 24, 20, 18, 17, 16, 15, 12, 8, 6, 3, 0};
    int toMultiply[] = {3, 6, 8, 12, 15, 16, 17, 18, 20, 24, 25, 26, 28};
    // Based on:
    // https://www.wolframalpha.com/input/?i=9%5E9+%3D+2%5E28+%2B+2%5E26+%2B+2%5E25+%2B+2%5E24+%2B+2%5E20+%2B+2%5E18+%2B+2%5E17+%2B+2%5E16+%2B+2%5E15+%2B+2%5E12+%2B+2%5E8+%2B+2%5E6+%2B+2%5E3+%2B+2%5E0

    mp::cpp_int final = powers[0];
    int previous = 0;
    for (int i = 0; i < 13; i++) {
        cout << "Calculating: " << toMultiply[i] << endl;
        final *= powers[toMultiply[i]];

        cout << "Saving..." << endl;
        saveResult(final, "partial." + to_string(toMultiply[i]));
        remove(("partial." + to_string(previous)).c_str());
    }

    for (auto &th: threads) {
        if (th.joinable()) {
            th.join();
        }
    }

    fstream result("result.txt", fstream::out);
    result << final;
    result.close();
    return 0;
}


#include "DataSet.hpp"

#define STB_IMAGE_IMPLEMENTATION
#include "stb_image.h"
#include "Eigen/Geometry"

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

#ifdef PCL_AVAILABLE
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#endif

std::string strip_file_suffix(const std::string& s)
{
  std::string::size_type idx = s.rfind('.');

  return std::string(s.substr(0, idx));
}

std::string get_file_name(const std::string& s)
{
  char sep = '/';

#ifdef _WIN32
  sep = '\\';
#endif

  size_t i = s.rfind(sep, s.length());
  if (i != std::string::npos)
  {
    return(s.substr(i+1, s.length() - i));
  }

  return("");
}

std::vector<double>::const_iterator closest(std::vector<double> const& vec, double value)
{
  auto const it = std::lower_bound(vec.begin(), vec.end(), value);

  return it;
}

// Calculate 3d point from pixel position and distance from camera using camera intrinsics
void pixel_to_camera_coord(const int x, const int y, const int z, Eigen::Vector3d& camCoord)
{
  const double fx = 517.3; // Focal length
  const double fy = 516.5; // Focal length
  const double cx = 318.6; // optical centre
  const double cy = 255.3; // optical centre

  const double Z = static_cast<double>(z)/5000.0;
  const double X = (static_cast<double>(x) - cx)*Z/fx;
  const double Y = (static_cast<double>(y) - cy)*Z/fy;

  camCoord << X,Y,Z;
}

std::string trim_left(const std::string& str, const char* t = " \t\n\r\f\v")
{
  std::string res = str;
  res.erase(0, res.find_first_not_of(t));
  return res;
}

DataSet::DataSet(const std::string& folder) : next_file_idx(0), folder_path(folder), rgb_ref_file_name(folder+"rgb.txt")
{
  boost::filesystem::directory_iterator depth_it(folder + "depth/");

  const std::string groundtruth_ref_file_name(folder+"groundtruth.txt");
  const std::string camera_ref_file_name(folder+"CameraTrajectory.txt");

  camera_ref_file.open(groundtruth_ref_file_name.c_str());
  if (!camera_ref_file.is_open()) {
    std::cout << "Couldn't open groundtruth.txt - trying CameraTrajectory.txt" << std::endl;
    std::cout << camera_ref_file_name << std::endl;
    camera_ref_file.open(camera_ref_file_name.c_str());
  }
  if (!camera_ref_file.is_open()) {
    std::cerr << "Couldn't open groundtruth.txt" << std::endl;
    return;
  }

  for (auto& it : depth_it)
    depth_files.push_back(it.path().string());

  std::sort(depth_files.begin(), depth_files.end());

  rOffset = Eigen::Matrix4d::Identity();
  operational = true;

  Eigen::MatrixXd P, C;
  Eigen::Matrix4d t_camera;
  get_next_point_cloud(P, C, t_camera);

#ifdef PCL_AVAILABLE
  Eigen::Affine3f r = findPlaneRotation(P);
  rOffset = r.matrix().cast<double>();
#endif
}

DataSet::~DataSet()
{

}

// get point cloud from next depth image
bool DataSet::get_next_point_cloud(Eigen::MatrixXd& points, Eigen::MatrixXd &colors, Eigen::Matrix4d& t_camera)
{
  if (!operational)
    return false;

  // Use the next depth image to load corresponding camera pose & rgb image data
  std::string time_stamp = strip_file_suffix(get_file_name(depth_files[next_file_idx]));
  double time_stamp_d = std::stod(time_stamp);

  if (!get_next_camera(t_camera, time_stamp_d))
    return false;
  t_camera = rOffset * t_camera;

  std::string rgb_filename = get_next_rgb(time_stamp_d);
  if (rgb_filename == "")
    return false;

  int rgb_width, rgb_height, rgb_bpp;
  std::string rgb_path = folder_path + rgb_filename;
  unsigned char* rgb = stbi_load(rgb_path.c_str(), &rgb_width, &rgb_height, &rgb_bpp, 3);

  if (static_cast<unsigned int>(next_file_idx) < depth_files.size())
  {
    int width, height, bpp;
    unsigned short* png = stbi_load_16(depth_files[next_file_idx].c_str(), &width, &height, &bpp, 1);

    points.resize(width*height, 3);
    colors.resize(width*height, 3);

    int rowcntr = 0;
#pragma omp parallel for
    for (int y = 0; y < height; ++y)
    {
      for (int x = 0; x < width; ++x)
      {
        if (png[(x+y*width)*bpp] == 0)
          continue;

        //if (x % 32 != 0 || y % 32 != 0)
        //  continue;

        // Transform depth image into point cloud using camera intrinsics
        Eigen::Vector3d camCoord;
        pixel_to_camera_coord(x, y, png[(x+y*width)/bpp], camCoord);

        // Transform points into world coordinates frame using camera pose
        Eigen::Vector4d camera_point; camera_point << camCoord,1.0;
        camera_point = t_camera * camera_point;
        Eigen::RowVector4d world_point = camera_point.transpose();
        unsigned char* color = rgb + (x+y*rgb_width)*rgb_bpp;

#pragma omp critical
        {
#ifdef PCL_AVAILABLE
          points.row(rowcntr) << world_point[0], world_point[1], world_point[2];
#else
          points.row(rowcntr) << world_point[0], world_point[2], world_point[1];
#endif
          colors.row(rowcntr) << (float)color[0]/255, (float)color[1]/255, (float)color[2]/255;
          ++rowcntr;
        }
      }
    }

    points.conservativeResize(rowcntr, Eigen::NoChange);
    colors.conservativeResize(rowcntr, Eigen::NoChange);

    stbi_image_free(png);
    stbi_image_free(rgb);
    next_file_idx++;
  }

  return true;
}

struct CameraEntry
{
  double time, tx, ty, tz, qi, qj, qk, ql;

  CameraEntry() : time(0), tx(0), ty(0), tz(0), qi(0), qj(0), qk(0), ql(0) {};
};

// Get camera transformation closest to timestamp
bool DataSet::get_next_camera(Eigen::Matrix4d& cam, const double timestamp)
{
  if (!operational)
    return false;

  std::string line;
  bool abort = false;

  struct CameraEntry previous;
  previous.time = 0;

  camera_ref_file.seekg(0, std::ios::beg);

  while (!abort)
  {
    while (std::getline(camera_ref_file, line) && trim_left(line)[0] == '#');

    if (line.empty())
      return false;

    std::stringstream line_stream(line);

    struct CameraEntry current;

    line_stream >> current.time >> current.tx >> current.ty >> current.tz >> current.qi >> current.qj >> current.qk >> current.ql;
    if (current.time > timestamp && previous.time > 0) // Previous was closer
    {
      // std::cout << (previous.time - 1527117859) << " " << (timestamp - 1527117859) << " " << (current.time - 1527117859) << std::endl;
      const double ti = (timestamp - previous.time) / (current.time - previous.time);
      const Eigen::Quaterniond qa(previous.ql,previous.qi,previous.qj,previous.qk),
                               qb(current.ql,current.qi,current.qj,current.qk);
      const Eigen::Quaterniond q_interp = qa.slerp(ti, qb);

      // std::cout << (timestamp-1527117859) << " " << ti << " " << q_interp.x() << " " << q_interp.y() << " " << q_interp.z() << " " << q_interp.w() << std::endl;

      const Eigen::Vector3d ta(previous.tx, previous.ty, previous.tz),
                            tb(current.tx, current.ty, current.tz);
      const Eigen::Translation3d t_interp = Eigen::Translation3d(ta + ti * (tb - ta));

      const Eigen::Affine3d t(t_interp);
      const Eigen::Affine3d r(q_interp);

      cam << t.matrix() * r.matrix();
      abort = true;
    }else{
      previous = current;
    }
  }

  return true;
}

// get rgb file name closest to a certain timestamp
std::string DataSet::get_next_rgb(const double timestamp)
{
  std::fstream rgb_ref_file(rgb_ref_file_name);
  double previousDT = std::numeric_limits<double>::max();
  std::string previous_filename;

  std::string line;
  while (std::getline(rgb_ref_file >> std::ws, line) && !line.empty())
  {
    if (trim_left(line)[0] == '#')
      continue;

    std::stringstream ss(line);

    double t;
    std::string filename;
    ss >> t >> filename;

    double currentDT = std::abs(t - timestamp);
    if (currentDT > previousDT) // Previous was closer
    {
      return previous_filename;
    }
    previous_filename = filename;
    previousDT = currentDT;
  }

  return "";
}

#ifdef PCL_AVAILABLE
// Find the plane rotation using RANSAC
Eigen::Affine3f DataSet::findPlaneRotation(Eigen::MatrixXd& points) {

  const long int size = points.rows();

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  cloud->width = size;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

#pragma omp parallel for
  for (int i = 0; i < size; ++i)
  {
    cloud->points[i].x = (float)points(i,0);
    cloud->points[i].y = (float)points(i,1);
    cloud->points[i].z = (float)points(i,2);
  }

  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.01);

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
    return Eigen::Affine3f::Identity();
  }

  Eigen::Matrix<float, 1, 3> floor_plane_normal_vector, xy_plane_normal_vector;

  floor_plane_normal_vector[0] = coefficients->values[0];
  floor_plane_normal_vector[1] = coefficients->values[1];
  floor_plane_normal_vector[2] = coefficients->values[2];

  if (floor_plane_normal_vector[1] > 0) floor_plane_normal_vector = -floor_plane_normal_vector;

  xy_plane_normal_vector[0] = 0.0;
  xy_plane_normal_vector[1] = 1.0;
  xy_plane_normal_vector[2] = 0.0;

  Eigen::Vector3f mu (0.0, 0.0, 0.0);

  // Get all inlier points to calculate mean position of plane
  for (uint i=0;i < inliers->indices.size();i++)
  {
    pcl::PointXYZ pt = cloud->points[inliers->indices[i]];

    mu[0] += pt.x;
    mu[1] += pt.y;
    mu[2] += pt.z;
  }

  mu /= (float)inliers->indices.size ();

  // Rotation axis is orthogonal to plane normal and target vector
  Eigen::Vector3f rotation_vector = xy_plane_normal_vector.cross(floor_plane_normal_vector);
  rotation_vector.normalize();

  // calculate rotation around axis
  float theta = -atan2f(rotation_vector.norm(), xy_plane_normal_vector.dot(floor_plane_normal_vector));

  // Create translation & rotation transformation
  Eigen::Affine3f transform_1(Eigen::Translation3f(-mu));
  Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
  transform_2.rotate(Eigen::AngleAxisf (theta, rotation_vector));

  return transform_2 * transform_1;
}
#endif


//
// Created by James Noeckel on 1/21/20.
//

#include "region_growing.h"
#include <queue>
#include <Eigen/Dense>
#include "math/GaussianMixture.h"
#include <iostream>

#define TRAINING_SIZE 100

typedef std::pair<float, std::pair<int, int>> WeightedPosLabel;

void region_growing(const cv::Mat &image, cv::Mat &seed, float gmm_weight) {
    if (image.empty() || seed.empty() || image.rows != seed.rows || image.cols != seed.cols) {
        return;
    }
    std::priority_queue<WeightedPosLabel, std::vector<WeightedPosLabel>, std::greater<WeightedPosLabel>> pq;
    //cv::Mat costs = cv::Mat::ones(seed.rows, seed.cols, CV_32FC1) * std::numeric_limits<float>::max();
    std::vector<GaussianMixture> gmms;
    std::unordered_map<int, size_t> cluster_indices;
    if (gmm_weight > 0) {
        //initialize GMMs for each cluster
        size_t index = 0;
        for (int i = 0; i < seed.rows; i++) {
            for (int j = 0; j < seed.cols; j++) {
                int cluster = seed.at<int>(i, j);
                auto it = cluster_indices.find(cluster);
                if (it == cluster_indices.end()) {
                    cluster_indices[cluster] = index;
                    index++;
                }
            }
        }
        std::vector<std::vector<Eigen::RowVector3i>> colors(index);
        for (int i = 0; i < seed.rows; i++) {
            for (int j = 0; j < seed.cols; j++) {
                int cluster = seed.at<int>(i, j);
                cv::Vec3b col = image.at<cv::Vec3b>(i, j);
                colors[cluster_indices[cluster]].emplace_back(col[0], col[1], col[2]);
            }
        }
        std::cout << "learning " << index << " gmms" << std::endl;

        gmms.reserve(index);
        for (int i = 0; i < index; i++) {
            gmms.emplace_back(3, 3);
            int n = std::min(TRAINING_SIZE, static_cast<int>(colors[i].size()));
            Eigen::MatrixX3d data(n, 3);
            int full_n = colors[i].size();
            Eigen::VectorXi picks = Eigen::VectorXi::Random(n);
            picks = picks.unaryExpr([&](const int num) { return abs(num) % full_n; });
            for (int j = 0; j < n; j++) {
                data.row(j) = colors[i][picks[j]].cast<double>();
            }
            //TODO: initialize with color range in mind, to prevent close centers
            gmms[i].initialize(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(255, 255, 255));
            int iters = gmms[i].learn(data);
            if (!gmms[i].success()) {
                std::cout << "warning: gmm " << i << " failed to learn from " << n << "points" << std::endl;
            }
        }
    }

    std::cout << "initializing" << std::endl;
    //initialize
    for (int i=0; i<seed.rows; i++) {
        for (int j=0; j<seed.cols; j++) {
            int pos = j + i * seed.cols;
            if (seed.at<int>(i, j) < 0) {
                for (int dim=0; dim<=1; dim++) {
                    for (int dx=-1; dx<=1; dx+=2) {
                        Eigen::Array2i offset(0, 0);
                        offset[dim] += dx;
                        int pi = i + offset.y();
                        int pj = j + offset.x();
                        if (pi < 0 || pj < 0 || pi >= seed.rows || pj >= seed.cols) continue;
                        int neighbor = seed.at<int>(pi, pj);
                        if (neighbor >= 0) {
                            cv::Vec3b col0 = image.at<cv::Vec3b>(i, j);
                            cv::Vec3b col1 = image.at<cv::Vec3b>(pi, pj);
                            cv::Vec<float, 3> col0f(col0[0], col0[1], col0[2]);
                            cv::Vec<float, 3> col1f(col1[0], col1[1], col1[2]);
                            cv::Vec<float, 3> diff = col1f - col0f;
                            float cost = diff.dot(diff);
                            Eigen::RowVector3d col_vec(col0[0], col0[1], col0[2]);
                            if (gmm_weight > 0)
                                cost -= gmm_weight * static_cast<float>(gmms[cluster_indices[neighbor]].getLogLikelihood(col_vec)(0));
                            //if (cost < costs.at<float>(i, j)) {
                            //    costs.at<float>(i, j) = cost;
                                pq.push(std::make_pair(cost, std::make_pair(pos, neighbor)));
                            //}
                        }
                    }
                }
            }
        }
    }
    std::cout << "segmenting" << std::endl;
    //region growing
    while (!pq.empty()) {
        std::pair<int, int> pos_label = pq.top().second;
        pq.pop();
        int i = pos_label.first / seed.cols;
        int j = pos_label.first % seed.cols;
        int curr_label = seed.at<int>(i, j);
        if (curr_label < 0) {
            seed.at<int>(i, j) = pos_label.second;
            cv::Vec3b col0 = image.at<cv::Vec3b>(i, j);
            cv::Vec<float, 3> col0f(col0[0], col0[1], col0[2]);
            for (int dim=0; dim<=1; dim++) {
                for (int dx=-1; dx<=1; dx+=2) {
                    Eigen::Array2i offset(0, 0);
                    offset[dim] += dx;
                    int pi = i + offset.y();
                    int pj = j + offset.x();
                    if (pi < 0 || pj < 0 || pi >= seed.rows || pj >= seed.cols) continue;
                    int neighbor = seed.at<int>(pi, pj);
                    if (neighbor < 0) {
                        cv::Vec3b col1 = image.at<cv::Vec3b>(pi, pj);
                        cv::Vec<float, 3> col1f(col1[0], col1[1], col1[2]);
                        cv::Vec<float, 3> diff = col1f - col0f;
                        Eigen::RowVector3d col_vec(col1[0], col1[1], col1[2]);
                        float cost = diff.dot(diff);
                        if (gmm_weight > 0)
                            cost -= gmm_weight * static_cast<float>(gmms[cluster_indices[pos_label.second]].getLogLikelihood(col_vec)(0));
                        //if (cost < costs.at<float>(pi, pj)) {
                        //    costs.at<float>(pi, pj) = cost;
                            int newpos = pj + pi * seed.cols;
                            pq.push(std::make_pair(cost, std::make_pair(newpos, pos_label.second)));
                        //}
                    }
                }
            }
        }
    }
}


// Copyright 2015-2020 The ALMA Project Developers
//
// 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.

/// @file
/// Test the beyondRTA module by computing the thermal
/// conductivity tensor of wurtzite GaN under the full BTE.

#include <iostream>
#include <functional>
#include <boost/filesystem.hpp>
#include <boost/mpi.hpp>
#include "gtest/gtest.h"
#include <utilities.hpp>
#include <vasp_io.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <beyondRTA.hpp>
#include <shengbte_iter.hpp>

TEST(beyondRTA_case, beyondRTA_test) {
    boost::mpi::environment env;
    boost::mpi::communicator world;

    // load information
    auto basedir = boost::filesystem::path(TEST_RESOURCE_DIR);
    auto data_dir = basedir / boost::filesystem::path("GaN_wurtzite");
    auto poscar_path = data_dir / boost::filesystem::path("POSCAR");
    auto ifc_path = data_dir / boost::filesystem::path("FORCE_CONSTANTS");
    auto born_path = data_dir / boost::filesystem::path("BORN");
    auto thirdorder_path =
        data_dir / boost::filesystem::path("FORCE_CONSTANTS_3RD");

    auto poscar = alma::load_POSCAR(poscar_path.string().c_str());
    auto syms = alma::Symmetry_operations(*poscar);
    auto force_constants =
        alma::load_FORCE_CONSTANTS(ifc_path.string().c_str(), *poscar, 5, 5, 5);
    auto born = alma::load_BORN(born_path.string().c_str());
    auto thirdorder = alma::load_FORCE_CONSTANTS_3RD(
        thirdorder_path.string().c_str(), *poscar);

    auto grid = std::make_unique<alma::Gamma_grid>(
        *poscar, syms, *force_constants, *born, 8, 8, 8);
    grid->enforce_asr();

    double Tref = 300.0;

    // determine phonon processes and precompute matrix elements
    auto three_ph_procs = alma::find_allowed_threeph(*grid, world, 0.1);
    auto two_ph_procs = alma::find_allowed_twoph(*grid, world);

    for (std::size_t i = 0; i < three_ph_procs.size(); ++i) {
        three_ph_procs[i].compute_vp2(*poscar, *grid, *thirdorder);
    }

    // compute scattering rates
    Eigen::ArrayXXd w3 =
        alma::calc_w0_threeph(*grid, three_ph_procs, Tref, world);
    Eigen::ArrayXXd w2 =
        alma::calc_w0_twoph(*poscar, *grid, two_ph_procs, world);
    Eigen::ArrayXXd w0(w3 + w2);

    // target results
    double kapparef_xx = 178.7125556;
    double kapparef_yy = 178.7125556;
    double kapparef_zz = 190.0217278;

    // test beyondRTA conductivity from iterative Eigen solver
    Eigen::Matrix3d kappa1 = alma::beyondRTA::calc_kappa(*poscar,
                                                         *grid,
                                                         syms,
                                                         three_ph_procs,
                                                         two_ph_procs,
                                                         w0,
                                                         Tref,
                                                         true,
                                                         world);

    EXPECT_TRUE(alma::almost_equal(kapparef_xx, kappa1(0, 0), 1e-8, 1e-3));
    EXPECT_TRUE(alma::almost_equal(kapparef_yy, kappa1(1, 1), 1e-8, 1e-3));
    EXPECT_TRUE(alma::almost_equal(kapparef_zz, kappa1(2, 2), 1e-8, 1e-3));
    kappa1(0, 0) = kappa1(1, 1) = kappa1(2, 2) = 0.0;
    EXPECT_TRUE(alma::almost_equal(kappa1.norm(), 0., 1e-13, 0.));

    // test beyondRTA conductivity from LU Eigen solver
    Eigen::Matrix3d kappa2 = alma::beyondRTA::calc_kappa(*poscar,
                                                         *grid,
                                                         syms,
                                                         three_ph_procs,
                                                         two_ph_procs,
                                                         w0,
                                                         Tref,
                                                         false,
                                                         world);

    EXPECT_TRUE(alma::almost_equal(kapparef_xx, kappa2(0, 0), 1e-8, 1e-3));
    EXPECT_TRUE(alma::almost_equal(kapparef_yy, kappa2(1, 1), 1e-8, 1e-3));
    EXPECT_TRUE(alma::almost_equal(kapparef_zz, kappa2(2, 2), 1e-8, 1e-3));
    kappa2(0, 0) = kappa2(1, 1) = kappa2(2, 2) = 0.0;
    EXPECT_TRUE(alma::almost_equal(kappa2.norm(), 0., 1e-13, 0.));

    // test the ShengBTE-like iterator
    double kapparef_xx_sheng = 178.7104296;
    double kapparef_yy_sheng = 178.6955812;
    double kapparef_zz_sheng = 203.9714615;
    Eigen::Matrix3d kappa3 = alma::calc_shengbte_kappa(
        *poscar, *grid, syms, three_ph_procs, two_ph_procs, Tref, world);

    EXPECT_TRUE(
        alma::almost_equal(kapparef_xx_sheng, kappa3(0, 0), 1e-8, 1e-3));
    EXPECT_TRUE(
        alma::almost_equal(kapparef_yy_sheng, kappa3(1, 1), 1e-8, 1e-3));
    EXPECT_TRUE(
        alma::almost_equal(kapparef_zz_sheng, kappa3(2, 2), 1e-8, 1e-3));
}


#ifndef DIIS_HH
#define DIIS_HH

/*!
 * \file DIIS.hh
 * \brief Definition of the DIIS class
 */

#include <Eigen/Core>

/*!
 * \brief The DIIS convergence optimizer
 *
 * Class DIIS implements the Direct Inversion in the Iterative Subspace
 * extrapolation technique to speed up the convergence in an SCF calculation.
 * It tries to make a better guess for the Fock matrix in the next iteration
 * by creating a linear combination of Fock matrices from previous iterations,
 * where the error term \f$FDS-SDF\f$ (with \f$F\f$ the Fock matrix, \f$D\f$
 * the density matrix, and \f$S\f$ the overlap matrix) is minimized.
 */
class DIIS
{
public:
	/*!
	 * \brief Constructor
	 *
	 * Create a new DIIS object, for a basis of size \a size.
	 * \param size The size of the matrices (basis set size).
	 */
	DIIS(): _size(0), _err_vecs_used(0), _err_vecs(), _values(),
		_started(false), _max_err(0) {}

	/*!
	 * \brief Create a new Fock matrix
	 *
	 * Create the new Fock matrix from the current guess for the matrix and
	 * the current density matrix. If the DIIS algorithm has not been
	 * started yet (because the error term is still too large with respect
	 * to the computed energy), it simply returns \a F. Otherwise a linear
	 * combination of previous Fock matrices \f$\tilde{F}\f$ that minimizes
	 * the error term \f$\tilde{F}DS-SD\tilde{F}\f$ is computed, and stored
	 * in \a F.
	 * \param F    The current Fock matrix
	 * \param P    The density matrix from which \a F was computed
	 * \param S    The overlap matrix for the basis
	 * \param X    The orthogonalization matrix \f$X = S^{-1/2}\f$ for the basis
	 * \param Etot The total energy
	 */
	void step(Eigen::MatrixXd& F, const Eigen::MatrixXd& D,
		const Eigen::MatrixXd& S, const Eigen::MatrixXd& X, double Etot);
	void step(Eigen::MatrixXd& Fa, const Eigen::MatrixXd& Da,
		Eigen::MatrixXd& Fb, const Eigen::MatrixXd& Db,
		const Eigen::MatrixXd& S, const Eigen::MatrixXd& X, double Etot);

	//! Return the maximum error on the last iteration
	double error() const { return _max_err; }

private:
	//! The basis set size
	int _size;
	//! The number of error vectors currently stored
	int _err_vecs_used;
	//! The error vectors themselves, one per column
	Eigen::MatrixXd _err_vecs;
	//! The previous Fock matrices
	std::vector<Eigen::MatrixXd> _values;
	//! Whether DIIS was started
	bool _started;
	//! Maximum error in the last DIIS step
	double _max_err;
};

#endif // DIIS_HH


#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>

#include "base/eigen2hdf.hpp"
#include "fft/fft2.hpp"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/construction/translation_grid.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

using namespace std;

TEST(base, rca)
{
  // for (int j = 1; j < 9; ++j) {

  //   RidgeletFrame rf(j, j, 1, 1);

  //   typedef RidgeletCellArray<> rca_t;

  //   rca_t rca(rf);
  //   auto& vcoeffs = rca.coeffs();

  //   auto& lambdas = rf.lambdas();

  //   for (int i = 0; i < vcoeffs.size(); ++i) {
  //     int rows = vcoeffs[i].rows();
  //     int cols = vcoeffs[i].cols();
  //     auto t = tgrid_dim(lambdas[i], rf);
  //     int tx = std::get<0>(t);
  //     int ty = std::get<1>(t);
  //     EXPECT_EQ(rows, tx) << j << lambdas[i];
  //     EXPECT_EQ(cols, ty) << j << lambdas[i];
  //   }
  // }
}