/* \author Bastian Steder */

#include <iostream>

#include <boost/thread/thread.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/range_image_border_extractor.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType;

// --------------------
// -----Parameters-----
// --------------------
float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame =
    pcl::RangeImage::CAMERA_FRAME;
bool setUnseenToMaxRange = false;

// --------------
// -----Help-----
// --------------
void printUsage(const char *progName) {
    std::cout << "\n\nUsage: " << progName << " [options] <scene.pcd>\n\n"
              << "Options:\n"
              << "-------------------------------------------\n"
              << "-r <float>   angular resolution in degrees (default "
              << angular_resolution << ")\n"
              << "-c <int>     coordinate frame (default "
              << (int)coordinate_frame << ")\n"
              << "-m           Treat all unseen points to max range\n"
              << "-h           this help\n"
              << "\n\n";
}

// --------------
// -----Main-----
// --------------
int main(int argc, char **argv) {
    // --------------------------------------
    // -----Parse Command Line Arguments-----
    // --------------------------------------
    if (pcl::console::find_argument(argc, argv, "-h") >= 0) {
        printUsage(argv[0]);
        return 0;
    }
    if (pcl::console::find_argument(argc, argv, "-m") >= 0) {
        setUnseenToMaxRange = true;
        cout << "Setting unseen values in range image to maximum range "
                "readings.\n";
    }
    int tmp_coordinate_frame;
    if (pcl::console::parse(argc, argv, "-c", tmp_coordinate_frame) >= 0) {
        coordinate_frame =
            pcl::RangeImage::CoordinateFrame(tmp_coordinate_frame);
        cout << "Using coordinate frame " << (int)coordinate_frame << ".\n";
    }
    if (pcl::console::parse(argc, argv, "-r", angular_resolution) >= 0)
        cout << "Setting angular resolution to " << angular_resolution
             << "deg.\n";
    angular_resolution = pcl::deg2rad(angular_resolution);

    // ------------------------------------------------------------------
    // -----Read pcd file or create example point cloud if not given-----
    // ------------------------------------------------------------------
    pcl::PointCloud<PointType>::Ptr point_cloud_ptr(
        new pcl::PointCloud<PointType>);
    pcl::PointCloud<PointType> &point_cloud = *point_cloud_ptr;
    pcl::PointCloud<pcl::PointWithViewpoint> far_ranges;
    Eigen::Affine3f scene_sensor_pose(Eigen::Affine3f::Identity());
    std::vector<int> pcd_filename_indices =
        pcl::console::parse_file_extension_argument(argc, argv, "pcd");
    if (!pcd_filename_indices.empty()) {
        std::string filename = argv[pcd_filename_indices[0]];
        if (pcl::io::loadPCDFile(filename, point_cloud) == -1) {
            cout << "Was not able to open file \"" << filename << "\".\n";
            printUsage(argv[0]);
            return 0;
        }
        scene_sensor_pose =
            Eigen::Affine3f(Eigen::Translation3f(
                point_cloud.sensor_origin_[0], point_cloud.sensor_origin_[1],
                point_cloud.sensor_origin_[2])) *
            Eigen::Affine3f(point_cloud.sensor_orientation_);

        std::string far_ranges_filename =
            pcl::getFilenameWithoutExtension(filename) + "_far_ranges.pcd";
        if (pcl::io::loadPCDFile(far_ranges_filename.c_str(), far_ranges) == -1)
            std::cout << "Far ranges file \"" << far_ranges_filename
                      << "\" does not exists.\n";
    } else {
        cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
        for (float x = -0.5f; x <= 0.5f; x += 0.01f) {
            for (float y = -0.5f; y <= 0.5f; y += 0.01f) {
                PointType point;
                point.x = x;
                point.y = y;
                point.z = 2.0f - y;
                point_cloud.points.push_back(point);
            }
        }
        point_cloud.width = (int)point_cloud.points.size();
        point_cloud.height = 1;
    }

    // -----------------------------------------------
    // -----Create RangeImage from the PointCloud-----
    // -----------------------------------------------
    float noise_level = 0.0;
    float min_range = 0.0f;
    int border_size = 1;
    boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
    pcl::RangeImage &range_image = *range_image_ptr;
    range_image.createFromPointCloud(point_cloud, angular_resolution,
                                     pcl::deg2rad(360.0f), pcl::deg2rad(180.0f),
                                     scene_sensor_pose, coordinate_frame,
                                     noise_level, min_range, border_size);
    range_image.integrateFarRanges(far_ranges);
    if (setUnseenToMaxRange)
        range_image.setUnseenToMaxRange();

    // --------------------------------------------
    // -----Open 3D viewer and add point cloud-----
    // --------------------------------------------
    pcl::visualization::PCLVisualizer viewer("3D Viewer");
    viewer.setBackgroundColor(1, 1, 1);
    viewer.addCoordinateSystem(1.0f);
    pcl::visualization::PointCloudColorHandlerCustom<PointType>
        point_cloud_color_handler(point_cloud_ptr, 0, 0, 0);
    viewer.addPointCloud(point_cloud_ptr, point_cloud_color_handler,
                         "original point cloud");
    // PointCloudColorHandlerCustom<pcl::PointWithRange>
    // range_image_color_handler (range_image_ptr, 150, 150, 150);
    // viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range
    // image"); viewer.setPointCloudRenderingProperties
    // (PCL_VISUALIZER_POINT_SIZE, 2, "range image");

    // -------------------------
    // -----Extract borders-----
    // -------------------------
    pcl::RangeImageBorderExtractor border_extractor(&range_image);
    pcl::PointCloud<pcl::BorderDescription> border_descriptions;
    border_extractor.compute(border_descriptions);

    // ----------------------------------
    // -----Show points in 3D viewer-----
    // ----------------------------------
    pcl::PointCloud<pcl::PointWithRange>::Ptr border_points_ptr(
        new pcl::PointCloud<pcl::PointWithRange>),
        veil_points_ptr(new pcl::PointCloud<pcl::PointWithRange>),
        shadow_points_ptr(new pcl::PointCloud<pcl::PointWithRange>);
    pcl::PointCloud<pcl::PointWithRange> &border_points = *border_points_ptr,
                                         &veil_points = *veil_points_ptr,
                                         &shadow_points = *shadow_points_ptr;
    for (int y = 0; y < (int)range_image.height; ++y) {
        for (int x = 0; x < (int)range_image.width; ++x) {
            if (border_descriptions.points[y * range_image.width + x]
                    .traits[pcl::BORDER_TRAIT__OBSTACLE_BORDER])
                border_points.points.push_back(
                    range_image.points[y * range_image.width + x]);
            if (border_descriptions.points[y * range_image.width + x]
                    .traits[pcl::BORDER_TRAIT__VEIL_POINT])
                veil_points.points.push_back(
                    range_image.points[y * range_image.width + x]);
            if (border_descriptions.points[y * range_image.width + x]
                    .traits[pcl::BORDER_TRAIT__SHADOW_BORDER])
                shadow_points.points.push_back(
                    range_image.points[y * range_image.width + x]);
        }
    }
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>
        border_points_color_handler(border_points_ptr, 0, 255, 0);
    viewer.addPointCloud<pcl::PointWithRange>(
        border_points_ptr, border_points_color_handler, "border points");
    viewer.setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "border points");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>
        veil_points_color_handler(veil_points_ptr, 255, 0, 0);
    viewer.addPointCloud<pcl::PointWithRange>(
        veil_points_ptr, veil_points_color_handler, "veil points");
    viewer.setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "veil points");
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange>
        shadow_points_color_handler(shadow_points_ptr, 0, 255, 255);
    viewer.addPointCloud<pcl::PointWithRange>(
        shadow_points_ptr, shadow_points_color_handler, "shadow points");
    viewer.setPointCloudRenderingProperties(
        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "shadow points");

    //-------------------------------------
    // -----Show points on range image-----
    // ------------------------------------
    pcl::visualization::RangeImageVisualizer *range_image_borders_widget = NULL;
    range_image_borders_widget =
        pcl::visualization::RangeImageVisualizer::getRangeImageBordersWidget(
            range_image, -std::numeric_limits<float>::infinity(),
            std::numeric_limits<float>::infinity(), false, border_descriptions,
            "Range image with borders");
    // -------------------------------------

    //--------------------
    // -----Main loop-----
    //--------------------
    while (!viewer.wasStopped()) {
        range_image_borders_widget->spinOnce();
        viewer.spinOnce();
        pcl_sleep(0.01);
    }
}


// Author: Andrius Mikonis (andrius.mikonis@gmail.com)
// License: BSD
// Last modified: Feb 12, 2019

// Wrapper for most external modules
#include <boost/python.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <exception>

// Opencv includes
#include <opencv2/opencv.hpp>

// np_opencv_converter
#include "np_opencv_converter.hpp"

// fbow
#include "fbow.h"
#include "vocabulary_creator.h"

#include <boost/log/trivial.hpp>
#include <boost/log/expressions.hpp>

#include <list>

namespace py = boost::python;

void init_logging()
{
    boost::log::core::get()->set_filter
    (
        boost::log::trivial::severity >= boost::log::trivial::info
    );
}

std::string type2str(int type) {
  std::string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+'0');

  return r;
}

class Vocabulary
{
public:

    Vocabulary(int k = 10, int L = 6, int nthreads = 1, int maxIters = 0, bool verbose = true){
        init_logging();
        voc_creator_params.k = k;
        voc_creator_params.L = L;
        voc_creator_params.nthreads = nthreads;
        voc_creator_params.maxIters = maxIters;
        voc_creator_params.verbose = verbose;
        _verbose = verbose;
        voc = new fbow::Vocabulary();
        voc_creator = new fbow::VocabularyCreator();
    }

    ~Vocabulary() {
        if (_verbose) std::cout << "Entering destructor" << std::endl;
        delete voc;
        delete voc_creator;
        if (_verbose) std::cout << "Exiting destructor" << std::endl;
    }

    void create(const  std::vector<cv::Mat>  &training_feat_vec ) {
        BOOST_LOG_TRIVIAL(info)  << "Creating vocabulary with: k=" << voc_creator_params.k
                                 << ", L=" << voc_creator_params.L
                                 << ", nthreads= " << voc_creator_params.nthreads
                                 << ", maxIters= " << voc_creator_params.maxIters ;

        BOOST_LOG_TRIVIAL(debug) << "Descriptor vec length: " << training_feat_vec.size();
        BOOST_LOG_TRIVIAL(debug) << "Descriptor 0 shape : "
                                 << training_feat_vec[0].rows << " rows x " << training_feat_vec[0].cols
                                 << " cols , type: " << type2str(training_feat_vec[0].type());

        std::srand(0);
        voc_creator->create(*voc, training_feat_vec, std::string("orb"), voc_creator_params);

        BOOST_LOG_TRIVIAL(debug) << "Vocabulary Created" << std::endl;

    }

    void clear() {
        voc->clear();
        BOOST_LOG_TRIVIAL(debug) << "Vocabulary cleared: ";
    }

    void readFromFile(const std::string& path) {
        BOOST_LOG_TRIVIAL(debug) << "Started reading file: " << path;
        voc->readFromFile(path);
        BOOST_LOG_TRIVIAL(debug) << "Completed reading file: " << path;
    }

    void saveToFile(const std::string& path) {
        BOOST_LOG_TRIVIAL(debug) << "Started writing to file: " << path;
        voc->saveToFile(path);
        BOOST_LOG_TRIVIAL(debug) << "Completed Writing to file: " << path;
    }

    fbow::fBow transform(const cv::Mat & features) {
        fbow::fBow bow;
        bow = voc->transform(features);
        return bow;
    }

    py::list transform2(const cv::Mat & features, int level) {
        fbow::fBow bow;
        fbow::fBow2 bow2;
        voc->transform(features, level, bow, bow2);
        py::list t;
        t.append(bow);
        t.append(bow2);
        return t;
    }

    uint32_t getDescType() {
        return voc->getDescType();
    }

    uint32_t getDescSize() {
        return voc->getDescSize();
    }

    //py::str getDescName() {
    //    return py::extract<string>(voc->getDescName());
    //}

    uint32_t getK() {
        return voc->getK();
    }

    bool isValid() {
        return voc->isValid();
    }

    uint32_t size() {
        return voc->size();
    }

    uint64_t hash() {
        return voc->hash();
    }

    fbow::Vocabulary * voc;
    fbow::VocabularyCreator * voc_creator;
    fbow::VocabularyCreator::Params voc_creator_params;
    bool _verbose;
};

struct map_item
{
    typedef fbow::fBow Map;

    static float get(Map & self, const uint32_t idx) {
      if( self.find(idx) != self.end() ) return self[idx].var;
      PyErr_SetString(PyExc_KeyError,"Map key not found");
      py::throw_error_already_set();
    }

    //static void set(Map& self, const Key idx, const float val) { self[idx]=val; }

    //static void del(Map& self, const Key n) { self.erase(n); }

    static bool in(Map const& self, const uint32_t n) { return self.find(n) != self.end(); }

    static py::list keys(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append(it->first);
        return t;
    }
    static py::list values(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append(it->second.var);
        return t;
    }
    static py::list items(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append( py::make_tuple(it->first, it->second.var) );
        return t;
    }
};

struct map_item2
{
    typedef fbow::fBow2 Map;

    static py::list get(Map & self, const uint32_t idx) {
      if( self.find(idx) != self.end() ){
        py::list t;
        std::vector<uint32_t> vec = self[idx];
        for(std::vector<uint32_t>::const_iterator it=vec.begin(); it!=vec.end(); ++it)
            t.append(*it);
        return t;
      }
      PyErr_SetString(PyExc_KeyError,"Map key not found");
      py::throw_error_already_set();
    }

    //static void set(Map& self, const Key idx, const float val) { self[idx]=val; }

    //static void del(Map& self, const Key n) { self.erase(n); }

    static bool in(Map const& self, const uint32_t n) { return self.find(n) != self.end(); }

    static py::list keys(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append(it->first);
        return t;
    }
    static py::list values(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append(it->second.back());
        return t;
    }
    static py::list items(Map const& self)
    {
        py::list t;
        for(Map::const_iterator it=self.begin(); it!=self.end(); ++it)
            t.append( py::make_tuple(it->first, it->second.back()) );
        return t;
    }
};

// Wrap a few functions and classes for testing purposes
namespace fs {
    namespace python {

        BOOST_PYTHON_MODULE(pyfbow)
        {
            // Main types export
            fs::python::init_and_export_converters();
            py::scope scope = py::scope();

            // Class
            py::class_<fbow::fBow>("fBow")
                .def("__len__", &fbow::fBow::size)
                .def("__getitem__", &map_item().get)
                //.def("__setitem__", &map_item().set)
                //.def("__delitem__", &map_item().del)
                .def("clear", &fbow::fBow::clear)
                .def("__contains__", &map_item().in)
                .def("has_key", &map_item().in)
                .def("keys", &map_item().keys)
                .def("values", &map_item().values)
                .def("items", &map_item().items)
                .def("hash", &fbow::fBow::hash)
                .def("score", &fbow::fBow::score);

            py::class_<fbow::fBow2>("fBow2")
                .def("__len__", &fbow::fBow2::size)
                .def("__getitem__", &map_item2().get)
                //.def("__setitem__", &map_item().set)
                //.def("__delitem__", &map_item().del)
                .def("clear", &fbow::fBow2::clear)
                .def("__contains__", &map_item2().in)
                .def("has_key", &map_item2().in)
                .def("keys", &map_item2().keys)
                .def("values", &map_item2().values)
                .def("items", &map_item2().items)
                .def("hash", &fbow::fBow2::hash);

            py::class_<Vocabulary>("Vocabulary", "Vocabulary class")
                .def(py::init< py::optional<int, int, int, int, bool> >(
                    (py::arg("k") = 10, py::arg("L") = 5, py::arg("nthreads") = 1,
                     py::arg("maxIters") = 0, py::arg("verbose") = true )))
                .def("create", &Vocabulary::create,
                     (py::arg("No feat x descriptor length numpy array"),
                      "Creates the vocabulary from features"))
                .def("saveToFile", &Vocabulary::saveToFile,
                     (py::arg("Filename"),
                      "Save vocabulary to file"))
                .def("readFromFile", &Vocabulary::readFromFile,
                     (py::arg("Filename"),
                      "Read vocabulary from file"))
                .def("transform", &Vocabulary::transform,
                     (py::arg("Features"),
                      "Transform features to Bag fo Words"))
                .def("transform2", &Vocabulary::transform2,
                     (py::arg("Features"),
                      "Transform features to Bag fo Words"))
                .def("getDescSize", &Vocabulary::getDescSize)
                .def("getDescType", &Vocabulary::getDescType)
                //.def("getDescName", &Vocabulary::getDescName)
                .def("getK", &Vocabulary::getK)
                .def("isValid", &Vocabulary::isValid)
                .def("size", &Vocabulary::size)
                .def("clear", &Vocabulary::clear)
                .def("hash", &Vocabulary::hash);
        }

    } // namespace fs
} // namespace python


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file SubgraphPreconditioner.cpp
 * @date Dec 31, 2009
 * @author: Frank Dellaert
 */

#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/DSFVector.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/types.h>
#include <gtsam/base/Vector.h>

#include <boost/algorithm/string.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/shared_ptr.hpp>

#include <algorithm>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <list>
#include <map>
#include <numeric> // accumulate
#include <queue>
#include <set>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
  for(const GaussianFactor::shared_ptr &gf: gfg) {
    JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
    if( !jf ) {
      jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
    }
    result->push_back(jf);
  }
  return result;
}

/*****************************************************************************/
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {

  /* compute the sum of the weights */
  const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);

  /* make a normalized and accumulated version of the weight vector */
  const size_t m = weight.size();
  vector<double> w; w.reserve(m);
  for ( size_t i = 0 ; i < m ; ++i ) {
    w.push_back(weight[i]/sum);
  }

  vector<double> acc(m);
  std::partial_sum(w.begin(),w.end(),acc.begin());

  /* iid sample n times */
  vector<size_t> result; result.reserve(n);
  const double denominator = (double)RAND_MAX;
  for ( size_t i = 0 ; i < n ; ++i ) {
    const double value = rand() / denominator;
    /* binary search the interval containing "value" */
    vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
    size_t idx = it - acc.begin();
    result.push_back(idx);
  }
  return result;
}

/*****************************************************************************/
vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {

  const size_t m = weight.size();
  if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");

  vector<size_t> result;

  size_t count = 0;
  std::vector<bool> touched(m, false);
  while ( count < n ) {
    std::vector<size_t> localIndices; localIndices.reserve(n-count);
    std::vector<double> localWeights; localWeights.reserve(n-count);

    /* collect data */
    for ( size_t i = 0 ; i < m ; ++i ) {
      if ( !touched[i] ) {
        localIndices.push_back(i);
        localWeights.push_back(weight[i]);
      }
    }

    /* sampling and cache results */
    vector<size_t> samples = iidSampler(localWeights, n-count);
    for ( const size_t &id: samples ) {
      if ( touched[id] == false ) {
        touched[id] = true ;
        result.push_back(id);
        if ( ++count >= n ) break;
      }
    }
  }
  return result;
}

/****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) {
  edges_.reserve(indices.size());
  for ( const size_t &idx: indices ) {
    edges_.push_back(SubgraphEdge(idx, 1.0));
  }
}

/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
  std::vector<size_t> eid; eid.reserve(size());
  for ( const SubgraphEdge &edge: edges_ ) {
    eid.push_back(edge.index_);
  }
  return eid;
}

/****************************************************************************/
void Subgraph::save(const std::string &fn) const {
  std::ofstream os(fn.c_str());
  boost::archive::text_oarchive oa(os);
  oa << *this;
  os.close();
}

/****************************************************************************/
Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
  std::ifstream is(fn.c_str());
  boost::archive::text_iarchive ia(is);
  Subgraph::shared_ptr subgraph(new Subgraph());
  ia >> *subgraph;
  is.close();
  return subgraph;
}

/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
  if ( edge.weight() != 1.0 )
    os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
  else
    os << edge.index() ;
  return os;
}

/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
  os << "Subgraph" << endl;
  for ( const SubgraphEdge &e: subgraph.edges() ) {
    os << e << ", " ;
  }
  return os;
}

/*****************************************************************************/
void SubgraphBuilderParameters::print() const {
  print(cout);
}

/***************************************************************************************/
void SubgraphBuilderParameters::print(ostream &os) const {
  os << "SubgraphBuilderParameters" << endl
      << "skeleton:            " << skeletonTranslator(skeleton_) << endl
      << "skeleton weight:     " << skeletonWeightTranslator(skeletonWeight_) << endl
      << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
      ;
}

/*****************************************************************************/
ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
  p.print(os);
  return os;
}

/*****************************************************************************/
SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
  std::string s = src;  boost::algorithm::to_upper(s);
  if (s == "NATURALCHAIN")    return NATURALCHAIN;
  else if (s == "BFS")        return BFS;
  else if (s == "KRUSKAL")    return KRUSKAL;
  throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
  return KRUSKAL;
}

/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
  if ( w == NATURALCHAIN )return "NATURALCHAIN";
  else if ( w == BFS )    return "BFS";
  else if ( w == KRUSKAL )return "KRUSKAL";
  else                    return "UNKNOWN";
}

/****************************************************************/
SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
  std::string s = src;  boost::algorithm::to_upper(s);
  if (s == "EQUAL")       return EQUAL;
  else if (s == "RHS")    return RHS_2NORM;
  else if (s == "LHS")    return LHS_FNORM;
  else if (s == "RANDOM") return RANDOM;
  throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
  return EQUAL;
}

/****************************************************************/
std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
  if ( w == EQUAL )           return "EQUAL";
  else if ( w == RHS_2NORM )  return "RHS";
  else if ( w == LHS_FNORM )  return "LHS";
  else if ( w == RANDOM )     return "RANDOM";
  else                        return "UNKNOWN";
}

/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
  std::string s = src;  boost::algorithm::to_upper(s);
  if (s == "SKELETON")      return SKELETON;
//  else if (s == "STRETCH")  return STRETCH;
//  else if (s == "GENERALIZED_STRETCH")  return GENERALIZED_STRETCH;
  throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
  return SKELETON;
}

/****************************************************************/
std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
  if ( w == SKELETON )                  return "SKELETON";
//  else if ( w == STRETCH )              return "STRETCH";
//  else if ( w == GENERALIZED_STRETCH )  return "GENERALIZED_STRETCH";
  else                                  return "UNKNOWN";
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
  const SubgraphBuilderParameters &p = parameters_;
  switch (p.skeleton_) {
  case SubgraphBuilderParameters::NATURALCHAIN:
    return natural_chain(gfg);
    break;
  case SubgraphBuilderParameters::BFS:
    return bfs(gfg);
    break;
  case SubgraphBuilderParameters::KRUSKAL:
    return kruskal(gfg, ordering, w);
    break;
  default:
    cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
    break;
  }
  return vector<size_t>();
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
  std::vector<size_t> result ;
  size_t idx = 0;
  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
    if ( gf->size() == 1 ) {
      result.push_back(idx);
    }
    idx++;
  }
  return result;
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
  std::vector<size_t> result ;
  size_t idx = 0;
  for ( const GaussianFactor::shared_ptr &gf: gfg ) {
    if ( gf->size() == 2 ) {
      const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
      if ( (k1-k0) == 1 || (k0-k1) == 1 )
        result.push_back(idx);
    }
    idx++;
  }
  return result;
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
  const VariableIndex variableIndex(gfg);
  /* start from the first key of the first factor */
  Key seed = gfg[0]->keys()[0];

  const size_t n = variableIndex.size();

  /* each vertex has self as the predecessor */
  std::vector<size_t> result;
  result.reserve(n-1);

  /* Initialize */
  std::queue<size_t> q;
  q.push(seed);

  std::set<size_t> flags;
  flags.insert(seed);

  /* traversal */
  while ( !q.empty() ) {
    const size_t head = q.front(); q.pop();
    for ( const size_t id: variableIndex[head] ) {
      const GaussianFactor &gf = *gfg[id];
      for ( const size_t key: gf.keys() ) {
        if ( flags.count(key) == 0 ) {
          q.push(key);
          flags.insert(key);
          result.push_back(id);
        }
      }
    }
  }
  return result;
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
  const VariableIndex variableIndex(gfg);
  const size_t n = variableIndex.size();
  const vector<size_t> idx = sort_idx(w) ;

  /* initialize buffer */
  vector<size_t> result;
  result.reserve(n-1);

  // container for acsendingly sorted edges
  DSFVector D(n) ;

  size_t count = 0 ; double sum = 0.0 ;
  for (const size_t id: idx) {
    const GaussianFactor &gf = *gfg[id];
    if ( gf.keys().size() != 2 ) continue;
    const size_t u = ordering.find(gf.keys()[0])->second,
                 u_root = D.find(u),
                 v = ordering.find(gf.keys()[1])->second,
                 v_root = D.find(v) ;
    if ( u_root != v_root ) {
      D.merge(u_root, v_root) ;
      result.push_back(id) ;
      sum += w[id] ;
      if ( ++count == n-1 ) break ;
    }
  }
  return result;
}

/****************************************************************/
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
  return uniqueSampler(weights, t);
}

/****************************************************************/
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {

  const SubgraphBuilderParameters &p = parameters_;
  const Ordering inverse_ordering = Ordering::Natural(gfg);
  const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
  const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;

  vector<double> w = weights(gfg);
  const vector<size_t> tree = buildTree(gfg, forward_ordering, w);

  /* sanity check */
  if ( tree.size() != n-1 ) {
    throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
  }

  /* down weight the tree edges to zero */
  for ( const size_t id: tree ) {
    w[id] = 0.0;
  }

  /* decide how many edges to augment */
  std::vector<size_t> offTree = sample(w, t);

  vector<size_t> subgraph = unary(gfg);
  subgraph.insert(subgraph.end(), tree.begin(), tree.end());
  subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());

  return boost::make_shared<Subgraph>(subgraph);
}

/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
{
  const size_t m = gfg.size() ;
  Weights weight; weight.reserve(m);

  for(const GaussianFactor::shared_ptr &gf: gfg ) {
    switch ( parameters_.skeletonWeight_ ) {
    case SubgraphBuilderParameters::EQUAL:
      weight.push_back(1.0);
      break;
    case SubgraphBuilderParameters::RHS_2NORM:
    {
      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
        weight.push_back(jf->getb().norm());
      }
      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
        weight.push_back(hf->linearTerm().norm());
      }
    }
      break;
    case SubgraphBuilderParameters::LHS_FNORM:
    {
      if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
        weight.push_back(std::sqrt(jf->getA().squaredNorm()));
      }
      else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
        weight.push_back(std::sqrt(hf->information().squaredNorm()));
      }
    }
      break;

    case SubgraphBuilderParameters::RANDOM:
      weight.push_back(std::rand()%100 + 1.0);
      break;

    default:
      throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
      break;
    }
  }
  return weight;
}

/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
         parameters_(p) {}

/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
    const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
        Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
        b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
}

/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
  return *xbar_ + Rc1_->backSubstitute(y);
}

/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
  Errors e(y);
  VectorValues x = this->x(y);
  Errors e2 = Ab2()->gaussianErrors(x);
  return 0.5 * (dot(e, e) + dot(e2,e2));
}

/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
  VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
  Errors e = (*Ab2()*x - *b2bar());               /* (A2*inv(R1)*y-b2bar) */
  VectorValues v = VectorValues::Zero(x);
  Ab2()->transposeMultiplyAdd(1.0, e, v);           /* A2'*(A2*inv(R1)*y-b2bar) */
  return y + Rc1()->backSubstituteTranspose(v);
}

/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {

  Errors e(y);
  VectorValues x = Rc1()->backSubstitute(y);   /* x=inv(R1)*y */
  Errors e2 = *Ab2() * x;                              /* A2*x */
  e.splice(e.end(), e2);
  return e;
}

/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {

  Errors::iterator ei = e.begin();
  for (size_t i = 0; i < y.size(); ++i, ++ei)
    *ei = y[i];

  // Add A2 contribution
  VectorValues x = Rc1()->backSubstitute(y);      // x=inv(R1)*y
  Ab2()->multiplyInPlace(x, ei);                  // use iterator version
}

/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {

  Errors::const_iterator it = e.begin();
  VectorValues y = zero();
  for (size_t i = 0; i < y.size(); ++i, ++it)
    y[i] = *it;
  transposeMultiplyAdd2(1.0, it, e.end(), y);
  return y;
}

/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {

  Errors::const_iterator it = e.begin();
  for (size_t i = 0; i < y.size(); ++i, ++it) {
    const Vector& ei = *it;
    axpy(alpha, ei, y[i]);
  }
  transposeMultiplyAdd2(alpha, it, e.end(), y);
}

/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
    Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {

  // create e2 with what's left of e
  // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
  Errors e2;
  while (it != end) e2.push_back(*(it++));

  VectorValues x = VectorValues::Zero(y); // x = 0
  Ab2_->transposeMultiplyAdd(1.0,e2,x);   // x += A2'*e2
  axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
}

/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
  cout << s << endl;
  Ab2_->print();
}

/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
{
  /* copy first */
  std::copy(y.data(), y.data() + y.rows(), x.data());

  /* in place back substitute */
  for (auto cg: boost::adaptors::reverse(*Rc1_)) {
    /* collect a subvector of x that consists of the parents of cg (S) */
    const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));

    /* compute the solution for the current pivot */
    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);

    /* assign subvector of sol to the frontal variables */
    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
  }
}

/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
{
  /* copy first */
  std::copy(y.data(), y.data() + y.rows(), x.data());

  /* in place back substitute */
  for(const boost::shared_ptr<GaussianConditional> & cg: *Rc1_) {
    const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
//    const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
    const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);

    // Check for indeterminant solution
    if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());

    /* assign subvector of sol to the frontal variables */
    setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);

    /* substract from parent variables */
    for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
      KeyInfo::const_iterator it2 = keyInfo_.find(*it);
      Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
      rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
    }
  }
}

/*****************************************************************************/
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
  /* identify the subgraph structure */
  const SubgraphBuilder builder(parameters_.builderParams_);
  Subgraph::shared_ptr subgraph = builder(gfg);

  keyInfo_ = keyInfo;

  /* build factor subgraph */
  GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);

  /* factorize and cache BayesNet */
  Rc1_ = gfg_subgraph->eliminateSequential();
}

/*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {

  /* a cache of starting index and dim */
  typedef vector<pair<size_t, size_t> > Cache;
  Cache cache;

  /* figure out dimension by traversing the keys */
  size_t d = 0;
  for ( const Key &key: keys ) {
    const KeyInfoEntry &entry = keyInfo.find(key)->second;
    cache.push_back(make_pair(entry.colstart(), entry.dim()));
    d += entry.dim();
  }

  /* use the cache to fill the result */
  Vector result = Vector::Zero(d, 1);
  size_t idx = 0;
  for ( const Cache::value_type &p: cache ) {
    result.segment(idx, p.second) = src.segment(p.first, p.second) ;
    idx += p.second;
  }

  return result;
}

/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
  /* use the cache  */
  size_t idx = 0;
  for ( const Key &key: keys ) {
    const KeyInfoEntry &entry = keyInfo.find(key)->second;
    dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
    idx += entry.dim();
  }
}

/*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {

  GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
  result->reserve(subgraph.size());
  for ( const SubgraphEdge &e: subgraph ) {
    const size_t idx = e.index();
    if ( clone ) result->push_back(gfg[idx]->clone());
    else result->push_back(gfg[idx]);
  }
  return result;
}

} // nsamespace gtsam


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

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
   This function object returns @c true if at least one element of the
   argument is non zero.

    @par Header <boost/simd/function/any.hpp>

    @see all, none, nbtrue

    @par Example:

      @snippet any.cpp any

    @par Possible output:

      @snippet any.txt any

  **/
  bool any(Value const &x);
} }
#endif

#include <boost/simd/function/scalar/any.hpp>
#include <boost/simd/function/simd/any.hpp>

#endif


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2021 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020-2021 Nikita Kaskov <nbering@nil.foundation>
//
// MIT License
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE sha256_component_test

#include <chrono>

#include <boost/test/unit_test.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>
#include <nil/crypto3/algebra/curves/mnt4.hpp>
#include <nil/crypto3/algebra/curves/mnt6.hpp>
#include <nil/crypto3/algebra/curves/edwards.hpp>

#include "sha256.hpp"

using namespace nil::crypto3::algebra;
using namespace nil::crypto3;
using namespace nil::crypto3::zk;

BOOST_AUTO_TEST_SUITE(sha2_256_component_test_suite)
BOOST_AUTO_TEST_CASE(sha256_component_test_bls12_381_case) {
    std::cout << "Starting SHA256 component test for BLS12-381 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    sha2_two_to_one_bp<typename curves::bls12<381>::scalar_field_type>();
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "SHA256 component test for BLS12-381 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(sha256_component_test_mnt4_case) {
    std::cout << "Starting SHA256 component test for MNT4-298 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    sha2_two_to_one_bp<typename curves::mnt4<298>::scalar_field_type>();
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "SHA256 component test for MNT4-298 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(sha256_component_test_mnt6_case) {
    std::cout << "Starting SHA256 component test for MNT6-298 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    sha2_two_to_one_bp<typename curves::mnt6<298>::scalar_field_type>();
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "SHA256 component test for MNT6-298 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_CASE(sha256_component_test_edwards_183_case) {
    std::cout << "Starting SHA256 component test for Edwards-183 ..." << std::endl;
    auto begin = std::chrono::high_resolution_clock::now();
    sha2_two_to_one_bp<typename curves::edwards<183>::scalar_field_type>();
    auto end = std::chrono::high_resolution_clock::now();
    auto elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin);
    std::cout << "SHA256 component test for Edwards-183 finished, time: " << elapsed.count() * 1e-9 << std::endl;
}

BOOST_AUTO_TEST_SUITE_END()


//
//  main.cpp

#include <iostream>
#include "Detector.h"
#include <string>
#include <vector>
#include "Response.h"
#include "CalcCoordinator.h"
#include <boost/program_options.hpp>
#include <boost/regex.hpp>
#include <exception>
#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include "ConsolePrint.hpp"
#include "Simulation.hpp"

namespace po = boost::program_options;

using namespace std;

class

AngularResponse parseAngularData(std::vector<std::string> angularData, double bkg, bool &uncertainty);
DistResponse parseDistData(std::vector<std::string> distData, double bkg, double activity, double activity_uncertinaty, bool curve_fit, bool &uncertainty);
void parseActivity(std::string actString, double &activity, double &actUncert, bool &uncertainty);
void parseBkg(std::string bkgString, double &pulses, double &livetime, bool &uncertainty);
void parseActVsTime(std::string inString, double &start_time, double &stop_time, unsigned int &steps);

int main(int argc, const char * argv[])
{
	po::options_description generic("Generic arguments");
	generic.add_options()
	("help", "Show this help message")
	("verbose", "Print calculation progress and debug messages.")
	("config_file", po::value<std::string>(), "Retrive input parameters from a config file.");
	po::options_description conf("Configuration");
	conf.add_options()
	("fph", po::value<double>()->default_value(1.0), "Acceptable number of false positives per hour. Must be > 0. Default value is 1.")
	("beta", po::value<double>()->default_value(1/20), "Probability of false negative. Must be a value > 0 and < 1. Default value is 0.05.")
	("uncertainty", po::value<bool>()->default_value(false), "Perform uncertainty calculation. Possible values are 'true' and 'false'. Default is 'false'.")
	("uncertainty_loops", po::value<int>()->default_value(100), "Number of loops in uncertainty calculation. Default value is 100.")
	("calc_type", po::value<std::vector<std::string> >()->multitoken(), "Type of calculation to perform. Possible values are: 'mean', 'best', 'worst' and 'list'. Accepts multiple options. Defaults to 'mean'.")
	("output", po::value<string>(), "If argument is given, will output to given file in JSON-format. If not; outputs results to screen.")
	("distance", po::value<std::vector<double> >()->multitoken(), "The distance at which the source is placed in metres. Must be > 0. Accepts multiple values. Default value is 10.")
	("velocity", po::value<std::vector<double> >()->multitoken(), "The velocity of the detector in metres per second. Must be > 0. Accepts multiple values. Default value is 8.33 (30 kph).")
	("background", po::value<std::vector<double> >()->multitoken(), "The simulated background (in cps). Must be > 0. Accepts multiple values.")
	("cal_act", po::value<std::string>(), "The activity of the source used for calibration. Can be in the form of a single value or \"activity:uncertainty\". The uncertainty must be given in percent.")
	("cal_bkg", po::value<std::string>(), "The background of the detector in the environment where calibration measurements were done. Can be in the form of a single cps value or \"pulses:livetime\".")
	("dist_response", po::value<std::vector<std::string> >()->multitoken(), "Detector response input. Can be in one of two formats: \"distance:pulses:time\" or \"distance:cps\" where \"distance\" is the distance between source and detector in metres, \"pulses\" is the number of pulses reigstered by the detector, \"time\" is the live time of the measurement, and \"cps\" is the number of pulses per second. Accepts multiple values. See manual and examples for more information.")
	("dist_model", po::value<std::string>()->default_value("mean"), "How the distance response function is calculated. Possible values are \"least_sq\" and \"mean\". Defaults to \"mean\".")
	("ang_response", po::value<std::vector<std::string> >()->multitoken(), "Detector response input. Can be in one of two formats: \"angle:pulses:time\" or \"angle:cps\" where \"angle\" is in radians, \"pulses\" is the number of pulses reigstered by the detector, \"time\" is the live time of the measurement, and \"cps\" is the number of pulses per second. Accepts multiple values. Defaults to a angular response function of 1.0 over all angles; See manual and examples for more information.")
	("curve_limit", po::value<double>()->default_value(0.01), "When performing calculations of type 'best', 'mean' or 'worst'; for how long should the dynamic part of the intensity function be followed? See manual and examples for more information. Must be < 1.0 and > 0.0. Defaults to 0.01 (1% of peak value).")
	("mean_iters", po::value<unsigned int>()->default_value(100), "When performing calculations of type 'mean'; how many different time alignments should be tested in order to calculate the mean value? Defaults to 100.")
	("act_vs_time", po::value<std::string>(), "Create detection limit as a function of integration time data. Arguments must be of the form: \"start_time:stop_time:steps\". If this option is used, use of the \"output\" argument is strongly recommended. See manual and example files for more information.")
	("integration_time", po::value<std::vector<double> >()->multitoken(), "Set a fixed integration time and calculate minimum detectable activity from that parameter. Can take multiple values. Do not use together with the \"act_vs_time\" argument. Must be greater than 0.")
	("sim_iters", po::value<unsigned int>()->default_value(500000), "When performing calculations on list mode measurements, how many iterations should be used to calculate the probability of detecting a source. Default value is 20000.")
	("mt_sim", po::value<bool>()->default_value(false), "When performing calculations on list mode measurements, use multithreading. This option should not be used when using multiple calculation parameters as the number of threads can then be to great. Default value is false.")
	;

	po::options_description cmd_line;
	cmd_line.add(generic).add(conf);

	po::options_description conf_file;
	conf_file.add(conf);

	po::variables_map vm;
	bool enableUncertainty = true;

	try {

		po::store(po::parse_command_line(argc, argv, cmd_line), vm);
		po::notify(vm);
	} catch (po::error e) {
		cout << "Unknown or invalid command line option. Try the \"--help\" option for a list of commands." << endl;

		return 0;
	}

	if (vm.count("config_file") > 0) {
		std::string fileName = vm["config_file"].as<std::string>();
		if (!boost::filesystem::exists(fileName)) {
			cout << "Unable to open config file, it does not exist." << endl;
			return 0;
		}
		ifstream ifs(fileName.c_str());
		if(!ifs) {
			cout << "Unable to open config file." << endl;
			return 0;
		} else {
			try {
				po::store(parse_config_file(ifs, conf_file), vm);
			} catch (boost::program_options::invalid_config_file_syntax e) {
				cout << "Unable to parse config file: \"" << e.what() << "\"" << endl;
				return 0;
			}
			po::notify(vm);
		}
	}

	if (vm.count("help") or argc == 1) {
		cout << cmd_line << "\n";
		return 0;
	}

	double fph = vm["fph"].as<double>();
	if (fph <= 0) {
		cout << "Number of false positives per hour (fph) must be more than 0." << endl;
		return 0;
	}

	double beta = vm["beta"].as<double>();
	if (beta <= 0 or beta >= 1.0) {
		cout << "Probability of false negative (beta) must fall within the range 0.0 < beta < 1.0." << endl;
		return 0;
	}

	std::vector<Detector::CalcType> cTypes;

	if (vm.count("calc_type") == 0) {
		cTypes.push_back(Detector::CalcType::MEAN);
	} else {
		std::vector<std::string> calcs = vm["calc_type"].as<std::vector<std::string>>();
		for (int y = 0; y < calcs.size(); y++) {
			if (calcs[y] == std::string("mean")) {
				cTypes.push_back(Detector::CalcType::MEAN);
			} else if (calcs[y] == std::string("worst")) {
				cTypes.push_back(Detector::CalcType::WORST);
			} else if (calcs[y] == std::string("best")) {
				cTypes.push_back(Detector::CalcType::BEST);
			} else if (calcs[y] == std::string("list")) {
				cTypes.push_back(Detector::CalcType::LIST_MODE);
			} else {
				cout << "Unknown calculation type \"" << calcs[y] << "\"." << endl;
				return 0;
			}
		}
		if (cTypes.size() == 0) {
			cout << "One or more type of calculation must be used." << endl;
			return 0;
		}
	}

	std::string output_file;
	if (vm.count("output") == 1) {
		output_file = vm["output"].as<std::string>();
	}

	std::vector<double> dists;
	if (vm.count("distance")) {
		dists = vm["distance"].as<vector<double> >();
	} else {
		dists.push_back(10.0);
	}
	for (int i = 0; i < dists.size(); i++) {
		if (dists[i] <= 0.0){
			cout << "Distances can not be negative." << endl;
			return 0;
		}
	}

	std::vector<double> velocities;
	if (vm.count("velocity")) {
		velocities = vm["velocity"].as<vector<double> >();
	} else {
		velocities.push_back(8.33);
	}
	for (int o = 0; o < velocities.size(); o++) {
		if (velocities[o] <= 0.0){
			cout << "Velocities can not be negative." << endl;
			return 0;
		}
	}

	double bkg_pulses, bkg_livetime;
	if (vm.count("cal_bkg") == 0) {
		cout << "Calibration background data is needed to run the simulation." << endl;
		return 0;
	} else {
		try {
			parseBkg(vm["cal_bkg"].as<std::string>(), bkg_pulses, bkg_livetime, enableUncertainty);
		} catch (std::runtime_error e) {
			cout << "Error when parsing calibration background data: \"" << e.what() << "\"" << endl;
			return 0;
		}
	}
	BkgResponse calBkg(bkg_pulses, bkg_livetime);

	double act, act_uncert;
	if (vm.count("cal_act") == 0) {
		cout << "Calibration source activity data is needed to run the simulation." << endl;
		return 0;
	} else {
		try {
			parseActivity(vm["cal_act"].as<std::string>(), act, act_uncert, enableUncertainty);
		} catch (std::runtime_error e) {
			cout << "Error when parsing activity data: \"" << e.what() << "\"" << endl;
			return 0;
		}
	}

	bool dist_fit_curve = false;
	if (vm.count("dist_model") != 0) {
		std::string distModel = vm["dist_model"].as<std::string>();
		if (std::string("mean") == distModel) {
			dist_fit_curve = false;
		} else if (std::string("least_sq") == distModel) {
			dist_fit_curve = true;
		} else {
			cout << "Error when reading distance model type. The model is unknown." << endl;
			return 0;
		}
	}

	DistResponse distResp;
	std::vector<std::string> distData;
	if (vm.count("dist_response") == 0) {
		cout << "Distance response data is needed to run the simulation." << endl;
		return 0;
	} else {
		distData	= vm["dist_response"].as<std::vector<std::string> >();
		try {
			distResp = parseDistData(distData, bkg_pulses / bkg_livetime, act, act_uncert, dist_fit_curve, enableUncertainty);
		} catch (std::runtime_error e) {
			cout << "Error when parsing distance data: \"" << e.what() << "\"" << endl;
			return 0;
		}
	}

	AngularResponse angResp;
	std::vector<std::string> angData;
	if (vm.count("ang_response") == 0) {

	} else {
		angData = vm["ang_response"].as<std::vector<std::string> >();
		try {
			angResp = parseAngularData(angData, bkg_pulses / bkg_livetime, enableUncertainty); //Fix me, bkg!
		} catch (std::runtime_error e) {
			cout << "Error when parsing angular data: \"" << e.what() << "\"" << endl;
			return 0;
		}
	}

	std::vector<double> background;
	if (vm.count("background") == 0) {
		cout << "One or more background count rates are needed to run the simulation." << endl;
		return 0;
	} else {
		background = vm["background"].as<std::vector<double> >();
		for (int i = 0; i < background.size(); i++) {
			if (background[i] <= 0.0) {
				cout << "Simulated background can not be equal to or less than zero." << endl;
				return 0;
			}
		}
	}

	bool uncert_calc = vm["uncertainty"].as<bool>();
	if (uncert_calc and !enableUncertainty) {
		cout << "Unable to calculate uncertainty as data needed to do so is not provided." << endl;
		return 0;
	}
	int uncertainty_loops = vm["uncertainty_loops"].as<int>();
	if (uncert_calc and uncertainty_loops < 2) {
		cout << "The number of uncertainty loops can not be less than 2." << endl;
		return 0;
	}
	if (!uncert_calc) {
		uncertainty_loops = 0;
	}

	double curve_limit = vm["curve_limit"].as<double>();
	if (curve_limit <= 0 or curve_limit >= 1.0) {
		cout << "The curve limit must be > 0 and < 1.0." << endl;
		return 0;
	}

	unsigned int mean_iters = vm["mean_iters"].as<unsigned int>();
	if (mean_iters == 0) {
		cout << "The number of iterations for calculating the mean must be greater than 0." << endl;
		return 0;
	}
	bool plotCalc = false;
	double start_time = 0, stop_time = 0;
	unsigned int steps = 0;
	if (vm.count("act_vs_time") == 1) {
		plotCalc = true;
		std::string actVsTimeData = vm["act_vs_time"].as<std::string>();
		try {
			parseActVsTime(actVsTimeData, start_time, stop_time, steps);
		} catch (std::runtime_error e) {
			cout << "Error when parsing activity vs time data: \"" << e.what() << "\"" << endl;
			return 0;
		}
	}

	bool fixedInt = false;
	std::vector<double> fixedIntTimes;
	if (vm.count("integration_time") > 0 and not plotCalc) {
		fixedInt = true;
		fixedIntTimes = vm["integration_time"].as<std::vector<double>>();
		for (int j = 0; j < fixedIntTimes.size(); j++) {
			if (fixedIntTimes[j] <= 0) {
				std::cout << "Integration time must be greater than 0." << std::endl;
				return 0;
			}
		}
	} else if (vm.count("integration_time") == 1 and plotCalc) {
		cout << "Can not use argument \"integration_time\" and \"act_vs_time\". Ignoring \"integration_time\"." << endl;
	}

	unsigned int sim_iters = vm["sim_iters"].as<unsigned int>();
	if (mean_iters == 0) {
		cout << "The number of simulation iterations must be greater than 0." << endl;
		return 0;
	}

	if (vm.count("verbose") > 0) {
		ConsolePrint::Init(true);
	} else {
		ConsolePrint::Init(false);
	}

	OutputResult::OutputType outDev = OutputResult::OutputType::SCREEN;
	if (output_file.size() != 0) {
		outDev = OutputResult::OutputType::JSON_FILE;
	}

	bool mt = vm["mt_sim"].as<bool>();

	boost::shared_ptr<ListModeSimulator> simulator(new ListModeSimulator(sim_iters, mt));

	CalcCoordinator calc(outDev, output_file);
	Detector det(calBkg, distResp, angResp, curve_limit, mean_iters, sim_iters, simulator);

	for (int a = 0; a < dists.size(); a++) {
		det.SetDistance(dists[a]);
		for (int b = 0; b < velocities.size(); b++) {
			det.SetVelocity(velocities[b]);
			for (int c = 0; c < background.size(); c++) {
				det.SetSimBkg(background[c]);
				for (int d = 0; d < cTypes.size(); d++) {
					if (plotCalc) {
						calc.AddPlotCalculation(det, cTypes[d], fph, beta, start_time, stop_time, steps);
					} else if (fixedInt) {
						for (int e = 0; e < fixedIntTimes.size(); e++) {
							calc.AddFixedCalculation(det, cTypes[d], fph, beta, fixedIntTimes[e], uncertainty_loops);
						}
					} else {
						calc.AddCalculation(det, cTypes[d], fph, beta, uncertainty_loops);
					}
				}
			}
		}
	}

	calc.RunCalculations();
	simulator.get()->CloseThreads();
	ConsolePrint::Close();
	return 0;
}

void parseActVsTime(std::string inString, double &start_time, double &stop_time, unsigned int &steps) {
	boost::regex expr{"^(\\d+\\.?\\d*):(\\d+\\.?\\d*):(\\d+)$"};
	boost::smatch res;
	bool match = boost::regex_match(inString, res, expr);
	if (!match) {
		throw std::runtime_error(std::string("Incorrectly formated activity vs time parameters."));
	}
	start_time = lexical_cast<double>(res[1]);
	stop_time = lexical_cast<double>(res[2]);
	steps = lexical_cast<unsigned int>(res[3]);
	if (start_time >= stop_time) {
		throw std::runtime_error(std::string("Start time can not be equal to or greater than stop time."));
	}
}

void parseActivity(std::string actString, double &activity, double &actUncert, bool &uncertainty) {
	boost::regex expr{"^(\\d+\\.?\\d*)(?::(\\d+\\.?\\d*))?$"};
	boost::smatch res;
	bool match = boost::regex_match(actString, res, expr);
	if (!match) {
		throw std::runtime_error(std::string("Incorrectly formated calibration source activity data."));
	}
	activity = lexical_cast<double>(res[1]);
	if (activity <= 0) {
		throw std::runtime_error(std::string("Calibration source activity can not be less than or equal to zero."));
	}
	if (res[2].length() != 0) {
		actUncert = lexical_cast<double>(res[2]);
		if (actUncert < 0) {
			throw std::runtime_error(std::string("Calibration source activity uncertainty can not be less than zero."));
		}
	} else {
		actUncert = 0.0;
		uncertainty = false;
	}
}

void parseBkg(std::string bkgString, double &pulses, double &livetime, bool &uncertainty) {
	boost::regex expr{"^(\\d+\\.?\\d*)(?::(\\d+\\.?\\d*))?$"};
	boost::smatch res;
	bool match = boost::regex_match(bkgString, res, expr);
	if (!match) {
		throw std::runtime_error(std::string("Incorrectly formated calibration background data."));
	}
	pulses = lexical_cast<double>(res[1]);
	if (pulses < 0) {
		throw std::runtime_error(std::string("Background can not be less than zero."));
	}
	if (res[2].length() != 0) {
		livetime = lexical_cast<double>(res[2]);
		if (livetime <= 0) {
			throw std::runtime_error(std::string("Background live time can not be less than or equal to zero."));
		}
	} else {
		livetime = 1.0;
		uncertainty = false;
	}
}

AngularResponse parseAngularData(std::vector<std::string> angularData, double bkg, bool &uncertainty) {
	boost::regex expr{"^(\\d+\\.?\\d*):(\\d+\\.?\\d*)(?::(\\d+\\.?\\d*))?$"};
	boost::smatch res;
	std::vector<double> livetime;
	std::vector<double> pulses;
	std::vector<double> angle;
	for (int y = 0; y < angularData.size(); y++) {
		bool match = boost::regex_match(angularData[y], res, expr);
		if (!match) {
			throw std::runtime_error(std::string("Incorrectly formated angular data."));
		}
		angle.push_back(lexical_cast<double>(res[1]));
		pulses.push_back(lexical_cast<double>(res[2]));
		if (res[3].length() != 0) {
			livetime.push_back(lexical_cast<double>(res[3]));
		} else {
			livetime.push_back(1.0);
			uncertainty = false;
		}
		if (angle[angle.size() - 1] < 0 or angle[angle.size() - 1] > (pi / 1.99)) { //Fix me: should be 2 but using 1.99 for UI reasons
			throw std::runtime_error(std::string("Angular value does not fall within the range 0.0 to pi / 2."));
		}
		if (pulses[pulses.size() - 1] <= 0) {
			throw std::runtime_error(std::string("Number of pulses can not be zero or negative."));
		}

		if (livetime[livetime.size() - 1] <= 0) {
			throw std::runtime_error(std::string("Live time can not be zero or negative."));
		}
	}
	return AngularResponse(pulses, livetime, angle, bkg);
}

DistResponse parseDistData(std::vector<std::string> distData, double bkg, double activity, double activity_uncertinaty, bool curve_fit, bool &uncertainty) {
	boost::regex expr{"^(\\d+\\.?\\d*):(\\d+\\.?\\d*)(?::(\\d+\\.?\\d*))?$"};
	boost::smatch res;
	std::vector<double> livetime;
	std::vector<double> pulses;
	std::vector<double> distance;
	for (int y = 0; y < distData.size(); y++) {
		bool match = boost::regex_match(distData[y], res, expr);
		if (!match) {
			throw std::runtime_error(std::string("Incorrectly formated distance data."));
		}
		distance.push_back(lexical_cast<double>(res[1]));
		pulses.push_back(lexical_cast<double>(res[2]));
		if (res[3].length() != 0) {
			livetime.push_back(lexical_cast<double>(res[3]));
		} else {
			livetime.push_back(1.0);
			uncertainty = false;
		}
		if (distance[distance.size() - 1] <= 0) {
			throw std::runtime_error(std::string("Distance value can not be less than or equal to zero."));
		}
		if (pulses[pulses.size() - 1] <= 0) {
			throw std::runtime_error(std::string("Number of pulses can not be zero or negative."));
		}

		if (livetime[livetime.size() - 1] <= 0) {
			throw std::runtime_error(std::string("Live time can not be zero or negative."));
		}
	}
	return DistResponse(pulses, livetime, distance, bkg, activity, activity_uncertinaty, curve_fit);
}


#ifndef __PROCESSMAIN_HPP
#define __PROCESSMAIN_HPP

#include <vector>

#include <boost/numeric/ublas/matrix.hpp>

#include <TTree.h>
#include <TBranch.h>

#include <epecur/geometry.hpp>

using namespace boost::numeric;

struct intersection_t
{
	double	x, y, z;
};

struct track3d_t
{
	ublas::vector<double>	a, b;
};

struct intersection_set_t
{
	intersection_t	i_lr, i_rl, i_f2r, i_f2l, i_rf2, i_lf2;
	TBranch	*br_lr, *br_rl, *br_f2r, *br_f2l, *br_rf2, *br_lf2;
};

TTree*	Process( TTree *events, TTree *cycle_efficiency, Geometry &geom, double central_momentum, intersection_set_t *s );

#endif


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "error_metrics.hpp"

#include <cmath>
#include <stack>

#include <boost/algorithm/string/join.hpp>
#include <boost/dynamic_bitset.hpp>
#include <boost/format.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/combine.hpp>
#include <boost/range/numeric.hpp>
#include <boost/version.hpp>

#include <core/utils/bitset_utils.hpp>
#include <core/utils/timer.hpp>
#include <classical/dd/arithmetic.hpp>
#include <classical/dd/bdd_to_truth_table.hpp>
#include <classical/dd/characteristic.hpp>
#include <classical/dd/count_solutions.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

inline void assert_valid( const std::vector<bdd>& f, const std::vector<bdd>& fhat )
{
  assert( f.size() == fhat.size() );
  assert( !f.empty() );
}

std::vector<bdd> compute_diff( const std::vector<bdd>& f, const std::vector<bdd>& fhat, bool print_truthtables = false )
{
  assert_valid( f, fhat );

  const auto to = f.size() + 1u;

  const auto zf    = zero_extend( f, to );
  const auto zfhat = zero_extend( fhat, to );
  const auto subtr = bdd_subtract( zf, zfhat );
  const auto diff  = bdd_abs( subtr );

#if ( BOOST_VERSION / 100000 ) >= 1 && ( BOOST_VERSION / 100 % 1000 ) >= 56
  if ( print_truthtables )
  {
    using boost::adaptors::transformed;
    using boost::format;
    using boost::str;

    auto valid = true;
    auto max   = 0u;
    auto sum   = 0u;

    std::cout << std::endl << "    f    zf  fhat zfhat subtr  diff valid" << std::endl
              << boost::join( boost::combine( bdds_to_truth_table_unsigned( f ),
                                              bdds_to_truth_table_unsigned( zf ),
                                              bdds_to_truth_table_unsigned( fhat ),
                                              bdds_to_truth_table_unsigned( zfhat ),
                                              bdds_to_truth_table_signed( subtr ),
                                              bdds_to_truth_table_unsigned( diff ) )
                              | transformed( [&]( const boost::tuple<unsigned, unsigned, unsigned, unsigned, int, unsigned>& p ) {
                                  const auto vf     = boost::get<0>( p );
                                  const auto vzf    = boost::get<1>( p );
                                  const auto vfhat  = boost::get<2>( p );
                                  const auto vzfhat = boost::get<3>( p );
                                  const auto vsubtr = boost::get<4>( p );
                                  const auto vdiff  = boost::get<5>( p );

                                  const auto row_valid = ( vf == vzf ) && ( vfhat == vzfhat ) && ( vsubtr == static_cast<int>( vzf - vzfhat ) ) && ( abs( vsubtr ) == static_cast<int>( vdiff ) );
                                  valid = valid && row_valid;
                                  max   = std::max( max, vdiff );
                                  sum  += vdiff;

                                  return str( format( "%5d %5d %5d %5d %5d %5d     %d") % vf % vzf % vfhat % vzfhat % vsubtr % vdiff % row_valid );
                                } ), "\n" ) << std::endl
              << format( "valid: %d, max: %d, avg: %.2f" ) % valid % max % ( sum / pow( 2.0, f.front().manager->num_vars() ) ) << std::endl;
  }
#endif

  assert( diff.size() == to );
  assert( diff.back().index == 0u );

  return diff;
}

boost::multiprecision::uint256_t get_max_value( const std::vector<bdd>& f )
{
  assert( !f.empty() );

  boost::dynamic_bitset<> bs( f.size() );
  auto mask = f.front().manager->bdd_top();

  for ( int k = f.size() - 1; k >= 0; --k )
  {
    auto r = mask && f[k];
    if ( r.index != 0 )
    {
      bs.set( k );
      mask = r;
    }
  }

  return to_multiprecision<boost::multiprecision::uint256_t>( bs );
}

boost::multiprecision::uint256_t get_max_value_with_chi( const std::vector<bdd>& f )
{
  bdd_manager mgr_chi( f.front().manager->num_vars() + f.size(), 10u ); /* can we approximate the number of used nodes? */

  auto fr  = f; boost::reverse( fr );
  auto chi = characteristic_function( fr, mgr_chi );

  boost::dynamic_bitset<> bs( f.size() );

  while ( chi.var() < f.size() )
  {
    if ( chi.high().is_bot() )
    {
      chi = chi.low();
    }
    else
    {
      bs.set( f.size() - 1u - chi.var() );
      chi = chi.high();
    }
  }

  return to_multiprecision<boost::multiprecision::uint256_t>( bs );
}

boost::multiprecision::uint256_t get_weighted_sum( const std::vector<bdd>& f )
{
  auto level = f.size();
  bdd_manager mgr_chi( f.front().manager->num_vars() + level, 10u ); /* can we approximate the number of used nodes? */

  auto fr = f; boost::reverse( fr );
  auto chi = characteristic_function( fr, mgr_chi );

  boost::multiprecision::uint256_t sum = 0;
  const boost::multiprecision::uint256_t one = 1;
  std::stack<std::pair<bdd, boost::dynamic_bitset<>>> stack;

  stack.push( {chi, boost::dynamic_bitset<>( level )} );

  while ( !stack.empty() )
  {
    auto p = stack.top(); stack.pop();

    if ( p.first.var() >= level )
    {
      sum += to_multiprecision<boost::multiprecision::uint256_t>( p.second ) * ( count_solutions( p.first ) / ( one << level ) );
    }
    else
    {
      stack.push( {p.first.low(), p.second} );
      p.second.set( p.first.var() );
      stack.push( {p.first.high(), p.second} );
    }
  }

  return sum;
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

boost::multiprecision::uint256_t error_rate( const std::vector<bdd>& f, const std::vector<bdd>& fhat,
                                             const properties::ptr& settings,
                                             const properties::ptr& statistics )
{
  properties_timer t( statistics );

  assert_valid( f, fhat );

  auto h = f.front().manager->bdd_bot();

  for ( auto i = 0u; i < f.size(); ++i )
  {
    h = h || ( f[i] ^ fhat[i] );
  }

  return count_solutions( h );
}

boost::multiprecision::uint256_t worst_case( const std::vector<bdd>& f, const std::vector<bdd>& fhat,
                                             const properties::ptr& settings,
                                             const properties::ptr& statistics )
{
  auto print_truthtables = get( settings, "print_truthtables", false );
  auto maximum_method    = get( settings, "maximum_method",    worst_case_maximum_method::shift );

  properties_timer t( statistics );

  auto diff = compute_diff( f, fhat, print_truthtables );

  switch ( maximum_method )
  {
  case worst_case_maximum_method::shift:
    return get_max_value( diff );
  case worst_case_maximum_method::chi:
    return get_max_value_with_chi( diff );
  default:
    assert( false );
  }
}

boost::multiprecision::cpp_dec_float_100 average_case( const std::vector<bdd>& f, const std::vector<bdd>& fhat,
                                                       const properties::ptr& settings, const properties::ptr& statistics )
{
  auto print_truthtables = get( settings, "print_truthtables", false );

  properties_timer t( statistics );

  auto diff = compute_diff( f, fhat, print_truthtables );

  const boost::multiprecision::uint256_t one = 1;
  return boost::multiprecision::cpp_dec_float_100( get_weighted_sum( diff ) ) /
         boost::multiprecision::cpp_dec_float_100( one << f.front().manager->num_vars() );
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include "Robot.hpp"
#include <sstream>
#include "Thread.hpp"
#include <boost/chrono.hpp>
#include <ctime>
#include "Logger.hpp"
#include "Goal.hpp"
#include "WayPoint.hpp"
#include "Wall.hpp"
#include "RobotWorld.hpp"
#include "Math.hpp"
#include "Shape2DUtils.hpp"
#include "CommunicationService.hpp"
#include "Client.hpp"
#include "Message.hpp"

/**
 *
 */
Robot::Robot( const std::string& aName) :
				name( aName),
				size( DefaultSize),
				position( DefaultPosition),
				front( 0, 0),
				speed( 0.0),
				stop(true),
				communicating(false)
{
	std::shared_ptr< AbstractSensor > laserSensor( new LaserDistanceSensor( this));
	attachSensor( laserSensor);

//	attachSensor(std::shared_ptr< AbstractSensor >(new LaserDistanceSensor(this)));
//	attachActuator(std::shared_ptr< AbstractActuator>(new SteeringActuator(this)));
}
/**
 *
 */
Robot::Robot(	const std::string& aName,
				const Point& aPosition) :
				name( aName),
				size( DefaultSize),
				position( aPosition),
				front( 0, 0),
				speed( 0.0),
				stop(true),
				communicating(false)
{
	std::shared_ptr< AbstractSensor > laserSensor( new LaserDistanceSensor( this));
	attachSensor( laserSensor);

//	attachSensor(std::shared_ptr< AbstractSensor >(new LaserDistanceSensor(this)));
//	attachActuator(std::shared_ptr< AbstractActuator>(new SteeringActuator(this)));
}
/**
 *
 */
Robot::~Robot()
{
	if(!stop)
	{
		stopActing();
	}
	if(communicating)
	{
		stopCommunicating();
	}
}
/**
 *
 */
void Robot::setName( const std::string& aName)
{
	name = aName;
	notifyObservers();
}
/**
 *
 */
Size Robot::getSize() const
{
	return size;
}
/**
 *
 */
void Robot::setSize(	const Size& aSize,
						bool aNotifyObservers /*= true*/)
{
	size = aSize;
	if (aNotifyObservers == true)
	{
		notifyObservers();
	}
}
/**
 *
 */
void Robot::setPosition(	const Point& aPosition,
							bool aNotifyObservers /*= true*/)
{
	position = aPosition;
	if (aNotifyObservers == true)
	{
		notifyObservers();
	}
}
/**
 *
 */
Vector Robot::getFront() const
{
	return front;
}
/**
 *
 */
void Robot::setFront(	const Vector& aVector,
						bool aNotifyObservers /*= true*/)
{
	front = aVector;
	if (aNotifyObservers == true)
	{
		notifyObservers();
	}
}
/**
 *
 */
float Robot::getSpeed() const
{
	return speed;
}
/**
 *
 */
void Robot::setSpeed( float aNewSpeed)
{
	speed = aNewSpeed;
}
/**
 *
 */
void Robot::startActing()
{
	std::thread newRobotThread( [this]
	{	stop = false; drive();});
	robotThread.swap( newRobotThread);
}
/**
 *
 */
void Robot::stopActing()
{
	//robotThread.interrupt();
	stop = true;
	robotThread.join();
}
/**
 *
 */
Region Robot::getRegion() const
{
	Point translatedPoints[] = { getFrontRight(), getFrontLeft(), getBackLeft(), getBackRight() };
	return Region( 4, translatedPoints);
}
/**
 *
 */
bool Robot::intersects( const Region& aRegion) const
{
	Region region = getRegion();
	region.Intersect( aRegion);
	return !region.IsEmpty();
}
/**
 *
 */
Point Robot::getFrontLeft() const
{
	// x and y are pointing to top left now
	int x = position.x - (size.x / 2);
	int y = position.y - (size.y / 2);

	Point originalFrontLeft( x, y);
	double angle = Shape2DUtils::getAngle( front) + 0.5 * PI;

	Point frontLeft( (originalFrontLeft.x - position.x) * std::cos( angle) - (originalFrontLeft.y - position.y) * std::sin( angle) + position.x, (originalFrontLeft.y - position.y) * std::cos( angle)
										+ (originalFrontLeft.x - position.x) * std::sin( angle) + position.y);

	return frontLeft;
}
/**
 *
 */
Point Robot::getFrontRight() const
{
	// x and y are pointing to top left now
	int x = position.x - (size.x / 2);
	int y = position.y - (size.y / 2);

	Point originalFrontRight( x + size.x, y);
	double angle = Shape2DUtils::getAngle( front) + 0.5 * PI;

	Point frontRight( (originalFrontRight.x - position.x) * std::cos( angle) - (originalFrontRight.y - position.y) * std::sin( angle) + position.x, (originalFrontRight.y - position.y)
					* std::cos( angle) + (originalFrontRight.x - position.x) * std::sin( angle) + position.y);

	return frontRight;
}
/**
 *
 */
Point Robot::getBackLeft() const
{
	// x and y are pointing to top left now
	int x = position.x - (size.x / 2);
	int y = position.y - (size.y / 2);

	Point originalBackLeft( x, y + size.y);

	double angle = Shape2DUtils::getAngle( front) + 0.5 * PI;

	Point backLeft( (originalBackLeft.x - position.x) * std::cos( angle) - (originalBackLeft.y - position.y) * std::sin( angle) + position.x, (originalBackLeft.y - position.y) * std::cos( angle)
									+ (originalBackLeft.x - position.x) * std::sin( angle) + position.y);

	return backLeft;

}
/**
 *
 */
Point Robot::getBackRight() const
{
	// x and y are pointing to top left now
	int x = position.x - (size.x / 2);
	int y = position.y - (size.y / 2);

	Point originalBackRight( x + size.x, y + size.y);

	double angle = Shape2DUtils::getAngle( front) + 0.5 * PI;

	Point backRight( (originalBackRight.x - position.x) * std::cos( angle) - (originalBackRight.y - position.y) * std::sin( angle) + position.x, (originalBackRight.y - position.y) * std::cos( angle)
										+ (originalBackRight.x - position.x) * std::sin( angle) + position.y);

	return backRight;
}
/**
 *
 */
void Robot::handleNotification()
{
//	std::unique_lock<std::recursive_mutex> lock(robotMutex);

	static int update = 0;
	if ((++update % 200) == 0)
	{
//		std::cerr << __PRETTY_FUNCTION__ << std::endl;
		notifyObservers();
	}
}
/**
 *
 */
void Robot::startCommunicating()
{
	communicating = true;
	CommunicationService::getCommunicationService().runRobotServer( this);
}
/**
 *
 */
void Robot::stopCommunicating()
{
	MessageASIO::Client c1ient( CommunicationService::getCommunicationService().getIOService(), "localhost", "12345", shared_from_this());
	MessageASIO::Message message( 1, "stop");
	c1ient.dispatchMessage( message);
	communicating = false;
}
/**
 *
 */
void Robot::handleRequest( MessageASIO::Message& aMessage)
{
	Logger::log( __PRETTY_FUNCTION__ + std::string( " not implemented, ") + aMessage.asString());
	aMessage.setBody( "Goodbye cruel world!");
}
/**
 *
 */
void Robot::handleResponse( MessageASIO::Message& aMessage)
{
	Logger::log( __PRETTY_FUNCTION__ + std::string( " not implemented, ") + aMessage.asString());
}
/**
 *
 */
std::string Robot::asString() const
{
	std::ostringstream os;

	os << "Robot " << name << " at (" << position.x << "," << position.y << ")";

	return os.str();
}
/**
 *
 */
std::string Robot::asDebugString() const
{
	std::ostringstream os;

	os << "Robot:\n";
	os << AbstractAgent::asDebugString();
	os << "Robot " << name << " at (" << position.x << "," << position.y << ")\n";

	return os.str();
}
/**
 *
 */
void Robot::drive()
{
	Logger::log( __PRETTY_FUNCTION__);

	try
	{
		for (std::shared_ptr< AbstractSensor > sensor : sensors)
		{
			//sensor->setOn();
		}

		if (speed == 0.0)
		{
			speed = 10.0;
		}

		GoalPtr goal = RobotWorld::getRobotWorld().getGoalByName( "Leon");
		calculateRoute(goal);

		unsigned pathPoint = 0;
		while (position.x > 0 && position.x < 500 && position.y > 0 && position.y < 500 && pathPoint < path.size())
		{
			const Vertex& vertex = path[pathPoint+=speed];
			front = Vector( vertex.asPoint(), position);
			position.x = vertex.x;
			position.y = vertex.y;

			if (arrived(goal) || collision())
			{
				notifyObservers();
				break;
			}

			notifyObservers();

			std::this_thread::sleep_for( std::chrono::milliseconds( 100));

			// this should be either the last call in the loop or
			// part of the while.
			if(stop == true)
			{
				return;
			}
		} // while

		for (std::shared_ptr< AbstractSensor > sensor : sensors)
		{
			//sensor->setOff();
		}
	}
	catch (std::exception& e)
	{
		std::cerr << __PRETTY_FUNCTION__ << ": " << e.what() << std::endl;
	}
	catch (...)
	{
		std::cerr << __PRETTY_FUNCTION__ << ": unknown exception" << std::endl;
	}
}
/**
 *
 */
void Robot::calculateRoute(GoalPtr aGoal)
{
	path.clear();
	if (aGoal)
	{
		// Turn off logging if not debugging AStar
		Logger::setDisable();

		front = Vector( aGoal->getPosition(), position);
		handleNotificationsFor( astar);
		path = astar.search( position, aGoal->getPosition(), size);
		stopHandlingNotificationsFor( astar);

		Logger::setDisable( false);
	}
}
/**
 *
 */
bool Robot::arrived(GoalPtr aGoal)
{
	if (aGoal && intersects( aGoal->getRegion()))
	{
		return true;
	}
	return false;
}
/**
 *
 */
bool Robot::collision()
{
	Point frontLeft = getFrontLeft();
	Point frontRight = getFrontRight();
	Point backLeft = getBackLeft();
	Point backRight = getBackRight();

	const std::vector< WallPtr >& walls = RobotWorld::getRobotWorld().getWalls();
	for (WallPtr wall : walls)
	{
		if (Shape2DUtils::intersect( frontLeft, frontRight, wall->getPoint1(), wall->getPoint2()) ||
			Shape2DUtils::intersect( frontLeft, backLeft, wall->getPoint1(), wall->getPoint2())	||
			Shape2DUtils::intersect( frontRight, backRight, wall->getPoint1(), wall->getPoint2()))
		{
			return true;
		}
	}
	return false;
}


#include <iostream>
#include <string>
#include <algorithm>
#include <thread>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <dirent.h>
#include <sys/types.h>
#include <cstdio>
#include <unistd.h>
#include <signal.h>

#include <boost/program_options.hpp>

#include "System.h"
#include "../common.h"

using namespace std;
using namespace ORB_SLAM2;
namespace po = boost::program_options;

typedef Eigen::Transform<float,3,Eigen::Affine> Transform3;

struct imagePose {
	vector<string> imagePaths;
	vector<double> timestamps;
	vector<Transform3> poses;
};

bool interruptStop = false;
void interruptHandler (int s)
{
	interruptStop = true;
}

void loadData (const string &dataDir, imagePose& dataset)
{
	DIR *fd = opendir(dataDir.c_str());
	if (fd==NULL) return;
//		throw exception ("Invalid directory");
	const string poseFn = dataDir + "/pose.csv";
	FILE *posefd = fopen(poseFn.c_str(), "r");

	dirent *de;
	while (true) {
		de = readdir (fd);
		if (de==NULL) break;
		string fname (de->d_name);

		// check if png
		string ext;
		try {
			ext = fname.substr(fname.size()-3);
		} catch (out_of_range &e) {
			continue;
		}
		if (ext != "jpg")
			continue;

		dataset.imagePaths.push_back(dataDir + '/' + fname);
	}

	closedir(fd);
	std::sort(dataset.imagePaths.begin(), dataset.imagePaths.end());
	cout << "Found " << dataset.imagePaths.size() << " images" << endl;

	// XXX: load pose
	while (!feof(posefd)) {
		Transform3 cpose;
		float row[8];
		float timestamp;
		int p = fscanf (posefd, "%f %f %f %f %f %f %f %f",
				&row[0],
				&row[1],
				&row[2],
				&row[3],
				&row[4],
				&row[5],
				&row[6],
				&row[7]);
		timestamp = row[0];
		Eigen::Quaternionf qf (row[7], row[4], row[5], row[6]);
		cpose = qf;
		cpose.translation().x() = row[1];
		cpose.translation().y() = row[2];
		cpose.translation().z() = row[3];

		dataset.timestamps.push_back(timestamp);
		dataset.poses.push_back(cpose);
	}
	cout << "Found " << dataset.poses.size() << " positions" << endl;

	fclose (posefd);
}

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

	po::options_description mapperOpts ("Mapper Options");
	mapperOpts.add_options()
		("help", "produce help messages")
		("config", po::value<string>(), "Configuration File")
		("mapfile", po::value<string>(), "Path for saved map")
		("datadir", po::value<string>(), "Path for image and pose directories")
		("start", po::value<int>(), "Start from numbered image")
		("stop", po::value<int>(), "Stop in sequence number")
		("delay", po::value<int>(), "Delay between frames in microseconds")
	;
	po::variables_map mapperOptMap;
	po::store (po::parse_command_line(argc, argv, mapperOpts), mapperOptMap);
	po::notify(mapperOptMap);

	if (mapperOptMap.count("help") or
		!mapperOptMap.count("config") or
		!mapperOptMap.count("mapfile") or
		!mapperOptMap.count("datadir"))
	{
		cout << mapperOpts << endl;
		return 1;
	}

	const string orbVocabFile (ORB_SLAM_VOCABULARY);
	string configFile = mapperOptMap["config"].as<string>();
	string mapPath = mapperOptMap["mapfile"].as<string>();
	string dataDir = mapperOptMap["datadir"].as<string>();
	int startNum, stopNum;
	try {
		startNum = mapperOptMap["start"].as<int>();
	} catch (boost::bad_any_cast &e) {
		startNum = 0;
	}

	cout << "Loading data..." << endl;
	imagePose mDataSet;
	loadData (dataDir, mDataSet);
	cout << "Loading complete" << endl;
//	stopNum = mDataSet.poses.size();
	try {
		stopNum = mapperOptMap["stop"].as<int>();
		if (stopNum >= mDataSet.poses.size())
			throw boost::bad_any_cast();
	} catch (boost::bad_any_cast &e) {
		stopNum = mDataSet.poses.size();
	}

	ORB_SLAM2::System SLAM(orbVocabFile,
		configFile,
		ORB_SLAM2::System::MONOCULAR,
		true,
		mapPath,
		System::MAPPING,
		// offline flag
		false);

	int usecDelay;
	try {
		usecDelay = mapperOptMap["delay"].as<int>();
	} catch (boost::bad_any_cast &e) {
		float us = 1 / (float)SLAM.fsSettings["Camera.fps"];
		usecDelay = (int) (us * 1e6);
	}
	cout << "Delay: " << usecDelay << " microseconds" << endl;

	double fx2, fy2, cx2, cy2;
	recomputeNewCameraParameter (
		(double)SLAM.fsSettings["Camera.fx"],
		(double)SLAM.fsSettings["Camera.fy"],
		(double)SLAM.fsSettings["Camera.cx"],
		(double)SLAM.fsSettings["Camera.cy"],
		fx2, fy2, cx2, cy2,
		1920, 1440,
		(int)SLAM.fsSettings["Camera.WorkingResolution.Width"],
		(int)SLAM.fsSettings["Camera.WorkingResolution.Height"]);
	// send camera parameters to tracker
	SLAM.getTracker()->ChangeCalibration (fx2, fy2, cx2, cy2);

	signal (SIGINT, interruptHandler);

	for (int i=startNum; i<stopNum-2; i++) {

		cout << "Processing Frame# " << i << "\r";
		cout.flush();

		if (interruptStop==true) {
			cout << "Stopped in Frame# " << i << endl;
			break;
		}

		const string &path = mDataSet.imagePaths[i];
		Transform3 pose = mDataSet.poses[i];
		double timestamp = mDataSet.timestamps[i];

		cv::Mat image = cv::imread (path);
		// Processing before sending image to tracker
		// Do Resizing and cropping here
		cv::resize(image, image,
			cv::Size(
				(int)SLAM.fsSettings["Camera.WorkingResolution.Width"],
				(int)SLAM.fsSettings["Camera.WorkingResolution.Height"]
			));
		image = image(
			cv::Rect(
				(int)SLAM.fsSettings["Camera.ROI.x0"],
				(int)SLAM.fsSettings["Camera.ROI.y0"],
				(int)SLAM.fsSettings["Camera.ROI.width"],
				(int)SLAM.fsSettings["Camera.ROI.height"]
			)).clone();

		{
			Transform3 &p = mDataSet.poses[i];
			unique_lock <mutex> lock (ORB_SLAM2::KeyFrame::extPoseMutex);
			ORB_SLAM2::KeyFrame::extEgoPosition = cv::Mat (3,1,CV_64F);
			ORB_SLAM2::KeyFrame::extEgoPosition.at<double>(0) = p.translation().x();
			ORB_SLAM2::KeyFrame::extEgoPosition.at<double>(1) = p.translation().y();
			ORB_SLAM2::KeyFrame::extEgoPosition.at<double>(2) = p.translation().z();

			ORB_SLAM2::KeyFrame::extEgoOrientation = cv::Mat (4,1,CV_64F);
			Eigen::Quaternionf rp (p.rotation().matrix());
			ORB_SLAM2::KeyFrame::extEgoOrientation.at<double>(0) = rp.x();
			ORB_SLAM2::KeyFrame::extEgoOrientation.at<double>(1) = rp.y();
			ORB_SLAM2::KeyFrame::extEgoOrientation.at<double>(2) = rp.z();
			ORB_SLAM2::KeyFrame::extEgoOrientation.at<double>(3) = rp.w();
//			cout << ORB_SLAM2::KeyFrame::extEgoOrientation << endl;
		}

		try {
			SLAM.TrackMonocular (image, timestamp);
		} catch (cv::Exception &e) {
			cerr << "CV Exception" << endl;
			raise (SIGSEGV);
		}

		// Check status
		if (SLAM.getTracker()->mState==Tracking::LOST) {
			cout << "Lost in sequence #" << i << endl;
//			break;
		}

		usleep (usecDelay);
	}

	SLAM.Shutdown();
	return 0;
}


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

#include "cpp/sensor_fusion/online_sensor_fusion.h"

#include "absl/memory/memory.h"
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace vr180 {
namespace {
Eigen::Matrix3d LoadGravityAdjustMatrix() {
  Eigen::Matrix3d rotation;
  rotation.row(0) = Eigen::RowVector3d(1, 0, 0);
  rotation.row(1) = Eigen::RowVector3d(0, 0, -1);
  rotation.row(2) = Eigen::RowVector3d(0, 1, 0);
  return rotation;
}

OrientationFilter::OrientationFilterConfiguration GetFilterConfiguration(
    const OnlineSensorFusion::Options& options) {
  OrientationFilter::OrientationFilterConfiguration config;
  config.stationary_bias_correction_gain =
      options.stationary_bias_correction_gain;
  config.gyroscope_bias_correction_gain =
      options.gyroscope_bias_correction_gain;
  config.accel_yaw_correction_gain = options.accel_yaw_correction_gain;
  return config;
}

}  // namespace

OnlineSensorFusion::OnlineSensorFusion(const Options& options) {
  orientation_filter_ =
      absl::make_unique<OrientationFilter>(GetFilterConfiguration(options));
  calibrated_imu_orientation_ = options.device_to_imu_transform;
  last_timestamp_s_ = 0.0;
}

void OnlineSensorFusion::AddGyroMeasurement(const Eigen::Vector3d& sample,
                                            double timestamp_s) {
  if (timestamp_s < last_timestamp_s_) {
    LOG(WARNING) << "gyro timestamps not monotonically increasing";
  }
  orientation_filter_->AddGyroMeasurement(sample, timestamp_s);
  last_timestamp_s_ = timestamp_s;
}

void OnlineSensorFusion::AddAccelMeasurement(const Eigen::Vector3d& sample,
                                             double timestamp_s) {
  if (timestamp_s < last_timestamp_s_) {
    LOG(WARNING) << "accel timestamps not monotonically increasing";
  }
  orientation_filter_->AddAccelMeasurement(sample, timestamp_s);
  last_timestamp_s_ = timestamp_s;
}

Eigen::Vector3f OnlineSensorFusion::GetOrientation() const {
  const Eigen::Matrix3d orientation =
      Eigen::Quaterniond(orientation_filter_->GetOrientation())
          .toRotationMatrix();
  const Eigen::Matrix3d matrix =
      (LoadGravityAdjustMatrix() * orientation * calibrated_imu_orientation_);
  const Eigen::AngleAxisd angle_axis(matrix);
  const Eigen::Vector3d aa_double = angle_axis.angle() * angle_axis.axis();
  return aa_double.cast<float>();
}

void OnlineSensorFusion::SetGyroBias(const Eigen::Vector3d& bias) {
  orientation_filter_->SetGyroBias(bias);
}

void OnlineSensorFusion::Recenter() {
  const Eigen::Vector4d q = orientation_filter_->GetOrientation();
  const Eigen::Quaterniond quaternion(q[3], q[0], q[1], q[2]);
  const Eigen::Matrix3d m = quaternion.toRotationMatrix();
  Eigen::Matrix3d adjust = Eigen::Matrix3d::Identity();
  adjust.row(1) = m.col(2).transpose();
  if (adjust.row(1).cross(adjust.row(2)).squaredNorm() <
      std::numeric_limits<double>::epsilon()) {
    return;
  }
  adjust.row(0) = adjust.row(1).cross(adjust.row(2)).normalized();
  adjust.row(1) = adjust.row(2).cross(adjust.row(0)).normalized();
  const Eigen::Quaterniond aq(adjust * m);
  orientation_filter_->SetOrientation(
      Eigen::Vector4d(aq.x(), aq.y(), aq.z(), aq.w()));
}
}  // namespace vr180