/**
 * forestmodevis: main.cpp
 * Copyright (c) Torr Vision Group, University of Oxford, 2016. All rights reserved.
 */

#include <Eigen/Geometry>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/viz.hpp>
#include <opencv2/viz/widget_accessor.hpp>
#include <sstream>
#include <vtkActor.h>
#include <vtkPolyData.h>
#include <vtkPolyDataMapper.h>
#include <vtkProperty.h>
#include <vtkSmartPointer.h>
#include <vtkSphereSource.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>

#include "tvgutil/filesystem/PathFinder.h"
#include "tvgutil/filesystem/SequentialPathGenerator.h"
#include "tvgutil/timing/TimeUtil.h"

namespace fs = boost::filesystem;
using namespace cv::viz;
using namespace tvgutil;

static const int NTREES = 5;

struct Mode
{
  cv::Vec3f position;
  cv::Matx33f covariance;
};

void readModes(const std::string &modesFile, std::vector<std::vector<Mode>> &modes)
{
  modes.clear();
  modes.resize(NTREES);

  std::ifstream inModes(modesFile);
  for(int treeIdx = 0; treeIdx < NTREES; ++treeIdx)
  {
    int nbModes, leafIdx;
    inModes >> nbModes >> leafIdx;
    for(int modeIdx = 0; modeIdx < nbModes; ++modeIdx)
    {
      int nbInliers;
      Mode mode;
      inModes >> nbInliers;
      for(int i = 0; i < 3; ++i) inModes >> mode.position.val[i];
      for(int i = 0; i < 9; ++i) inModes >> mode.covariance.val[i];

      modes[treeIdx].push_back(mode);
    }
  }
}

void readExamples(const std::string &examplesFile, std::vector<std::vector<cv::Point3d>> &examples)
{
  examples.clear();
  examples.resize(NTREES);

  std::ifstream inExamples(examplesFile);
  for(int treeIdx = 0; treeIdx < NTREES; ++treeIdx)
  {
    int nbExamples;
    inExamples >> nbExamples;
//    std::cout << examplesFile << " - Nb examples for tree " << treeIdx << ": " << nbExamples << '\n';

    for(int exampleIdx = 0; exampleIdx < nbExamples; ++exampleIdx)
    {
      cv::Point3d example;

      inExamples >> example.x >> example.y >> example.z;

//      std::cout << example << '\n';

      examples[treeIdx].push_back(example);
    }
  }
}

class WCovarianceEllipsoid : public Widget3D
{
public:
  WCovarianceEllipsoid(const cv::Vec3f &position, const cv::Matx33f &covariance, Color color = Color::white())
  {
    // Compute the transformation parameters
    cv::Mat eVals;
    cv::Mat eVecs;
    cv::eigen(covariance, eVals, eVecs);
    cv::sqrt(eVals, eVals);

    auto scalingTransform =
        Eigen::Scaling(std::sqrt(covariance.val[0]) * 2.0f, std::sqrt(covariance.val[4]) * 2.0f, std::sqrt(covariance.val[8]) * 2.0f);
    //    auto scalingTransform = Eigen::Scaling(
    //        Eigen::Map<Eigen::Vector3f>(eVals.ptr<float>()));
    auto rotation = Eigen::AngleAxisf(Eigen::Map<Eigen::Matrix3f>(eVecs.ptr<float>()));
    auto translation = Eigen::Translation3f(Eigen::Map<const Eigen::Vector3f>(position.val));

    Eigen::Affine3f affineTransform = translation * scalingTransform * rotation;

    // row major
    Eigen::Matrix4d transformMat = affineTransform.matrix().transpose().cast<double>();

    vtkSmartPointer<vtkSphereSource> sphereSource = vtkSmartPointer<vtkSphereSource>::New();
    sphereSource->SetCenter(0.0, 0.0, 0.0);
    sphereSource->SetPhiResolution(180);
    sphereSource->SetThetaResolution(360);
    sphereSource->SetRadius(1.0);

    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
    transform->SetMatrix(transformMat.data());

    vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
    transformFilter->SetInputConnection(sphereSource->GetOutputPort());
    transformFilter->SetTransform(transform);
    transformFilter->Update();
    // Set up the actor to display the transformed polydata

    vtkSmartPointer<vtkPolyDataMapper> transformedMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
    transformedMapper->SetInputConnection(transformFilter->GetOutputPort());

    vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
    actor->SetMapper(transformedMapper);
    actor->GetProperty()->SetColor(color.val[2] / 255, color.val[1] / 255, color.val[0] / 255);

    WidgetAccessor::setProp(*this, actor);
  }
};

struct VisualizationCookie
{
  Viz3d *visualizer;
  std::vector<Color> treeColours;
  std::vector<std::vector<Mode>> modesByTree;
  SequentialPathGenerator *pathGenerator;
  WMesh *mesh;
  std::string baseName;
  std::vector<std::string> widgetNames;
  std::string animationModesBaseName;
  std::string animationExamplesBaseName;
};

std::vector<std::string>
    drawModes(const std::vector<Mode> &modes, const std::string &nameBase, const Color &color, Viz3d &visualizer)
{
  std::vector<std::string> widgetNames;

  for(size_t modeIdx = 0; modeIdx < modes.size(); ++modeIdx)
  {
    const Mode &mode = modes[modeIdx];
    const std::string modeName = nameBase + '_' + boost::lexical_cast<std::string>(modeIdx);
    widgetNames.push_back(modeName);

    WCovarianceEllipsoid ellipsoid(mode.position, mode.covariance, color);
    visualizer.showWidget(modeName, ellipsoid);
  }

  return widgetNames;
}

std::vector<std::string> drawExamples(const std::vector<cv::Point3d> &examples,
                                      const std::string &nameBase,
                                      const Color &color,
                                      Viz3d &visualizer)
{
  std::vector<std::string> widgetNames;

  for(size_t exampleIdx = 0; exampleIdx < examples.size(); ++exampleIdx)
  {
    const cv::Point3d &example = examples[exampleIdx];

    const std::string exampleName = nameBase + '_' + boost::lexical_cast<std::string>(exampleIdx);
    widgetNames.push_back(exampleName);

    WSphere sphere(example, 0.01, 10, color);
    visualizer.showWidget(exampleName, sphere);
  }

  return widgetNames;
}

static void vizCallbackSingleModes(const KeyboardEvent &event, void *c)
{
  const VisualizationCookie *cookie = static_cast<VisualizationCookie *>(c);

  if(event.action == KeyboardEvent::KEY_UP && event.code == 't')
  {
    std::cout << "Saving screenshots from current viewpoint.\n";

    Viz3d &visualizer = *cookie->visualizer;

    // Clean the visualizer
    for(const auto &x : cookie->widgetNames)
    {
      visualizer.removeWidget(x);
    }

    for(size_t treeIdx = 2; treeIdx < cookie->modesByTree.size(); ++treeIdx)
    {
      std::vector<std::string> modeNames =
          drawModes(cookie->modesByTree[treeIdx], "wModes_", cookie->treeColours[treeIdx], visualizer);

      visualizer.saveScreenshot(cookie->baseName + "_" + boost::lexical_cast<std::string>(treeIdx) + ".png");

      for(const auto &x : modeNames)
      {
        visualizer.removeWidget(x);
      }
    }
  }
}

static void vizCallbackAnimation(const KeyboardEvent &event, void *c)
{
  const VisualizationCookie *cookie = static_cast<VisualizationCookie *>(c);

  if(event.action == KeyboardEvent::KEY_UP && event.code == 't')
  {
    std::cout << "Saving screenshots from current viewpoint.\n";

    Viz3d &visualizer = *cookie->visualizer;

    // Clean the visualizer
    for(const auto &x : cookie->widgetNames)
    {
      visualizer.removeWidget(x);
    }

    fs::path currentModeFileName = cookie->pathGenerator->make_path(cookie->animationModesBaseName);
    fs::path currentExamplesFileName = cookie->pathGenerator->make_path(cookie->animationExamplesBaseName);

    while(fs::is_regular_file(currentModeFileName))
    {
      std::vector<std::vector<Mode>> currentModes;
      readModes(currentModeFileName.string(), currentModes);

      std::vector<std::vector<cv::Point3d>> currentExamples;
      readExamples(currentExamplesFileName.string(), currentExamples);

      for(size_t treeIdx = 2; treeIdx < currentModes.size(); ++treeIdx)
      {
//        // Draw modes
//        {
//          std::vector<std::string> modeNames =
//              drawModes(currentModes[treeIdx], "wModes_", cookie->treeColours[treeIdx], visualizer);

//          fs::path screenshotPath = cookie->pathGenerator->make_path(
//              "modes_" + boost::lexical_cast<std::string>(treeIdx) + '_' + cookie->animationModesBaseName + ".png");
//          std::cout << "Saving screenshot: " << screenshotPath << '\n';

//          visualizer.saveScreenshot(screenshotPath.string());

//          for(const auto &x : modeNames)
//          {
//            visualizer.removeWidget(x);
//          }
//        }

//        // Draw examples
//        {
//          std::vector<std::string> exampleNames = drawExamples(currentExamples[treeIdx], "wExamples_", cookie->treeColours[treeIdx], visualizer);

//          fs::path screenshotPath = cookie->pathGenerator->make_path(
//              "examples_" + boost::lexical_cast<std::string>(treeIdx) + '_' + cookie->animationModesBaseName + ".png");
//          std::cout << "Saving screenshot: " << screenshotPath << '\n';

//          visualizer.saveScreenshot(screenshotPath.string());

//          for(const auto &x : exampleNames)
//          {
//            visualizer.removeWidget(x);
//          }
//        }

        // Draw combined
        {
          std::vector<std::string> exampleNames = drawExamples(currentExamples[treeIdx], "wExamples_", cookie->treeColours[treeIdx], visualizer);
          std::vector<std::string> modeNames = drawModes(currentModes[treeIdx], "wModes_", Color::gold(), visualizer);

          fs::path screenshotPath = cookie->pathGenerator->make_path(
              "combined_" + boost::lexical_cast<std::string>(treeIdx) + '_' + cookie->animationModesBaseName + ".png");
          std::cout << "Saving screenshot: " << screenshotPath << '\n';

          visualizer.saveScreenshot(screenshotPath.string());

          for(const auto &x : exampleNames)
          {
            visualizer.removeWidget(x);
          }

          for(const auto &x : modeNames)
          {
            visualizer.removeWidget(x);
          }
        }

      }

      cookie->pathGenerator->increment_index();
      currentModeFileName = cookie->pathGenerator->make_path(cookie->animationModesBaseName);
      currentExamplesFileName = cookie->pathGenerator->make_path(cookie->animationExamplesBaseName);
    }
  }
}

int main(int argc, char *argv[])
{
  if(argc < 3)
  {
    std::cout << "Usage: " << argv[0] << " mesh.obj modes.txt\n";
    return 1;
  }

  const std::string meshFile = argv[1];
  const std::string modesFile = argv[2];

  VisualizationCookie cookie;

  if(argc > 3)
  {
    cookie.animationModesBaseName = argv[3];
    cookie.animationExamplesBaseName = argv[4];
  }

  // Load mesh
  Mesh mesh = Mesh::load(meshFile, Mesh::LOAD_OBJ);
  WMesh wMesh(mesh);

  // Load modes
  // To visualize Kabsch modes
  // static const int NTREES = 3;

  readModes(modesFile, cookie.modesByTree);

  // Show everything
  Viz3d visualizer("Modes Visualizer");
  visualizer.showWidget("wMesh", wMesh);

  //  std::vector<Color> treeColors { Color::red(), Color::green(), Color::blue(), Color::magenta(), Color::cyan() };
  cookie.treeColours.push_back(Color::blue());
  cookie.treeColours.push_back(Color::green());
  cookie.treeColours.push_back(Color::red());
  cookie.treeColours.push_back(Color::magenta());
  cookie.treeColours.push_back(Color::cyan());

  // Here we draw all modes for visualization purpose.
  static const int INITIAL_TREE = 2;
  for(int treeIdx = INITIAL_TREE; treeIdx < NTREES; ++treeIdx)
  {
    Color color = cookie.treeColours[treeIdx];
    for(size_t modeIdx = 0; modeIdx < cookie.modesByTree[treeIdx].size(); ++modeIdx)
    {
      const Mode &mode = cookie.modesByTree[treeIdx][modeIdx];
      const std::string modeName =
          "wMode_" + boost::lexical_cast<std::string>(treeIdx) + '_' + boost::lexical_cast<std::string>(modeIdx);
      cookie.widgetNames.push_back(modeName);

      WCovarianceEllipsoid ellipsoid(mode.position, mode.covariance, color);
      visualizer.showWidget(modeName, ellipsoid);
    }
  }

  SequentialPathGenerator spg("./clusters");

  cookie.visualizer = &visualizer;
  cookie.pathGenerator = &spg;
  cookie.mesh = &wMesh;
  cookie.baseName = fs::path(modesFile).stem().string();

//  visualizer.registerKeyboardCallback(vizCallbackSingleModes, &cookie);
  visualizer.registerKeyboardCallback(vizCallbackAnimation, &cookie);

  //  WCovarianceEllipsoid test(cv::Vec3f(), cv::Matx33f(), Color::bluberry());
  //  visualizer.showWidget("test", test);

  visualizer.spin();

  return 0;
}


// An interval object.
#pragma once

#include <string>

#include <Eigen/Core>
#include <boost/numeric/interval.hpp>

namespace intervalccd {

namespace interval_options {
    typedef boost::numeric::interval_lib::checking_base<double> CheckingPolicy;
} // namespace interval_options

typedef boost::numeric::interval<
    double,
    boost::numeric::interval_lib::policies<
        boost::numeric::interval_lib::save_state<
            boost::numeric::interval_lib::rounded_arith_std<double>>,
        interval_options::CheckingPolicy>>
    Interval;

} // namespace intervalccd

namespace Eigen {
typedef Matrix<intervalccd::Interval, 2, 1> Vector2I;
typedef Matrix<intervalccd::Interval, 3, 1> Vector3I;
typedef Matrix<intervalccd::Interval, Dynamic, 1> VectorXI;
typedef Matrix<intervalccd::Interval, Dynamic, 1, ColMajor, 3, 1> VectorX3I;
typedef Matrix<double, 3, 1> Vector3d;
typedef Matrix<double, Dynamic, 1, ColMajor, 3, 1> VectorX3d;

template <typename T, int dim, int max_dim = dim>
using Vector = Matrix<T, dim, 1, ColMajor, max_dim, 1>;

} // namespace Eigen


/**
 * @file polynomial_smoother.cpp
 * @brief This defines a polynomial smoother update filter
 *
 * This smooths the noisy update.
 *
 * @author Jorge Nicho
 * @date June 3, 2016
 * @version TODO
 * @bug No known bugs
 *
 * @copyright Copyright (c) 2016, Southwest Research Institute
 *
 * @par License
 * Software License Agreement (Apache License)
 * @par
 * 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
 * @par
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <ros/console.h>
#include <Eigen/Geometry>
#include <eigen_conversions/eigen_msg.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>
#include <pluginlib/class_list_macros.h>
#include <stomp_moveit/update_filters/polynomial_smoother.h>
#include <stomp_moveit/utils/polynomial.h>
#include <XmlRpcException.h>

PLUGINLIB_EXPORT_CLASS(stomp_moveit::update_filters::PolynomialSmoother,stomp_moveit::update_filters::StompUpdateFilter)

namespace stomp_moveit
{
namespace update_filters
{

const double JOINT_LIMIT_MARGIN = 0.00001;

PolynomialSmoother::PolynomialSmoother():
    name_("ExponentialSmoother")
{
  // TODO Auto-generated constructor stub

}

PolynomialSmoother::~PolynomialSmoother()
{
  // TODO Auto-generated destructor stub
}

bool PolynomialSmoother::initialize(moveit::core::RobotModelConstPtr robot_model_ptr,
                        const std::string& group_name,const XmlRpc::XmlRpcValue& config)
{
  group_name_ = group_name;
  robot_model_ = robot_model_ptr;

  return configure(config);
}

bool PolynomialSmoother::configure(const XmlRpc::XmlRpcValue& config)
{
  using namespace XmlRpc;

  try
  {
    XmlRpcValue params = config;
    poly_order_ = static_cast<int>(params["poly_order"]);
  }
  catch(XmlRpc::XmlRpcException& e)
  {
    ROS_ERROR("%s failed to load parameters, %s",getName().c_str(),e.getMessage().c_str());
    return false;
  }

  return true;
}

bool PolynomialSmoother::setMotionPlanRequest(const planning_scene::PlanningSceneConstPtr& planning_scene,
                 const moveit_msgs::MotionPlanRequest &req,
                 const stomp::StompConfiguration &config,
                 moveit_msgs::MoveItErrorCodes& error_code)
{
  error_code.val = error_code.SUCCESS;
  return true;
}

bool PolynomialSmoother::filter(std::size_t start_timestep,
                    std::size_t num_timesteps,
                    int iteration_number,
                    const Eigen::MatrixXd& parameters,
                    Eigen::MatrixXd& updates,
                    bool& filtered)
{
  using namespace Eigen;
  using namespace moveit::core;
  using namespace utils::polynomial;

  filtered = false;
  Eigen::MatrixXd parameters_updates = parameters + updates;
  if(applyPolynomialSmoothing(robot_model_,group_name_,parameters_updates,poly_order_,JOINT_LIMIT_MARGIN))
  {
    updates = parameters_updates - parameters;
    filtered = true;
  }
  else
  {
    ROS_ERROR("Unable to polynomial smooth trajectory!");
    return false;
  }

  return true;
}

} /* namespace update_filters */
} /* namespace stomp_moveit */


#include "conv2d.h"
#include <iostream>
#include <Eigen/Core>

#include <chrono>  // for high_resolution_clock

AsynSparseConvolution2D::AsynSparseConvolution2D(int dimension, int nIn, int nOut, int filter_size, bool first_layer, bool use_bias, bool debug)
: dimension_(dimension), nIn_(nIn), nOut_(nOut), filter_size_(filter_size), first_layer_(first_layer), use_bias_(use_bias),
  filter_size_tensor_(dimension), filter_volume_(std::pow(filter_size, dimension)), bias_(nOut), padding_(2*dimension),
  kernel_indices_(filter_volume_, dimension_), weights_(filter_volume_, nIn*nOut), initialized_output_maps_(false), debug_(debug),
  initialized_input_maps_(false)
{

    filter_size_tensor_ = Eigen::VectorXi::Constant(dimension, filter_size_);
    padding_ = Eigen::VectorXi::Constant(dimension * 2, filter_size_/2);

    for (int i=0; i<filter_volume_; i++)
    {
        kernel_indices_(i,1) = i % filter_size;
        kernel_indices_(i,0) = i / filter_size;
    }
}

AsynSparseConvolution2D::~AsynSparseConvolution2D()
{

}

void AsynSparseConvolution2D::setParameters(Eigen::VectorXf bias, Eigen::MatrixXf weights)
{
    bias_ = bias;
    weights_ = weights;
}

void AsynSparseConvolution2D::initMaps(int H, int W)
{
    H_ = H;
    W_ = W;

    num_pixels_ = H*W;

    if (!initialized_input_maps_)
    {
        old_input_feature_map_ = Eigen::MatrixXf(num_pixels_, nIn_);
        old_input_feature_map_ = old_input_feature_map_.setConstant(0);
        initialized_input_maps_ = true;
    }

    if (!initialized_output_maps_)
    {
        output_feature_map_ = Eigen::MatrixXf(num_pixels_, nOut_);
        output_feature_map_ = output_feature_map_.setConstant(0);
        initialized_output_maps_ = true;
    }
}

AsynSparseConvolution2D::ActiveMatrix AsynSparseConvolution2D::initActiveMap(Eigen::MatrixXf input_feature_map, const Eigen::MatrixXi update_location)
{
    Eigen::VectorXi update_location_linear = update_location(Eigen::all,1) + W_ * update_location(Eigen::all,0);

    ActiveMatrix active_sites_map = ActiveMatrix::Constant(num_pixels_, 1, Site::INACTIVE);
    for (int i=0; i<input_feature_map.rows(); i++)
        if (input_feature_map(i,Eigen::all).array().abs().sum()>0)
            active_sites_map(i,0) = Site::ACTIVE;
            if (debug_)  std::cout << input_feature_map.rows() << std::endl;

    for (int i=0; i<update_location_linear.rows(); i++)
        active_sites_map(update_location_linear(i,0),0) = Site::ACTIVE;

    if (debug_)  std::cout << "init active sites" << std::endl;

    return active_sites_map;
}

AsynSparseConvolution2D::ReturnType AsynSparseConvolution2D::forward(const Eigen::Ref<const Eigen::MatrixXi> update_location,
                                                                     const Eigen::Ref<const Eigen::MatrixXf> input_feature_map,
                                                                     Eigen::Ref<ActiveMatrix>& active_sites_map,
                                                                     RuleBook& rule_book,
                                                                     bool no_update_locations)
{
    if (debug_)  std::cout << "init active sites" << std::endl;
    Eigen::VectorXi update_location_linear = update_location(Eigen::all,1) + rule_book.W_ * update_location(Eigen::all,0);
    if (debug_)  std::cout << "init active sites" << std::endl;

    // time before rulebook
    std::chrono::_V2::system_clock::time_point start, finish, global_start, global_finish ;
    std::chrono::duration<double> time_span;

    start = std::chrono::high_resolution_clock::now();
//    global_start = std::chrono::high_resolution_clock::now();
    if (debug_)  std::cout << "init active sites" << std::endl;

    int num_update_locations = update_location.rows();
    Eigen::Matrix<bool,-1,1> bool_new_active_site = Eigen::Matrix<bool,-1,1>::Constant(num_update_locations, false);
    Eigen::Matrix<bool,-1,1> zero_input_update = Eigen::Matrix<bool,-1,1>::Constant(num_update_locations, false);

    if (first_layer_)
    {
        zero_input_update = input_feature_map(update_location_linear, Eigen::all).array().abs().rowwise().sum() == 0;
        bool_new_active_site = old_input_feature_map_(update_location_linear, Eigen::all).array().abs().rowwise().sum() == 0;

        if (debug_)  std::cout << "Zero input update: " << zero_input_update.transpose()<< std::endl;
        if (debug_)  std::cout << "bool_new_active_site: " << bool_new_active_site.transpose()<< std::endl;
    }

//    finish = std::chrono::high_resolution_clock::now();
//    time_span = finish - start;
//    std::cout << "time before rulebook: " << time_span.count() << std::endl;
//    start = std::chrono::high_resolution_clock::now();

    Eigen::VectorXi new_update_location_linear;

    if (!no_update_locations)
    {
        updateRulebooks(bool_new_active_site,
                        zero_input_update,
                        update_location_linear,
                        active_sites_map,
                        new_update_location_linear,
                        rule_book);
     }

    Eigen::MatrixXi new_update_location(new_update_location_linear.rows(),2);
    new_update_location(Eigen::all, 0) = new_update_location_linear.unaryExpr([&](const int x) { return x / rule_book.W_; });
    new_update_location(Eigen::all, 1) = new_update_location_linear.unaryExpr([&](const int x) { return x % rule_book.W_; });

    if (debug_)  rule_book.print();

    finish = std::chrono::high_resolution_clock::now();
    time_span = finish - start;
//    std::cout << "time during rulebook: " << time_span.count() << "     " << float(time_span.count()) << std::endl;
//    start = std::chrono::high_resolution_clock::now();

    Eigen::MatrixXf output_feature_map = output_feature_map_;

    bool first = true;
    for (int kernel_index=0; kernel_index<filter_volume_; kernel_index++)
    {
        int nrules = rule_book.nrules(kernel_index);
        if (nrules == 0)
            continue;

        Eigen::MatrixXf matrix = weights_(kernel_index, Eigen::all).reshaped(nOut_, nIn_);

        std::vector<int> input, output;
        rule_book.getRules(kernel_index, input, output);

        Eigen::MatrixXf delta_feature(nrules, nIn_);

        for (int r=0; r<nrules; r++)
        {
            if (active_sites_map(output[r]) != Site::NEW_ACTIVE)
            {
                delta_feature(r, Eigen::all) = input_feature_map(input[r], Eigen::all) - old_input_feature_map_(input[r], Eigen::all);
            }
            else
            {
                delta_feature(r, Eigen::all) = input_feature_map(input[r], Eigen::all);
            }

            //std::cout << (matrix * delta_feature(r, Eigen::all).transpose()).transpose() << std::endl;
            output_feature_map(output[r], Eigen::all) += matrix * delta_feature(r, Eigen::all).transpose();;

        }
        //Eigen::MatrixXf update_term = matrix * delta_feature.transpose();
        //std::cout << "A "<< update_term.transpose() << std::endl;
        //std::cout << "B "<< update_term1.transpose() << std::endl;

        if (first)
        {
            first = false;
            if (debug_)  std::cout << "Feature delta: " << delta_feature << std::endl;
            if (debug_)  std::cout << "Weights: " << weights_(kernel_index, Eigen::all).reshaped(nOut_, nIn_) << std::endl;
        }

        if (debug_)  std::cout << "output_feature_map: " << output_feature_map.sum() << std::endl;
    }
//    finish = std::chrono::high_resolution_clock::now();
//    time_span = finish - start;
//    std::cout << "time during update: " << time_span.count() << std::endl;
//    start = std::chrono::high_resolution_clock::now();

    for (int i=0; i<active_sites_map.rows(); i++)
    {
        if (active_sites_map(i) == Site::NEW_INACTIVE)
        {
            for (int j=0; j<nOut_; j++)
                output_feature_map(i,j) = 0;
        }
        if (use_bias_ && active_sites_map(i) == Site::NEW_ACTIVE)
            output_feature_map(i, Eigen::all) += bias_;
    }

    if (debug_)  std::cout << "output and input feature_map after bias: " << output_feature_map.sum() << " " << input_feature_map.sum() << std::endl;

    old_input_feature_map_ = input_feature_map;
    output_feature_map_ = output_feature_map;

//    finish = std::chrono::high_resolution_clock::now();
//    time_span = finish - start;
//    std::cout << "time after rulebook: " << time_span.count() << std::endl;

//    global_finish = std::chrono::high_resolution_clock::now();
//    time_span = global_finish - global_start;
//    std::cout << "global time: " << time_span.count() << std::endl;

    if (debug_)  std::cout << "output and input feature_map after bias: " << new_update_location.cols() << " " << new_update_location.rows() << std::endl;
    if (debug_)  std::cout << new_update_location << std::endl;

//    std::cout << "Time for Rulebook: " << output_feature_map(0, 0) << std::endl;

    return {new_update_location, output_feature_map, active_sites_map};
}

void AsynSparseConvolution2D::updateRulebooks(Eigen::Matrix<bool,-1,1> bool_new_active_site,
                                              Eigen::Matrix<bool,-1,1> zero_input_update,
                                              Eigen::VectorXi update_location_linear,
                                              Eigen::Ref<ActiveMatrix>& active_sites_map,
                                              Eigen::VectorXi& new_update_location_linear,
                                              RuleBook& rulebook)
{
    // find kernel update location
    if (debug_)  std::cout << "Updating Rulebook." << std::endl;
    int num_new_sites_to_update = update_location_linear.rows() * kernel_indices_.rows();

    ActiveMatrix active_sites_to_update(num_new_sites_to_update,1);
    for (int i=0; i<update_location_linear.rows(); i++)
    {
        int j_ = update_location_linear(i) % rulebook.W_;
        int i_ = update_location_linear(i) / rulebook.W_;

        for (int j=0; j<kernel_indices_.rows(); j++)
        {
            int i_new = i_ + kernel_indices_(j, 0) - filter_size_/2;
            int j_new = j_ + kernel_indices_(j, 1) - filter_size_/2;

            int new_update_lin_idx = j_new + i_new*rulebook.W_;
            int lin_idx = i * kernel_indices_.rows() + j ;

            if (i_new >= 0 && j_new >= 0 && j_new < rulebook.W_ && i_new < rulebook.H_)
            {
                active_sites_to_update(lin_idx,0) = active_sites_map(new_update_lin_idx,0); //  N x k^2
            }
            else
            {
                active_sites_to_update(lin_idx,0) = Site::INACTIVE;
            }
        }
    }

    if (debug_)  std::cout << "Active sites to update: " << active_sites_to_update.cast<int>().transpose() << std::endl;

    for (int i=0; i<update_location_linear.rows(); i++)
        active_sites_map(update_location_linear(i)) = Site::UPDATED;

    if (first_layer_)
        for (int i=0; i<zero_input_update.rows(); i++)
            if (zero_input_update(i))
                active_sites_map(update_location_linear(i)) = Site::NEW_INACTIVE;

    if (debug_)  std::cout << "active_sites_map: " << active_sites_map.cast<int>().transpose() << std::endl;

    for (int a_index=0; a_index<active_sites_to_update.rows(); a_index++)
    {
        if (active_sites_to_update(a_index) == Site::INACTIVE)
            continue;

        int k = a_index % filter_volume_;
        int i_active_site = a_index / filter_volume_;

        int position_kernel = filter_volume_ - 1 - k;
        int input_location = update_location_linear(i_active_site);

        int i_input = input_location / rulebook.W_;
        int j_input = input_location % rulebook.W_;

        int i_output = i_input + filter_size_/2 - kernel_indices_(position_kernel, 0);
        int j_output = j_input + filter_size_/2 - kernel_indices_(position_kernel, 1);

        int output_location = j_output + i_output * rulebook.W_;

        if (active_sites_map(output_location) == Site::NEW_ACTIVE) continue;

        if (active_sites_map(output_location) == Site::NEW_INACTIVE)
            continue;

        rulebook.addRule(position_kernel, input_location, output_location);

        if (active_sites_map(output_location) == Site::ACTIVE)
        {
            new_update_location_linear.conservativeResize(new_update_location_linear.rows()+1,
                                                            new_update_location_linear.cols());
            new_update_location_linear(new_update_location_linear.rows()-1) = output_location;

            active_sites_map(output_location) = Site::UPDATED;
        }
    }

    if (debug_) rulebook.print();

    // Update New Active Site Rules
    ActiveMatrix active_sites_map_copied = active_sites_map;

    for (int i=0; i<bool_new_active_site.rows(); i++)
    {
        if (bool_new_active_site(i))
            active_sites_map(update_location_linear(i)) = Site::NEW_ACTIVE;
        active_sites_map_copied(update_location_linear(i)) = Site::INACTIVE;
    }

    if (debug_)  std::cout << "active sites map: " << active_sites_map.cast<int>().transpose() << std::endl;
    if (debug_)  std::cout << "Updating New Active Rules: " << std::endl;

    for (int i=0; i<bool_new_active_site.rows(); i++)
    {
        if (!bool_new_active_site(i))
            continue;
        int output_location = update_location_linear(i);
        int i_ = output_location / rulebook.W_;
        int j_ = output_location % rulebook.W_;

        for (int k=0; k<kernel_indices_.rows(); k++)
        {
            if (k==int(filter_volume_)/2) continue;

            int i_new = i_ + kernel_indices_(k,0) - filter_size_/2;
            int j_new = j_ + kernel_indices_(k,1) - filter_size_/2;
            if (!(i_new >= 0 && j_new >= 0 && j_new < rulebook.W_ && i_new < rulebook.H_)) continue;
            int input_location = j_new + i_new * (rulebook.W_);

            if (active_sites_map_copied(input_location)==Site::INACTIVE)
                continue;

            if (debug_)  std::cout << "Kernel Index: " << kernel_indices_(k,0) << " " << kernel_indices_(k,1) << std::endl;
            if (debug_)  std::cout << "Output Location Index: " << i_ << " " << j_  << std::endl;
            if (debug_)  std::cout << "Input Location Index: " << i_new << " "<< j_new  << std::endl;
            if (debug_)  std::cout << "Adding rule to rulebook: " << input_location << " " << output_location << " for k=" << k << std::endl;

            rulebook.addRule(k, input_location, output_location);
        }
    }
}


#ifndef STAN_MATH_REV_MAT_FUN_DOT_PRODUCT_HPP
#define STAN_MATH_REV_MAT_FUN_DOT_PRODUCT_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/err/check_vector.hpp>
#include <stan/math/prim/arr/err/check_matching_sizes.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <vector>

namespace stan {
namespace math {

namespace {
template <typename T>
struct dot_product_store_type;

template <>
struct dot_product_store_type<var> {
  typedef vari** type;
};

template <>
struct dot_product_store_type<double> {
  typedef double* type;
};

template <typename T1, typename T2>
class dot_product_vari : public vari {
 protected:
  typename dot_product_store_type<T1>::type v1_;
  typename dot_product_store_type<T2>::type v2_;
  size_t length_;

  inline static double var_dot(vari** v1, vari** v2, size_t length) {
    Eigen::VectorXd vd1(length), vd2(length);
    for (size_t i = 0; i < length; i++) {
      vd1[i] = v1[i]->val_;
      vd2[i] = v2[i]->val_;
    }
    return vd1.dot(vd2);
  }

  inline static double var_dot(const T1* v1, const T2* v2, size_t length) {
    Eigen::VectorXd vd1(length), vd2(length);
    for (size_t i = 0; i < length; i++) {
      vd1[i] = value_of(v1[i]);
      vd2[i] = value_of(v2[i]);
    }
    return vd1.dot(vd2);
  }

  template <typename Derived1, typename Derived2>
  inline static double var_dot(const Eigen::DenseBase<Derived1>& v1,
                               const Eigen::DenseBase<Derived2>& v2) {
    Eigen::VectorXd vd1(v1.size()), vd2(v1.size());
    for (int i = 0; i < v1.size(); i++) {
      vd1[i] = value_of(v1[i]);
      vd2[i] = value_of(v2[i]);
    }
    return vd1.dot(vd2);
  }
  inline void chain(vari** v1, vari** v2) {
    for (size_t i = 0; i < length_; i++) {
      v1[i]->adj_ += adj_ * v2_[i]->val_;
      v2[i]->adj_ += adj_ * v1_[i]->val_;
    }
  }
  inline void chain(double* v1, vari** v2) {
    for (size_t i = 0; i < length_; i++) {
      v2[i]->adj_ += adj_ * v1_[i];
    }
  }
  inline void chain(vari** v1, double* v2) {
    for (size_t i = 0; i < length_; i++) {
      v1[i]->adj_ += adj_ * v2_[i];
    }
  }
  inline void initialize(vari**& mem_v, const var* inv, vari** shared = NULL) {
    if (shared == NULL) {
      mem_v = reinterpret_cast<vari**>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(vari*)));
      for (size_t i = 0; i < length_; i++)
        mem_v[i] = inv[i].vi_;
    } else {
      mem_v = shared;
    }
  }
  template <typename Derived>
  inline void initialize(vari**& mem_v, const Eigen::DenseBase<Derived>& inv,
                         vari** shared = NULL) {
    if (shared == NULL) {
      mem_v = reinterpret_cast<vari**>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(vari*)));
      for (size_t i = 0; i < length_; i++)
        mem_v[i] = inv(i).vi_;
    } else {
      mem_v = shared;
    }
  }

  inline void initialize(double*& mem_d, const double* ind,
                         double* shared = NULL) {
    if (shared == NULL) {
      mem_d = reinterpret_cast<double*>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(double)));
      for (size_t i = 0; i < length_; i++)
        mem_d[i] = ind[i];
    } else {
      mem_d = shared;
    }
  }
  template <typename Derived>
  inline void initialize(double*& mem_d, const Eigen::DenseBase<Derived>& ind,
                         double* shared = NULL) {
    if (shared == NULL) {
      mem_d = reinterpret_cast<double*>(
          ChainableStack::memalloc_.alloc(length_ * sizeof(double)));
      for (size_t i = 0; i < length_; i++)
        mem_d[i] = ind(i);
    } else {
      mem_d = shared;
    }
  }

 public:
  dot_product_vari(typename dot_product_store_type<T1>::type v1,
                   typename dot_product_store_type<T2>::type v2, size_t length)
      : vari(var_dot(v1, v2, length)), v1_(v1), v2_(v2), length_(length) {}

  dot_product_vari(const T1* v1, const T2* v2, size_t length,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2, length)), length_(length) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  template <typename Derived1, typename Derived2>
  dot_product_vari(const Eigen::DenseBase<Derived1>& v1,
                   const Eigen::DenseBase<Derived2>& v2,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2)), length_(v1.size()) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  template <int R1, int C1, int R2, int C2>
  dot_product_vari(const Eigen::Matrix<T1, R1, C1>& v1,
                   const Eigen::Matrix<T2, R2, C2>& v2,
                   dot_product_vari<T1, T2>* shared_v1 = NULL,
                   dot_product_vari<T1, T2>* shared_v2 = NULL)
      : vari(var_dot(v1, v2)), length_(v1.size()) {
    if (shared_v1 == NULL) {
      initialize(v1_, v1);
    } else {
      initialize(v1_, v1, shared_v1->v1_);
    }
    if (shared_v2 == NULL) {
      initialize(v2_, v2);
    } else {
      initialize(v2_, v2, shared_v2->v2_);
    }
  }
  virtual void chain() { chain(v1_, v2_); }
};
}  // namespace

/**
 * Returns the dot product.
 *
 * @param[in] v1 First column vector.
 * @param[in] v2 Second column vector.
 * @return Dot product of the vectors.
 * @throw std::domain_error if length of v1 is not equal to length of v2.
 */
template <typename T1, int R1, int C1, typename T2, int R2, int C2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const Eigen::Matrix<T1, R1, C1>& v1,
            const Eigen::Matrix<T2, R2, C2>& v2) {
  check_vector("dot_product", "v1", v1);
  check_vector("dot_product", "v2", v2);
  check_matching_sizes("dot_product", "v1", v1, "v2", v2);
  return var(new dot_product_vari<T1, T2>(v1, v2));
}
/**
 * Returns the dot product.
 *
 * @param[in] v1 First array.
 * @param[in] v2 Second array.
 * @param[in] length Length of both arrays.
 * @return Dot product of the arrays.
 */
template <typename T1, typename T2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const T1* v1, const T2* v2, size_t length) {
  return var(new dot_product_vari<T1, T2>(v1, v2, length));
}

/**
 * Returns the dot product.
 *
 * @param[in] v1 First vector.
 * @param[in] v2 Second vector.
 * @return Dot product of the vectors.
 * @throw std::domain_error if sizes of v1 and v2 do not match.
 */
template <typename T1, typename T2>
inline typename boost::enable_if_c<
    boost::is_same<T1, var>::value || boost::is_same<T2, var>::value, var>::type
dot_product(const std::vector<T1>& v1, const std::vector<T2>& v2) {
  check_matching_sizes("dot_product", "v1", v1, "v2", v2);
  return var(new dot_product_vari<T1, T2>(&v1[0], &v2[0], v1.size()));
}

}  // namespace math
}  // namespace stan
#endif


/*
 * @Description: g2o edge for LIO odo pre-integration measurement
 * @Author: Ge Yao
 * @Date: 2020-11-29 15:47:49
 */
#ifndef LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_ODO_PRE_INTEGRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_ODO_PRE_INTEGRATION_HPP_

#include <iostream>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <sophus/so3.hpp>

#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/types/slam3d_addons/types_slam3d_addons.h>

#include "lidar_localization/models/graph_optimizer/g2o/vertex/vertex_prvag.hpp"

#include <g2o/core/base_binary_edge.h>

#include "glog/logging.h"

namespace g2o {

namespace EdgePRVAGOdoPreIntegration {

static const int DIM = 6;

typedef Eigen::Matrix<double, DIM, 1> Measurement;

class EdgePRVAGOdoPreIntegration : public g2o::BaseBinaryEdge<DIM, Measurement, g2o::VertexPRVAG, g2o::VertexPRVAG> {
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    static const int INDEX_P = 0;
    static const int INDEX_R = 3;

	EdgePRVAGOdoPreIntegration()
	 : g2o::BaseBinaryEdge<DIM, Measurement, g2o::VertexPRVAG, g2o::VertexPRVAG>() {
	 }

	virtual void computeError() override {
        g2o::VertexPRVAG* v0 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[0]);
        g2o::VertexPRVAG* v1 = dynamic_cast<g2o::VertexPRVAG*>(_vertices[1]);

		const Eigen::Vector3d &pos_i = v0->estimate().pos;
		const Sophus::SO3d    &ori_i = v0->estimate().ori;

		const Eigen::Vector3d &pos_j = v1->estimate().pos;
		const Sophus::SO3d    &ori_j = v1->estimate().ori;

		const Eigen::Vector3d &alpha_ij = _measurement.block<3, 1>(INDEX_P, 0);
		const Eigen::Vector3d &theta_ij = _measurement.block<3, 1>(INDEX_R, 0);

		_error.block<3, 1>(INDEX_P, 0) = ori_i.inverse() * (pos_j - pos_i) - alpha_ij;
		_error.block<3, 1>(INDEX_R, 0) = (Sophus::SO3d::exp(theta_ij).inverse()*ori_i.inverse()*ori_j).log();
    }

    virtual void setMeasurement(const Measurement& m) override {
		_measurement = m;
	}

	virtual bool read(std::istream& is) override {
		Measurement m;
		is >> m(INDEX_P + 0) >> m(INDEX_P + 1) >> m(INDEX_P + 2)
		   >> m(INDEX_R + 0) >> m(INDEX_R + 1) >> m(INDEX_R + 2);

    	setMeasurement(m);

		for (int i = 0; i < information().rows(); ++i) {
			for (int j = i; j < information().cols(); ++j) {
				is >> information()(i, j);
				// update cross-diagonal element:
				if (i != j) {
					information()(j, i) = information()(i, j);
				}
			}
		}

		return true;
	}

	virtual bool write(std::ostream& os) const override {
    	Measurement m = _measurement;

		os << m(INDEX_P + 0) << " " <<m(INDEX_P + 1) << " " << m(INDEX_P + 2) << " "
		   << m(INDEX_R + 0) << " " <<m(INDEX_R + 1) << " " << m(INDEX_R + 2) << " ";

		for (int i = 0; i < information().rows(); ++i)
			for (int j = i; j < information().cols(); ++j)
				os << " " << information()(i, j);

		return os.good();
	}
};

} // namespace EdgePRVAGOdoPreIntegration

} // namespace g2o

#endif // LIDAR_LOCALIZATION_MODELS_GRAPH_OPTIMIZER_G2O_EDGE_EDGE_PRVAG_ODO_PRE_INTEGRATION_HPP_


#include "dSFMTsearch.hpp"
#include "Annihilate.h"
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/vector.h>
#include <MTToolBox/period.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include <MTToolBox/AlgorithmCalculateParity.hpp>
#include "printBinary.h"
#include "calc_parity.h"

using namespace MTToolBox;
using namespace std;

static bool check = false;
static bool verbose = true;

static void test_parity0(const dSFMT_param& param, GF2X& f, w128_t parity) {
    dSFMT dsfmt(param);
    GF2X mpoly;
    GF2X q;
    GF2X rem;
    Vec<GF2> vec;
    int i;
    int r;
    int result = 0;
    int count = 100;
    int mexp = dsfmt.getMexp();
    int point = 0;

    if (verbose) printf("start parity zero\n");
    dsfmt.setParityValue(parity);
    for (i = 0; i < count; i++) {
        if (verbose) printf("------\n");
        if (verbose) printf("==shoki (%d)\n", i);
        w128_t seed;
        seed.u64[0] = i + 1;
        seed.u64[1] = 0;
        dsfmt.seed(seed);
        //dsfmt.d_p();
        minpoly<w128_t>(mpoly, dsfmt);
        DivRem(q, rem, mpoly, f);
        if (deg(rem) != -1) {
            if (verbose) printf("minpoly = %ld\n", deg(mpoly));
            if (verbose) printf("rem != 0 deg rempoly = %ld\n", deg(rem));
            if (verbose) printf("deg q = %ld\n", deg(q));
            result = 0;
            point = i;
            break;
        }
        if (deg(mpoly) < mexp) {
            if (verbose) printf("mpoly = %ld\n", deg(mpoly));
        }
        if (deg(mpoly) < mexp) {
            result = 0;
            point = i;
            break;
        }
        r = dsfmt.periodCertification(true);
        if (r == 1) {
            if (verbose) printf("period certification OK\n");
        } else {
            if (verbose) printf("period certification NG -> OK 1\n");
            if (dsfmt.periodCertification(true) != 1) {
                result = 0;
                printf("period critification didn't change status 1!!\n");
                point = i;
                break;
            }
        }
        seed.u64[0] = i + 3;
        seed.u64[1] = 0;
        dsfmt.seed(seed);
        //dsfmt.d_p();
        annihilate<w128_t>(&dsfmt, f);
        if (verbose) printf("==zero\n");
        minpoly<w128_t>(mpoly, dsfmt);
        if (verbose || deg(mpoly) >= mexp) {
            printf("mpoly = %ld\n", deg(mpoly));
        }
        if (deg(mpoly) >= mexp) {
            printf("make zero state failed\n");
            result = 0;
            break;
        }
        //dsfmt.d_p();
        r = dsfmt.periodCertification(true);
        if (r == 1) {
            if (verbose) printf("period certification OK [ERROR]\n");
            dsfmt.d_p();
            result = 0;
            break;
        } else {
            if (verbose) printf("period certification NG -> OK\n");
            if (!dsfmt.periodCertification(true)) {
                result = 0;
                point = i;
                printf("period certification didn't chanege status!!\n");
                break;
            }
        }
        minpoly<w128_t>(mpoly, dsfmt);
        if (verbose || deg(mpoly) < mexp) {
            printf("mpoly = %ld\n", deg(mpoly));
        }
        if (deg(mpoly) < mexp) {
            result = 0;
            point = i;
            break;
        }
        r = dsfmt.periodCertification(true);
        if (r == 1) {
            if (verbose) printf("period certification OK\n");
        } else {
            if (verbose) printf("period certification NG -> OK\n");
            if (!dsfmt.periodCertification(true)) {
                printf("error!!\n");
                point = i;
                return;
            }
        }
        result++;
    }
    if (result) {
        printf("test successed %d / %d\n", result, count);
    } else {
        printf("test failed at count %d\n", point);
    }
}

static bool option(dSFMT_param& param, int argc, char * argv[])
{
    int c;
    bool error = false;
    check = false;
    verbose = false;
    for (;;) {
        c = getopt(argc, argv, "hcv");
        if (error) {
            break;
        }
        if (c == -1) {
            break;
        }
        switch (c) {
        case 'c':
            check = true;
            break;
        case 'v':
            verbose = true;
            break;
        case 'h':
        default:
            error = true;
            break;
        }
    }
    char * para = argv[optind];
    param.mexp = strtoul(para, &para, 10);
    para++;
    param.pos1 = strtoul(para, &para, 10);
    para++;
    param.sl1 = strtoul(para, &para, 10);
    para++;
    param.msk1 = strtoull(para, &para, 16);
    para++;
    param.msk2 = strtoull(para, &para, 16);
    para++;
    param.fix1 = strtoull(para, &para, 16);
    para++;
    param.fix2 = strtoull(para, &para, 16);
    return true;
}

int main(int argc, char *argv[]) {
    dSFMT_param param;
    if (!option(param, argc, argv)) {
        return -1;
    }
    //cout << param.get_string() << endl;
    dSFMT dsfmt(param);
    int mexp = dsfmt.getMexp();
    int maxdegree = dsfmt.bitSize();
    if (check) {
        printf("mexp = %d, maxdegree = %d\n", mexp, maxdegree);
    }
    w128_t w;
    w.u64[0] = 1;
    dsfmt.seed(w);
    GF2X poly;
    GF2X smallpoly;
    GF2X tmp;
    GF2X rempoly;
    GF2X filler;
    w128_t parity;

    minpoly<w128_t>(poly, dsfmt);
    GF2X irreducible = poly;
    if (!hasFactorOfDegree(irreducible, dsfmt.getMexp())) {
        cout << "error does not have factor of degree "
             << dec << dsfmt.getMexp() << endl;
        cout << "degree = " << dec << deg(irreducible) << endl;
        return false;
    }
    parity = calc_parity(dsfmt, irreducible);
    cout << dsfmt.getParamString() << endl;
    if (check) {
        test_parity0(param, irreducible, parity);
    }
    return 0;
}


#include "RockOdometry.hpp"

#include <slam3d/core/Graph.hpp>
#include <boost/format.hpp>
using namespace slam3d;

RockOdometry::RockOdometry(const std::string& name, Graph* graph, Solver* solver, Logger* logger, transformer::Transformation& tf)
 : PoseSensor(name, graph, logger), mTransformation(tf), mSolver(solver)
{
	mLastVertex = 0;
	mLastOdometricPose = Transform::Identity();
	mCurrentOdometricPose = Transform::Identity();
	mGravityReference = Direction::UnitZ();
}

RockOdometry::~RockOdometry()
{
}

void RockOdometry::handleNewVertex(IdType vertex)
{
	// Add odometry transform to previous vertex
	if(mLastVertex > 0)
	{
		TransformWithCovariance twc;
		twc.transform = mLastOdometricPose.inverse() * mCurrentOdometricPose;
		twc.covariance = calculateCovariance(twc.transform);
		SE3Constraint::Ptr se3(new SE3Constraint(mName, twc));
		mGraph->addConstraint(mLastVertex, vertex, se3);
		mGraph->setCorrectedPose(vertex, mGraph->getVertex(mLastVertex).corrected_pose * twc.transform);
	}

	// Add a gravity vector to this vertex
	Eigen::Quaterniond state(mCurrentOdometricPose.rotation());
	Direction upVector = state.inverse() * Eigen::Vector3d::UnitZ();
	GravityConstraint::Ptr grav(new GravityConstraint(mName, upVector, mGravityReference, Covariance<2>::Identity()));
	mGraph->addConstraint(vertex, 0, grav);

	mLastVertex = vertex;
	mLastOdometricPose = mCurrentOdometricPose;
}

Transform RockOdometry::getPose(timeval stamp)
{
	base::Time ts = base::Time::fromSeconds(stamp.tv_sec, stamp.tv_usec);
	getPose(ts);
	return mCurrentOdometricPose;
}

Eigen::Affine3d RockOdometry::getPose(base::Time t)
{
	Eigen::Affine3d affine;
	bool res;
	try
	{
		res = mTransformation.get(t, affine, false);
	}catch(std::exception& e)
	{
		throw InvalidPose(e.what());
	}

	if(!res)
	{
		transformer::TransformationStatus s = mTransformation.getStatus();
		throw InvalidPose(
			(boost::format("Transformation not available! [success: %1%, no chain: %2%, no sample: %3%, interpolation failed: %4%]")
			% s.generated_transformations
			% s.failed_no_chain
			% s.failed_no_sample
			% s.failed_interpolation_impossible).str());
	}

	if((affine.matrix().array() == affine.matrix().array()).all())
	{
		mCurrentOdometricPose.linear() = affine.linear();
		mCurrentOdometricPose.translation() = affine.translation();
	}else
	{
		throw InvalidPose("Odometry sample contained invalid data.");
	}

	return affine;
}

Covariance<6> RockOdometry::calculateCovariance(const Transform &tf)
{
	return Covariance<6>::Identity() * 100;
}


/* Copyright Institute of Sound and Vibration Research - All rights reserved */

#include "point_source_parser.hpp"

#include "point_source.hpp"

#include <libefl/cartesian_spherical_conversion.hpp>
#include <libefl/degree_radian_conversion.hpp>

#include <boost/property_tree/ptree.hpp>

#include <ciso646>

namespace visr
{
namespace objectmodel
{

/*virtual*/ void PointSourceParser::
parse( boost::property_tree::ptree const & tree, Object & src ) const
{
  // parse generic part of object (parent of PointSource)
  ObjectParser::parse( tree, src );

  try
  {
    // note: cannot check for object type id since src might be a subclass of PointSource
    PointSource & pointSrc = dynamic_cast<PointSource&>(src);

    PointSource::Coordinate xPos, yPos, zPos;
    parsePosition( tree.get_child( "position"), xPos, yPos, zPos );
    pointSrc.setX( xPos );
    pointSrc.setY( yPos );
    pointSrc.setZ( zPos );

    pointSrc.setChannelLock( tree.get<PointSource::Coordinate>("channelLock", static_cast<PointSource::Coordinate>( 0.0 ) ) );
  }
  // TODO: distinguish between boost property_tree parse errors and bad dynamic casts.
  catch( std::exception const & ex )
  {
    throw std::invalid_argument( std::string( "Error while parsing point source: object: ") + ex.what() );
  }
}

/*virtual*/ void PointSourceParser::
write( Object const & obj, boost::property_tree::ptree & tree ) const
{
  // note: cannot check for object type id since obj might be a subclass of PointSource
  PointSource const& psObj = dynamic_cast<PointSource const&>(obj);

  ObjectParser::write( obj, tree );
  tree.put<PointSource::Coordinate>( "position.x", psObj.x() );
  tree.put<PointSource::Coordinate>( "position.y", psObj.y() );
  tree.put<PointSource::Coordinate>( "position.z", psObj.z() );

  if( psObj.channelLockDistance() != static_cast<PointSource::Coordinate>(0.0) )
  {
    tree.put<PointSource::Coordinate>( "channelLock", psObj.channelLockDistance() );
  }
}

/*static*/ void
PointSourceParser::parsePosition( boost::property_tree::ptree const & posTree,
                                  Object::Coordinate & x,
                                  Object::Coordinate & y,
                                  Object::Coordinate & z )
{
  // Parse point source-specific data members
  // Allow both cartesian and spherical coordinates, but no mixture between them
  boost::optional<PointSource::Coordinate> const coordX = posTree.get_optional<PointSource::Coordinate>( "x" );
  boost::optional<PointSource::Coordinate> const coordY = posTree.get_optional<PointSource::Coordinate>( "y" );
  boost::optional<PointSource::Coordinate> const coordZ = posTree.get_optional<PointSource::Coordinate>( "z" );
  boost::optional<PointSource::Coordinate> const coordAz = posTree.get_optional<PointSource::Coordinate>( "az" );
  boost::optional<PointSource::Coordinate> const coordEl = posTree.get_optional<PointSource::Coordinate>( "el" );
  boost::optional<PointSource::Coordinate> const coordRadius = posTree.get_optional<PointSource::Coordinate>( "radius" );

  bool const anyCartCoord = (coordX or coordY or coordZ);
  bool const anySphCoord = (coordAz or coordEl or coordRadius);

  if( not( anyCartCoord or anySphCoord ) )
  {
    throw std::invalid_argument( "Positions contain either Cartesian or spherical coordinates" );
  }
  if( anyCartCoord and anySphCoord )
  {
    throw std::invalid_argument( "Positions must not contain both Cartesian or spherical coordinates" );
  }
  if( anyCartCoord )
  {
    if( not( coordX and coordY ) )
    {
      throw std::invalid_argument( "Positions in Cartesian coordinates must provide the \"x\" and \"y\" attributes." );
    }
    x = *coordX;
    y = *coordY;
    z = coordZ ? *coordZ : static_cast<PointSource::Coordinate>(0.0);
  }
  else // use spherical coordinates
  {
    if( not( coordAz and coordEl ) )
    {
      throw std::invalid_argument( "Positions in spherical coordinates must provide the \"az\" and \"el\" attributes." );
    }
    PointSource::Coordinate const radius = coordRadius ? *coordRadius : static_cast<PointSource::Coordinate>(1.0);
    std::tie( x, y, z ) = efl::spherical2cartesian( efl::degree2radian( *coordAz ),
                                                    efl::degree2radian( *coordEl ),
                                                    radius );
  }
}

} // namespace objectmodel
} // namespace visr


#include <ros/ros.h>
#include <ros/publisher.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>
#include <boost/asio.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/math/constants/constants.hpp>
#include <algorithm>
#include <string>

using namespace std;

/*
  README
  @author Brian Cochran, github: @btdubs
  Node for taking in serial CSV distances from ultrasonic array,
  then outputing a point cloud. Ultrasonic sensors defined in
  urdf and uses that for transforming points.
  Requires a tf tree being published.
*/

//defaults for launch file paramenters
#define NUM_SENSORS_DEFAULT 6
#define DRAW_METHOD_DEFAULT 0 //0 = Single Point, 1 = wall, 2 = circle, 3 = semicircle
#define NUM_POINTS_DEFAULT 12 //# minimum points for each semicircle/circle/wall
#define RADIUS_DEFAULT 0.1f //radius of semicircle/circle in meters
#define LENGTH_DEFAULT 0.1f //length of wall
#define DISTANCE_CLIP_DEFAULT 5.0f //distance beyond which we ignore point in meters
#define BAUD_RATE_DEFAULT 9600 //rate of transfer. Needs to match Arduino

const double pi = boost::math::constants::pi<double>();

string readLine(boost::asio::serial_port &port) {
    string line = "";
    bool inLine = false;
    while (true) {
        char in;
        try {
            boost::asio::read(port, boost::asio::buffer(&in, 1));
            if (in == '\n') {
                return line;
            }
            line += in;
        } catch (
            boost::exception_detail::clone_impl <boost::exception_detail::error_info_injector<boost::system::system_error>> &err) {
            ROS_ERROR("Error reading serial port.");
            ROS_ERROR_STREAM(err.what());
        }

    }
}

vector<double> parseLine(string line) {
  vector<string> strs;
  boost::split(strs, line, boost::is_any_of(","));

  vector<double> dist(strs.size());
  std::transform(strs.begin(), strs.end(), dist.begin(), [](const auto &s){return std::stod(s);}); //convert string to double

  return dist;
}

void drawWall(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ center, float length, int numPoints) {
  length = (length + 1) / 2.0; //@Note: adds one to ensure we get at least the numPoints desired
  float step = length / (float) numPoints / 2.0;
  float dist = step;
  for (int i = 0; i < numPoints / 2; i++) {
    //draw half the line and mirror
    pcl::PointXYZ point1(center.x, dist, 0); //#TODO: might need to swap x and y
    pcl::PointXYZ point2(center.x, -dist, 0); //#TODO: might need to swap x and y

    cloud.push_back(point1);
    cloud.push_back(point2);
    dist = dist + step;
  }

}

void drawSemiCircle(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ point, float radius, int numPoints) {
  numPoints = (numPoints + 1) / 2; //@Note: adds one to ensure we get at least the numPoints desired
  float angleStep = (pi / 2.0) / numPoints;

  float angle = 0.0; //#TODO: may need to shift this start angle

  for (int i = 0; i < numPoints ; i++) {
    //draw a quarter of a cicle and mirror
    float x = radius * cos(angle);
    float y = radius * sin(angle);

    cloud.push_back(pcl::PointXYZ(point.x + radius - x, y, 0));
    cloud.push_back(pcl::PointXYZ(point.x + radius - x, -y, 0));

    angle = angle + angleStep;
  }

}

void drawCircle(pcl::PointCloud<pcl::PointXYZ> &cloud, pcl::PointXYZ point, double radius, int numPoints) {
  double angleStep = (2.0 * pi) / numPoints;

  double angle = 0; //start angle

  for (int i = 0; i < numPoints; i++) {
    //draw a circle
    double x = radius * cos(angle);
    double y = radius * sin(angle);

    cloud.push_back(pcl::PointXYZ(point.x + radius + x, y, 0)); //point.x + radius is center of circle
    angle = angle + angleStep;

  }

}

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

  ros::init(argc, argv, "ultrasonic_array");

  ros::NodeHandle nh;
  ros::NodeHandle nhp("~");

  ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("/ultrasonic_array", 1);

  //Roslaunch file
  string sensor_base_link;
  string sensor_link;
  float update_rate;
  string serial_port_name;
  float distance_clip;
  float length;
  float radius;
  int num_sensors;
  int num_points;
  int draw_method;
  int baud_rate;
  nhp.param(string("serial_port_name"), serial_port_name, string("/dev/ttyUSB0"));
  nhp.param(string("sensor_base_link"), sensor_base_link, string("ultrasonic_array_base"));
  nhp.param(string("sensor_link_prefix"), sensor_link, string("ultrasonic_"));
  nhp.param(string("update_rate"), update_rate, 10.0f);
  nhp.param(string("number_of_sensors"), num_sensors, NUM_SENSORS_DEFAULT);
  nhp.param(string("baud_rate"), baud_rate, BAUD_RATE_DEFAULT);
  nhp.param(string("distance_clip"), distance_clip, DISTANCE_CLIP_DEFAULT);
  nhp.param(string("draw_method"), draw_method, DRAW_METHOD_DEFAULT);
  nhp.param(string("wall_length"), length, LENGTH_DEFAULT);
  nhp.param(string("circle_radius"), radius, RADIUS_DEFAULT);
  nhp.param(string("number_of_points"), num_points, NUM_POINTS_DEFAULT);

  //Connect serial
  ROS_INFO_STREAM("Connecting to serial at port: " + serial_port_name);
  boost::asio::io_service io_service;
  boost::asio::serial_port serial(io_service, serial_port_name);
  serial.set_option(boost::asio::serial_port_base::baud_rate(baud_rate));

  // wait for microcontroller to start
  ros::Duration(2.0).sleep();

  ros::Rate rate(update_rate);

  //Transforms
  tf::TransformListener tf_listener;
  tf::StampedTransform tf_transform;
  pcl::PointCloud<pcl::PointXYZ> compiled_cloud;

  //lookup transforms and store them as they do not change
  vector<tf::StampedTransform> tf_transform_vector;
  for (int i = 0; i < num_sensors; i++) {
    if(tf_listener.waitForTransform(sensor_base_link, sensor_link + to_string(i), ros::Time(0), ros::Duration(5.0))) {
      tf_listener.lookupTransform(sensor_base_link, sensor_link + to_string(i), ros::Time(0), tf_transform);
    }

    tf_transform_vector.push_back(tf_transform);
  }

  function<void(pcl::PointCloud<pcl::PointXYZ> &a, pcl::PointXYZ &b)> draw;
  switch (draw_method) {
      default:
      case 0: draw = [](pcl::PointCloud<pcl::PointXYZ> a, pcl::PointXYZ b){};
              break;
      case 1: draw = bind(drawWall, _1, _2, length, num_points);
              break;
      case 2: draw = bind(drawCircle, _1, _2, radius, num_points);
              break;
      case 3: draw = bind(drawSemiCircle, _1, _2, radius, num_points);
              break;
  }

  while(ros::ok() && serial.is_open()) {
    ros::spinOnce();

    string line = readLine(serial);
    ROS_INFO_STREAM(line);
    vector<double> distances = parseLine(line);

    for (int i = 0; i < num_sensors; i++) {
      pcl::PointCloud<pcl::PointXYZ> cloud;

      if(distances[i] < distance_clip) {
        pcl::PointXYZ point(distances[i], 0.0, 0.0);

        cloud.push_back(point); //add point direct from Arduino sensor
        draw(cloud, point);

        pcl_ros::transformPointCloud(cloud, cloud, tf_transform_vector[i]);

        //add point cloud to the output cloud
        compiled_cloud += cloud;
      }
    }

    sensor_msgs::PointCloud2 outmsg;
    pcl::toROSMsg(compiled_cloud, outmsg);
    outmsg.header.frame_id = sensor_base_link;

    pub.publish(outmsg);
    compiled_cloud.clear(); //remove old data for next round
    rate.sleep();
  }

  ROS_INFO_STREAM("Shutting down Ultrasonic Array");
  serial.close();

  return 0;
}


#include "CUDD_Context.hpp"
#include <cstdlib>

#include <boost/format.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>

namespace metaSMT {
  namespace solver {

class CUDD_Distributed : public CUDD_Context
{

public:

	CUDD_Distributed ()
  : CUDD_Context()
  , previous(_manager.bddZero())
  {
	  reset();
	}

	bool solve() {
 	 	BDD complete =_assertions & _assumptions;
    if ( previous != complete) {
      //std::cout << "resetting distribution cache" << std::endl;
			reset();
      previous = complete;
    } else {
      //std::cout << "keeping distribution cache" << std::endl;
    }
	 	bool  ret =  complete != _manager.bddZero();
         	_assumptions = _manager.bddOne();
        	//printDD(complete.getNode(),"BDD.dot");
	        if(ret) {
			store_solution( complete.getNode() );
		}
		return ret;
	}

private:
	std::map<DdNode *, unsigned> map_0;
	std::map<DdNode *, unsigned> map_1;

	void reset() {

		map_0.clear();
		map_1.clear();

		map_0 [_manager.bddZero().getNode()] = 1;
		map_1 [_manager.bddZero().getNode()] = 0;
		map_0 [_manager.bddOne().getNode()] = 0;
		map_1 [_manager.bddOne().getNode()] = 1;
	}
	unsigned distance(DdNode *parent, DdNode *child) {
		int i_1 = Cudd_ReadPerm(_manager.getManager(),Cudd_NodeReadIndex(parent));
		int i_2 = (Cudd_IsConstant(child)) ? _manager.ReadSize() : Cudd_ReadPerm(_manager.getManager(), Cudd_NodeReadIndex(child));
		//std::cout << boost::format( "distance(v%d,v%d) = %d - %d = %d\n")
		//	% parent->index % child->index % i_2 % i_1 % (i_2 - i_1);
		return i_2 - i_1;
	}

	unsigned skipped ( DdNode *parent, DdNode *child) {
		unsigned t = distance (parent, child);
		if(t > 0) return t-1;
		else return 0;
	}

	unsigned count_0 (DdNode *root) {
		if( Cudd_IsComplement(root)) {
			DdNode * comp = Cudd_Regular(root);
 			//std::cout << boost::format("count_0(v%d)=count_1( ~v%d )\n") % root->index% comp->index;
			return count_1(comp);
		}
		//std::cout << boost::format("count_0(v%d)\n") % root->index;
		std::map<DdNode *, unsigned>::iterator iter = map_0.find(root);
	 	if(iter != map_0.end()) {
		   	//std::cout << boost::format("count_0(v%d) = %d\n") % root->index % iter->second;
			return iter->second;
		} else {

			unsigned ret = count_0 ( Cudd_T( root ) )*( 1ull << skipped( root, Cudd_T(root) ) ) ;
			//std::cout << "then count is " << ret << std::endl;
		   	ret += count_0 (Cudd_E(root)) *  ( 1ull << skipped( root, Cudd_E( root ) ) );
			//std::cout << "after else count is " << ret << std::endl;
		   	map_0.insert( std::make_pair(root,ret) );
			//std::cout << boost::format("count_0(v%d) = %d\n") % root->index % ret;
		   	return ret;
		}
	}

	unsigned count_1 (DdNode *root) {
		if( Cudd_IsComplement(root)) {
			DdNode * comp = Cudd_Regular(root);
 			//std::cout << boost::format("count_1(v%d)=count_0( ~v%d )\n") % root->index% comp->index;
			return count_0(comp);
		}
		//std::cout << boost::format("count_1(v%d)\n") % root->index;
		std::map<DdNode *, unsigned>::iterator iter = map_1.find(root);

	if(iter != map_1.end()) {
		   //std::cout << boost::format("count_1(v%d) = %d\n") % root->index % iter->second;
		  return iter->second;
		} else {
		     	unsigned ret = count_1 ( Cudd_T( root ) )*( 1ull << skipped( root, Cudd_T(root) ) ) ;
			//std::cout << "then count is " << ret << std::endl;
		   	ret += count_1 (Cudd_E(root)) *  ( 1ull << skipped( root, Cudd_E( root ) ) );
			//std::cout << "after else count is " << ret << std::endl;

		   map_1.insert( std::make_pair(root,ret) );
	           //std::cout << boost::format("count_1(v%d) = %d\n") % root->index % ret;
		   return ret;

	         }
	}

	void store_solution ( DdNode * root) {

	   // clear solution
            unsigned size = _manager.ReadSize();
	    std::vector< boost::logic::tribool > (size, boost::logic::indeterminate).swap(_solution);

	    bool negated = false;

            while( ! Cudd_IsConstant(root) ) {

		//std::cout << "chosing for v" << root->index << " (negated: " << negated << ")" << std::endl;

		unsigned cnt_r = 0;
		unsigned cnt_t = 0;
		negated ^= Cudd_IsComplement( root );
		root = Cudd_Regular(root);
		DdNode* child = Cudd_T(root);
		if( negated ) {
			cnt_r = count_0(root);
			cnt_t = count_0(child) << skipped(root, child);
		} else {
			cnt_r = count_1(root);
			cnt_t = count_1(child)<< skipped(root, child);
		}

		//std::cout << "count: " << cnt_r << " " << cnt_t << std::endl;

      		boost::uniform_int<unsigned> rnd( 0, cnt_r-1);
		unsigned select = rnd(gen);
		//std::cout << boost::format("%u %u %u") %select%cnt_r%cnt_t << std::endl;
		if(select <  cnt_t )
		{
			//std::cout << "v" << root->index << " = 1" << std::endl;
			_solution[Cudd_NodeReadIndex(root)] = true;
			root = Cudd_T( root );
		}
		else
		{
			//std::cout << "v" << root->index << " = 0" << std::endl;
			_solution[Cudd_NodeReadIndex(root)] = false;
			root = Cudd_E( root );
		}
	    }
	    assert( Cudd_V(root) == 1 );
	}

  void printDD(DdNode *root, std::string fileName) {
    FILE *file = fopen(fileName.c_str(),"w");

    char **iname = (char**) malloc(sizeof(char *)*_manager.ReadSize());
    for(unsigned i = 0; i < (unsigned)_manager.ReadSize(); ++i)
    {
      char *name = (char*) malloc(256);
      sprintf(name,"v%d", _manager.bddVar(i).NodeReadIndex());
      iname [i] = name;
    }
    //extern int Cudd_DumpDot (DdManager *dd, int n, DdNode **f, char **inames, char **onames, FILE *fp);
    Cudd_DumpDot (_manager.getManager(),1,&root,iname, NULL,file);
    fclose(file);
  }
    protected:
      boost::mt19937 gen;

    private:
      BDD previous;
    }; // class CUDD_Distribuded

  }//  namespace solver
} //namespace metaSMT


//!
//! Common header files included by all the input implementations. Contains helper functions.
//!
//! \file common.hpp
//! \author Lachlan McCalman
//! \author Nahid Akbar
//! \date 2014
//! \license General Public License version 3 or later
//! \copyright (c) 2014, NICTA
//!

#pragma once

#include "input.hpp"
#include "io/npy.hpp"
#include "io/string.hpp"
#include "io/csv.hpp"

#include <Eigen/Core>

namespace Eigen
{
  //! for reading from boost::program_options
  //!
  inline void validate(boost::any& v, const std::vector<std::string>& values, Eigen::Vector2d*, int)
  {
    std::vector<std::string> input = obsidian::io::split(values[0]);
    Eigen::Vector2d output;
    if (input.size() == 2)
    {
      output(0) = std::stod(input[0]);
      output(1) = std::stod(input[1]);
    } else
    {
      throw po::validation_error(po::validation_error::invalid_option_value);
    }
    v = output;
  }

  //! for reading from boost::program_options
  //!
  inline void validate(boost::any& v, const std::vector<std::string>& values, Eigen::Vector3i*, int)
  {
    std::vector<std::string> input = obsidian::io::split(values[0]);
    Eigen::Vector3i output;
    if (input.size() == 3)
    {
      output(0) = std::stoi(input[0]);
      output(1) = std::stoi(input[1]);
      output(2) = std::stoi(input[2]);
    } else
    {
      throw po::validation_error(po::validation_error::invalid_option_value);
    }
    v = output;
  }

  //! for reading from boost::program_options
  //!
  inline void validate(boost::any& v, const std::vector<std::string>& values, Eigen::Vector3d*, int)
  {
    std::vector<std::string> input = obsidian::io::split(values[0]);
    Eigen::Vector3d output;
    if (input.size() == 3)
    {
      output(0) = std::stod(input[0]);
      output(1) = std::stod(input[1]);
      output(2) = std::stod(input[2]);
    } else
    {
      throw po::validation_error(po::validation_error::invalid_option_value);
    }
    v = output;
  }

  //! for reading from boost::program_options
  //!
  inline void validate(boost::any& v, const std::vector<std::string>& values, Eigen::VectorXd*, int)
  {
    std::vector<std::string> input = obsidian::io::split(values[0]);
    Eigen::VectorXd output(input.size());
    try
    {
      for (uint i = 0; i < input.size(); i++)
      {
        output(i) = std::stod(input[i]);
      }
    } catch (...)
    {
      throw po::validation_error(po::validation_error::invalid_option_value);
    }
    v = output;
  }

}

namespace obsidian
{
  //! for reading a rectangular portion of list of list of fields as a Matrix
  //!
  template<typename T, int C, int Offset>
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> readFixed(const std::vector<std::vector<std::string>> & data)
  {
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> results(data.size(), C);
    for (uint i = 0; i < data.size(); i++)
    {
      for (uint j = 0; j < C; j++)
      {
        results(i, j) = io::from_string<T>(data[i][Offset + j]);
      }
    }
    return results;
  }

  //! for writing a matrix into a list of list of fields
  //!
  template<typename T>
  void writeFixed(std::vector<std::vector<std::string>> & data, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> & obj)
  {
    if (data.size() < static_cast<size_t>(obj.rows()))
    {
      data.resize(obj.rows());
    }
    for (uint i = 0; i < obj.rows(); i++)
    {
      for (uint j = 0; j < obj.cols(); j++)
      {
        data[i].push_back(io::to_string<T>(obj(i, j)));
      }
    }
  }

  //! for reading a ragged array (vector of matrix) from a list of list of fields
  //!
  template<typename T, int Offset>
  std::vector<Eigen::Matrix<T, Eigen::Dynamic, 1>> readRagged(std::vector<std::vector<std::string>> & data)
  {
    std::vector<Eigen::Matrix<T, Eigen::Dynamic, 1>> results;
    for (uint i = 0; i < data.size(); i++)
    {
      Eigen::Matrix<T, Eigen::Dynamic, 1> freqs(data[i].size() - Offset);

      for (uint j = Offset; j < data[i].size(); j++)
      {
        freqs[j - Offset] = io::from_string<T>(data[i][j]);
      }
      results.push_back(freqs);
    }
    return results;
  }

  //! for wtiting a ragged array (vector of matrix) into a list of list of fields
  //!
  template<typename T>
  void writeRagged(std::vector<std::vector<std::string>> & data, std::vector<Eigen::Matrix<T, Eigen::Dynamic, 1>> & obj)
  {
    if (data.size() < obj.size())
    {
      data.resize(obj.size());
    }
    for (uint i = 0; i < obj.size(); i++)
    {
      for (uint j = 0; j < obj[i].rows(); j++)
      {
        data[i].push_back(io::to_string<T>(obj[i](j)));
      }
    }
  }

  //! helper method for building boost::program_options::variable_map
  //!
  inline po::variables_map build_vm(po::variables_map vm, const po::options_description & od, const std::string & topic,
                                    const std::map<std::string, std::string> & map)
  {
    std::ostringstream oss;
    oss << "[" << topic << "]" << std::endl;
    for (auto kv : map)
    {
      oss << kv.first << " = " << kv.second << std::endl;
    }
    std::istringstream iss(oss.str());
    po::store(po::parse_config_file(iss, od), vm);
    return vm;
  }

}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_SIMPLIFY_HPP
#define BOOST_GEOMETRY_TEST_SIMPLIFY_HPP

// Test-functionality, shared between single and multi tests

#include <iomanip>
#include <sstream>
#include <geometry_test_common.hpp>
#include <boost/geometry/algorithms/simplify.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/variant/variant.hpp>

template <typename Tag, typename Geometry>
struct test_inserter
{
    static void apply(Geometry& , std::string const& , double )
    {}
};

template <typename Geometry>
struct test_inserter<bg::linestring_tag, Geometry>
{
    template <typename DistanceMeasure>
    static void apply(Geometry& geometry,
            std::string const& expected,
            DistanceMeasure const& distance)
    {
        {
            Geometry simplified;
            bg::detail::simplify::simplify_insert(geometry,
                std::back_inserter(simplified), distance);

            std::ostringstream out;
            // TODO: instead of comparing the full string (with more or less decimal digits),
            // we should call something more robust to check the test for example geometry::equals
            out << std::setprecision(12) << bg::wkt(simplified);
            BOOST_CHECK_EQUAL(out.str(), expected);
        }

#ifdef TEST_PULL89
        {
            typedef typename bg::point_type<Geometry>::type point_type;
            typedef typename bg::strategy::distance::detail::projected_point_ax<>::template result_type<point_type, point_type>::type distance_type;
            typedef bg::strategy::distance::detail::projected_point_ax_less<distance_type> less_comparator;

            distance_type max_distance(distance);
            less_comparator less(max_distance);

            bg::strategy::simplify::detail::douglas_peucker
                <
                    point_type,
                    bg::strategy::distance::detail::projected_point_ax<>,
                    less_comparator
                > strategy(less);

            Geometry simplified;
            bg::detail::simplify::simplify_insert(geometry,
                std::back_inserter(simplified), max_distance, strategy);

            std::ostringstream out;
            // TODO: instead of comparing the full string (with more or less decimal digits),
            // we should call something more robust to check the test for example geometry::equals
            out << std::setprecision(12) << bg::wkt(simplified);
            BOOST_CHECK_EQUAL(out.str(), expected);
        }
#endif
    }
};

template <typename Geometry, typename DistanceMeasure>
void check_geometry(Geometry const& geometry,
                    std::string const& expected,
                    DistanceMeasure const&  distance)
{
    Geometry simplified;
    bg::simplify(geometry, simplified, distance);

    std::ostringstream out;
    out << std::setprecision(12) << bg::wkt(simplified);
    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename Geometry, typename Strategy, typename DistanceMeasure>
void check_geometry(Geometry const& geometry,
                    std::string const& expected,
                    DistanceMeasure const& distance,
                    Strategy const& strategy)
{
    Geometry simplified;
    bg::simplify(geometry, simplified, distance, strategy);

    std::ostringstream out;
    out << std::setprecision(12) << bg::wkt(simplified);
    BOOST_CHECK_EQUAL(out.str(), expected);
}

template <typename Geometry, typename DistanceMeasure>
void test_geometry(std::string const& wkt,
        std::string const& expected,
        DistanceMeasure distance)
{
    typedef typename bg::point_type<Geometry>::type point_type;

    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    boost::variant<Geometry> v(geometry);

    // Define default strategy for testing
    typedef bg::strategy::simplify::douglas_peucker
        <
            typename bg::point_type<Geometry>::type,
            bg::strategy::distance::projected_point<double>
        > dp;

    check_geometry(geometry, expected, distance);
    check_geometry(v, expected, distance);

    BOOST_CONCEPT_ASSERT( (bg::concepts::SimplifyStrategy<dp, point_type>) );

    check_geometry(geometry, expected, distance, dp());
    check_geometry(v, expected, distance, dp());

    // Check inserter (if applicable)
    test_inserter
        <
            typename bg::tag<Geometry>::type,
            Geometry
        >::apply(geometry, expected, distance);

#ifdef TEST_PULL89
    // Check using non-default less comparator in douglass_peucker
    typedef typename bg::strategy::distance::detail::projected_point_ax<>::template result_type<point_type, point_type>::type distance_type;
    typedef bg::strategy::distance::detail::projected_point_ax_less<distance_type> less_comparator;

    distance_type const max_distance(distance);
    less_comparator const less(max_distance);

    typedef bg::strategy::simplify::detail::douglas_peucker
        <
            point_type,
            bg::strategy::distance::detail::projected_point_ax<>,
            less_comparator
        > douglass_peucker_with_less;

    BOOST_CONCEPT_ASSERT( (bg::concepts::SimplifyStrategy<douglass_peucker_with_less, point_type>) );

    check_geometry(geometry, expected, distance, douglass_peucker_with_less(less));
    check_geometry(v, expected, distance, douglass_peucker_with_less(less));
#endif
}

template <typename Geometry, typename Strategy, typename DistanceMeasure>
void test_geometry(std::string const& wkt,
        std::string const& expected,
        DistanceMeasure const& distance,
        Strategy const& strategy)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    boost::variant<Geometry> v(geometry);

    BOOST_CONCEPT_ASSERT( (bg::concepts::SimplifyStrategy<Strategy,
                           typename bg::point_type<Geometry>::type>) );

    check_geometry(geometry, expected, distance, strategy);
    check_geometry(v, expected, distance, strategy);
}

#endif


#include <stdlib.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <memory>
/* these are needed to generate normally distributed random numbers */
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/math/distributions/normal.hpp>
#include "seispp.h"
#include "ensemble.h"
#include "PfStyleMetadata.h"
#include "StreamObjectReader.h"
#include "StreamObjectWriter.h"

using namespace std;
using namespace SEISPP;
void usage()
{
    cerr << "impulse_simulator > out [-i pattern -v --help -text -pf pffile]"
        <<endl
        << "Creates a simulated ThreeComponentEnsemble of 3C impulses according to"
        << " a recipe in a parameter file"<<endl
        << " -i is used to define the data as a clone of the contents of file=pattern. "<<endl
        << "pattern file must be a serialized (binary) file with one ThreeComponentEnsemble object"
        <<endl
        << "Default output is created according to parameter file definitions"
        <<endl
        << " -v - be more verbose"<<endl
        << " --help - prints this message"<<endl
        << " -text - switch to text input and output (default is binary)"<<endl
        << " -pf - use alternate parameter file pffile (default impulse_response.pf)"
        <<endl;
    exit(-1);
}
class Impulse3CDefinition
{
public:
  double time, A[3];
  int member_number;
  Impulse3CDefinition(string line);
  Impulse3CDefinition(const Impulse3CDefinition& parent);
  Impulse3CDefinition& operator=(const Impulse3CDefinition& parent);
};
Impulse3CDefinition::Impulse3CDefinition(string line)
{
  try{
    stringstream ss(line.c_str());
    ss >> member_number;
    ss >> time;
    int k;
    for(k=0;k<3;++k) ss >> A[k];
  }catch(...){throw;};
}
Impulse3CDefinition::Impulse3CDefinition(const Impulse3CDefinition& parent)
{
  member_number=parent.member_number;
  time=parent.time;
  int k;
  for(k=0;k<3;++k)A[k]=parent.A[k];
}
Impulse3CDefinition& Impulse3CDefinition::operator=(const Impulse3CDefinition& parent)
{
  if(this!=&parent)
  {
   member_number=parent.member_number;
   time=parent.time;
   int k;
   for(k=0;k<3;++k)A[k]=parent.A[k];
  }
  return *this;
}
vector<Impulse3CDefinition> LoadImpulses(PfStyleMetadata& control)
{
  try{
    const string key("impulses");
    vector<Impulse3CDefinition> result;
    list<string> lines(control.get_tbl(key));
    list<string>::iterator lptr;
    for(lptr=lines.begin();lptr!=lines.end();++lptr)
    {
      result.push_back(Impulse3CDefinition(*lptr));
    }
    return result;
  }catch(...){throw;};
}
ThreeComponentEnsemble load_pattern_file(string fname)
{
  try{
    ThreeComponentEnsemble d;
    /* Require this be binary */
    StreamObjectReader<ThreeComponentEnsemble> inp(fname,'b');
    d=inp.read();
    /* Zero the contents */
    vector<ThreeComponentSeismogram>::iterator dptr;
    for(dptr=d.member.begin();dptr!=d.member.end();++dptr)
        dptr->u.zero();
    return d;
  }catch(...){throw;};
}
ThreeComponentEnsemble create_pattern_from_pf(PfStyleMetadata& control)
{
  try{
    int nmembers=control.get<int>("number_seismograms");
    int ns=control.get<int>("number_samples");
    double dt=control.get<double>("dt");
    double t0=control.get<double>("t0");
    /* this key defines the integer count key set for each seismogram*/
    string member_count_key=control.get<string>("member_count_key");
    ThreeComponentEnsemble result(nmembers,ns);
    int i,evid;
    for(i=0,evid=666;i<nmembers;++i,++evid)
    {
      ThreeComponentSeismogram d(ns);
      d.u.zero();
      d.t0=t0;
      d.ns=ns;
      d.dt=dt;
      d.components_are_cardinal=true;
      d.components_are_orthogonal=true;
      d.tref=absolute;
      d.live=true;
      int k,j;
      for(k=0;k<3;++k)
      {
        for(j=0;j<3;++j)
        {
          if(i==k)
            d.tmatrix[k][k]=1.0;
          else
            d.tmatrix[k][j]=0.0;
        }
      }
      d.put("nsamp",ns);
      d.put("time",t0);
      d.put("dt",dt);
      d.put("samprate",1.0/dt);
      d.put(member_count_key.c_str(),i);
      /* This oddity is necessary to set an internal boolean
       * */
      d.ator(0.0);
      /* We arbitrarily set sta and evid as a means to define a gather
       * downstream if desired.  This meshes with current use but
       * is not as generic as it should be*/
      d.put("sta","impulse_simulation");
      d.put("evid",evid);
      /* Constructor used above creates the pattern so we
       * copy this new d into slot i*/
      result.member[i]=d;
    }
    return result;
  }catch(...){throw;};
}
/* Copied form http://en.wikibooks.org/wiki/C++_Programming/Libraries/Boost */
using namespace boost;
double SampleNormal(double mean, double sigma)
{
    /* Generate a random number from the current time */
    //static mt19937 rng(static_cast<unsigned> (std::time(0)));
    uint64_t seed;
    seed=static_cast<uint64_t>(std::time(NULL));
    static boost::random::mt19937 rng(seed);
    /* This defines a normal distribution to generate numbers */
    boost::random::normal_distribution<double> norm_dist(mean,sigma);
    // bind random number generator to distribution to forma function object
    variate_generator<boost::random::mt19937&, boost::random::normal_distribution<double> >
        normal_sampler(rng, norm_dist);
    return(normal_sampler());
}
void add_gaussian_noise(ThreeComponentEnsemble& d,double sigma)
{
    int i,j,k;
    for(i=0;i<d.member.size();++i)
    {
        for(j=0;j<d.member[i].u.columns();++j)
        {
            for(k=0;k<3;++k)
            {
                d.member[i].u(k,j) += SampleNormal(0.0,sigma);
            }
        }
    }
}
bool SEISPP::SEISPP_verbose(false);
int main(int argc, char **argv)
{
    int i;
    if(argc>1)
      if(string(argv[1])=="--help") usage();
    bool binary_data(true);
    bool use_pattern_file(false);
    string patternfile("");
    string pffile("impulse_simulator.pf");
    for(i=1;i<argc;++i)
    {
        string sarg(argv[i]);
        if(sarg=="--help")
        {
            usage();
        }
        else if(sarg=="-i")
        {
            ++i;
            if(i>=argc)usage();
            patternfile=string(argv[i]);
            use_pattern_file=true;
        }
        else if(sarg=="-pf")
        {
            ++i;
            if(i>=argc)usage();
            pffile=string(argv[i]);
        }
        else if(sarg=="-text")
        {
            binary_data=false;
        }
        else if(sarg=="-v")
          SEISPP_verbose=true;
        else
            usage();
    }
    try{
        PfStyleMetadata control=pfread(pffile);
        std::shared_ptr<StreamObjectWriter<ThreeComponentEnsemble>> out;
        if(binary_data)
        {
          out=std::shared_ptr<StreamObjectWriter<ThreeComponentEnsemble>>
             (new StreamObjectWriter<ThreeComponentEnsemble>('b'));
        }
        else
        {
          out=std::shared_ptr<StreamObjectWriter<ThreeComponentEnsemble>>
             (new StreamObjectWriter<ThreeComponentEnsemble>);
        }
        bool add_noise=control.get_bool("add_noise");
        double sigma(0.0);
        if(add_noise) sigma=control.get<double>("gaussian_noise_sigma");
        ThreeComponentEnsemble d;
        if(use_pattern_file)
        {
          d=load_pattern_file(patternfile);
        }
        else
        {
          d=create_pattern_from_pf(control);
        }
        vector<Impulse3CDefinition> impulses=LoadImpulses(control);
        int nmembers=d.member.size();
        vector<Impulse3CDefinition>::iterator impptr;
        for(impptr=impulses.begin();impptr!=impulses.end();++impptr)
        {
          int i,j,k;
          i=impptr->member_number;
          j=d.member[i].sample_number(impptr->time);
          if((j>=0) && (j<d.member[i].ns))
          {
            for(k=0;k<3;++k)d.member[i].u(k,j)=impptr->A[k];
            if(SEISPP_verbose)
            {
                cerr << "Inserted vector=";
                for(k=0;k<3;++k) cerr<<impptr->A[k]<<" ";
                cerr <<"  at time="<<impptr->time <<" = sample number "
                    << j<<" in ensemble number number="<<i<<endl;
            }
          }
          else
          {
            cerr << "impulse_simulator:  Requested impulse at time="
              << impptr->time<<" is outside data range"<<endl
              << "Pattern seismogram has t0="<<d.member[i].t0
              << " with endtime="<<d.member[i].endtime()<<endl;
          }
        }
        if(add_noise) add_gaussian_noise(d,sigma);
        out->write(d);
    }catch(SeisppError& serr)
    {
        serr.log_error();
    }
    catch(std::exception& stexc)
    {
        cerr << stexc.what()<<endl;
    }
}


#include <boost/geometry/algorithms/simplify.hpp>


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph_filtering.hh"
#include "graph_python_interface.hh"

#include <boost/python.hpp>
#include <boost/graph/astar_search.hpp>

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_util.hh"

#include "graph_astar.hh"

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_astar_search
{
    template <class Graph, class DistanceMap>
    void operator()(Graph& g, size_t s, DistanceMap dist, pair<boost::any,
                    boost::any> pc, boost::any aweight, AStarVisitorWrapper vis,
                    pair<AStarCmp, AStarCmb> cmp, pair<python::object,
                    python::object> range, python::object h,
                    GraphInterface& gi) const
    {

        typedef typename property_traits<DistanceMap>::value_type dtype_t;
        dtype_t z = python::extract<dtype_t>(range.first);
        dtype_t i = python::extract<dtype_t>(range.second);

        checked_vector_property_map<default_color_type,
                                    decltype(get(vertex_index, g))>
            color(get(vertex_index, g));
        typedef typename property_map_type::
            apply<int64_t, decltype(get(vertex_index, g))>::type pred_t;
        typedef typename graph_traits<Graph>::edge_descriptor edge_t;
        DynamicPropertyMapWrap<dtype_t, edge_t> weight(aweight,
                                                       edge_properties());
        astar_search_no_init(g, vertex(s, g),
                             AStarH<Graph, dtype_t>(gi, g, h), vis,
                             any_cast<pred_t>(pc.first),
                             any_cast<DistanceMap>(pc.second), dist, weight,
                             color, get(vertex_index, g), cmp.first,
                             cmp.second, i, z);
    }
};

void a_star_search_implicit(GraphInterface& g, size_t source,
                            boost::any dist_map, boost::any pred,
                            boost::any cost, boost::any weight,
                            python::object vis, python::object cmp,
                            python::object cmb, python::object zero,
                            python::object inf, python::object h)
{
    run_action<graph_tool::all_graph_views,mpl::true_>()
        (g, std::bind(do_astar_search(), std::placeholders::_1, source,
                      std::placeholders::_2, make_pair(pred, cost),
                      weight,
                      AStarVisitorWrapper(g, vis),
                      make_pair(AStarCmp(cmp),
                                AStarCmb(cmb)),
                      make_pair(zero, inf), h, std::ref(g)),
         writable_vertex_properties())(dist_map);
}

void export_astar_implicit()
{
    using namespace boost::python;
    def("astar_search_implicit", &a_star_search_implicit);
}