#include <ros/ros.h>
#include <vector>
#include <math.h>

#include<geometry_msgs/Quaternion.h>
#include<geometry_msgs/Pose.h>

//PCL includes
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

#include <pcl/features/normal_3d.h>

#include <pcl/filters/crop_box.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/conversions.h>

//tf includes
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <tf/tf.h>
#include <tf_conversions/tf_eigen.h>

//includes Eigens
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

//#include "roi_extractor/boxfilter.hpp"

// services ,msgs includes
//#include <roi_extractor_msg/roiExtract.h> custom msgs
//#include <assembly_msgs/GetOccupancyVoxel.h>
#include "gb_visual_detection_3d_msgs/BoundingBoxes3d.h"

sensor_msgs::PointCloud2 g_cloud;
ros::Publisher g_pub_filtered;
ros::Publisher g_pub_test;
bool smapore = false;
/*
geometry_msgs::Pose g_Pose;
sensor_msgs::PointCloud2 g_cloud;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
ros::Publisher g_pub_filtered;
ros::Publisher g_pub_clone;

void cloud_cb()
{
    //reset
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr g_cloud_init (new pcl::PointCloud<pcl::PointXYZRGB>);
    copyPointCloud(*g_cloud_filtered, *g_cloud_init);
    // Convert to pcl point cloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg(g_cloud,*cloud_msg);

    //Data Saving Place
    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr restored_cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
    Eigen::Affine3f T_BO, T_OB, T_CB, T_BC;
    T_BO.translation() << g_Pose.position.x, g_Pose.position.y, g_Pose.position.z;
    T_BO.linear() = Eigen::Quaternionf(g_Pose.orientation.w,g_Pose.orientation.x,g_Pose.orientation.y,g_Pose.orientation.z).toRotationMatrix();
    std::cout << "T_BO"<<std::endl;
    std::cout << T_BO.matrix()<<std::endl;
    T_OB = T_BO.inverse();
    std::cout << "T_OB"<<std::endl;
    std::cout << T_OB.matrix() <<std::endl;
    T_BC.matrix() << 0.0324386,  0.9170405,  0.3974727, 0.021856,
                  0.9990817, -0.0408897,  0.0128025, 0.0439238,
                  0.0279930,  0.3966924, -0.9175247, 2.55231,
                  0,          0,          0,          1;
    T_CB = T_BC.inverse();
    pcl::transformPointCloud (*cloud_msg, *transformed_cloud, T_OB * T_BC);
    // pass Through Filter cloud  --> rnage filter
    // ROI SET
    // 50 x 50 x 50 ROI
    Eigen::Vector4f minPoint;
    minPoint[0] = -0.4;
    minPoint[1] = -0.4;
    minPoint[2] = -0.3;
    minPoint[3] = 1;
    Eigen::Vector4f maxPoint;
    maxPoint[0] = 0.4;
    maxPoint[1] = 0.4;
    maxPoint[2] = 0.3;
    maxPoint[3] = 1;

    pcl::CropBox<pcl::PointXYZRGB> boxFilter;
    boxFilter.setInputCloud(transformed_cloud);
    boxFilter.setMin(minPoint);
    boxFilter.setMax(maxPoint);
    boxFilter.filter(*transformed_cloud);

    //voxel Filter
    pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    sor.setInputCloud(transformed_cloud);
    sor.setLeafSize(0.04, 0.04, 0.04);
    sor.filter(*transformed_cloud);
    std::cout << "maxboxcor " << sor.getMaxBoxCoordinates() << std::endl;
    std::cout << "minboxcor " << sor.getMinBoxCoordinates() << std::endl;
    std::cout << "size: " << sor.getMaxBoxCoordinates() - sor.getMinBoxCoordinates() << std::endl;

    //StatisticalOutlierRemoval Filter
    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> rmv;
    rmv.setInputCloud(transformed_cloud);
    rmv.setMeanK(50);
    rmv.setStddevMulThresh(1.0);
    rmv.filter(*transformed_cloud);

    int len_points =

    Eigen::Tensor<int, 3> return_matrix(16, 16, 16);
    return_matrix.setZero();

    pcl::transformPointCloud (*transformed_cloud, *restored_cloud,  T_CB * T_BO);

    // //must be acess in base_frame (지금은 zvid_pc_50704 )
    // geometry_msgs::Pose transformedPose = g_Pose;

    // //Homogenous Transform Matrix
    // Eigen::Matrix4f BaseTozivid;
    // //zividToBase << 0.0324386,  0.9170405,  0.3974727, 0.021856,
    // //               0.9990817, -0.0408897,  0.0128025, 0.0439238,
    // //               0.0279930,  0.3966924, -0.9175247, 2.55231,
    // //               0,          0,          0,          1;
    // //inverse Matrix Base to zvid 들어온 좌표를 zivid로 변환
    // BaseTozivid << 0.032439,	 0.999082,	 0.027993,	-0.116039,
    //                0.917040,	-0.040890,	 0.396692,	-1.030729,
    //                0.397473,	 0.012803,	-0.917525,	 2.332558,
    //                0,	         0,  	     0,	         1;

    // // You can either apply transform_1 or transform_2; they are the same

    // //Min and Max refer to the two diagonal points of the cube. Each point is represented by a four-dimensional vector, usually the last one is 1. (I don’t know why there are four, the big god knows to give an answer)
    // Eigen::Vector4f boxTranslatation;
    // boxTranslatation[0]=g_Pose.position.x;
    // boxTranslatation[1]=g_Pose.position.y;
    // boxTranslatation[2]=g_Pose.position.z;
    // boxTranslatation[3]=1;

    // boxTranslatation = BaseTozivid*boxTranslatation;

    // Eigen::Vector3f posTranslate;
    // posTranslate[0] = boxTranslatation[0];
    // posTranslate[1] = boxTranslatation[1];
    // posTranslate[2] = boxTranslatation[2];

    // tf::Quaternion inverse_zivid( 0.7054882, 0.6790058, 0.1507701, -0.1360367);
    // //회전역시 마찬가지로 변환을 해주어야 한다.
    // Eigen::Vector3f boxRotation;
    // tf::Quaternion boxratationTF(g_Pose.orientation.x,g_Pose.orientation.y,g_Pose.orientation.z,g_Pose.orientation.w);

    // //먼저한 linear transformation이 뒤로온다.
    // boxratationTF = boxratationTF*inverse_zivid;

    // tf::Matrix3x3 boxratationTFMat(boxratationTF);
    // double roll,pitch,yaw;
    // boxratationTFMat.getRPY(roll,pitch,yaw);
    // boxRotation[0]=roll;
    // boxRotation[1]=pitch;
    // boxRotation[2]=yaw;

    // pcl::CropBox<pcl::PointXYZRGB> boxFilter;
    // boxFilter.setInputCloud(cloud_msg);
    // boxFilter.setMin(minPoint);
    // boxFilter.setMax(maxPoint);
    // boxFilter.setTranslation(posTranslate);
    // boxFilter.setRotation(boxRotation);
    // boxFilter.filter(*cloud);

    // //voxel Filter
    // pcl::VoxelGrid<pcl::PointXYZRGB> sor;
    // sor.setInputCloud(cloud);
    // sor.setLeafSize(0.04, 0.04, 0.04);
    // sor.filter(*cloud);
    // std::cout << "maxboxcor " << sor.getMaxBoxCoordinates() << std::endl;
    // std::cout << "minboxcor " << sor.getMinBoxCoordinates() << std::endl;
    // std::cout << "size: " << sor.getMaxBoxCoordinates() - sor.getMinBoxCoordinates() << std::endl;

    // //StatisticalOutlierRemoval Filter
    // pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> rmv;
    // rmv.setInputCloud(cloud);
    // rmv.setMeanK(50);
    // rmv.setStddevMulThresh(1.0);
    // rmv.filter(*cloud);

    // pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filling (new pcl::PointCloud<pcl::PointXYZRGB>);
    // pcl::PointXYZRGB pointd;
    // pointd.x = posTranslate[0];
    // pointd.y = posTranslate[1];
    // pointd.z = posTranslate[2];

    // cloud_filling->push_back(pointd);

    // ROS_INFO("request:Position       x=%f, y=%f, z=%f",g_Pose.position.x,g_Pose.position.y,g_Pose.position.z);
    // *cloud += *cloud_filling;

    // copyPointCloud(*cloud, *g_cloud_filtered);

    // Publish points
    sensor_msgs::PointCloud2 filtered_publish;

    pcl::toROSMsg(*restored_cloud,filtered_publish);
    filtered_publish.header = g_cloud.header;    //.header.frame_id = "base";// --> base header
    g_pub_filtered.publish(filtered_publish);
}

void rcv_cloud (const sensor_msgs::PointCloud2ConstPtr& msg)
{
    // Convert to pcl point cloud
    ROS_INFO("rcv cloud");
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg(*msg,*cloud_msg);
    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*cloud_msg,cloud_publish);
    cloud_publish.header = msg->header;  //-->zvid_pc_50704

    //std::cerr<<"frame id : "<<cloud_publish.header.frame_id<<std::endl;
    if(cloud_publish.width * cloud_publish.height > 1)
    {
        g_cloud = cloud_publish;
    }

    //echo publish clouds
    g_pub_clone.publish(g_cloud);

}

bool extractRoi(assembly_msgs::GetOccupancyVoxel::Request &req,
    assembly_msgs::GetOccupancyVoxel::Response &res)
{
    ROS_INFO("Rececivecd !!");
    g_Pose = req.pose;
    cloud_cb();
    // sensor_msgs::PointCloud2 responese_cloud;
    // pcl::toROSMsg(*g_cloud_filtered,responese_cloud);
    // res.PointCloud2 = responese_cloud;
    // res.respose = req.pose;

    //이거 써야지 제대로 됨
    return true;
}
*/

void rcv_cloud (const sensor_msgs::PointCloud2ConstPtr& msg)
{
    // Convert to pcl point cloud
    //ROS_INFO("rcv cloud");
    if(!smapore)
    {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(*msg,*cloud_msg);
        sensor_msgs::PointCloud2 cloud_publish;

        //pcl::VoxelGrid<pcl::PointXYZRGB> sor;
        //sor.setInputCloud(cloud_msg);
        //sor.setLeafSize(0.02, 0.02, 0.02);
        //sor.filter(*cloud_msg);

        pcl::toROSMsg(*cloud_msg,cloud_publish);
        cloud_publish.header = msg->header;  //-->zvid_pc_50704

        //std::cerr<<"frame id : "<<cloud_publish.header.frame_id<<std::endl;
        if(cloud_publish.width * cloud_publish.height > 1)
        {
            g_cloud = cloud_publish;
        }
    }

    //echo publish clouds
    //g_pub_clone.publish(g_cloud);

}

void roiCallback (const gb_visual_detection_3d_msgs::BoundingBoxes3dConstPtr& boxes)
{
    //Eigen::Affine3f T_KO,T_LK,T_LO;
    smapore = true;
    Eigen::Affine3f T_LK;

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    for(unsigned int num = 0; num < boxes->bounding_boxes.size(); num++)
    {
        //T_KO.translation() << (boxes->bounding_boxes.at(num).xmin+boxes->bounding_boxes.at(num).xmax)/2.0, (boxes->bounding_boxes.at(num).ymin+boxes->bounding_boxes.at(num).ymax)/2.0
        //, (boxes->bounding_boxes.at(num).zmin+boxes->bounding_boxes.at(num).zmax)/2.0;
        //T_KO.linear() = Eigen::Quaternionf(1,0,0,0).toRotationMatrix();

        T_LK.translation() << -0.00014467 , 0.014879, 0.000155564;
        T_LK.linear() = Eigen::Quaternionf(-0.49997,0.49875,-0.49919,0.50208).toRotationMatrix();
        //std::cerr<<"Max Points  num :"<<num<<"  , "<<maxPoint<<std::endl;

        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rcv (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(g_cloud,*cloud_filtered);
        //pcl::transformPointCloud (*cloud_rcv, *cloud_filtered, T_LK.inverse());

        Eigen::Vector4f minPoint;
        minPoint[0] = boxes->bounding_boxes.at(num).xmin;
        minPoint[1] = boxes->bounding_boxes.at(num).ymin;
        minPoint[2] = boxes->bounding_boxes.at(num).zmin;
        minPoint[3] = 1.0;
        Eigen::Vector4f maxPoint;
        maxPoint[0] = boxes->bounding_boxes.at(num).xmax;
        maxPoint[1] = boxes->bounding_boxes.at(num).ymax;
        maxPoint[2] = boxes->bounding_boxes.at(num).zmax;
        maxPoint[3] = 1.0;

        Eigen::Vector3f posTranslate;
        posTranslate[0] = (boxes->bounding_boxes.at(num).xmin+boxes->bounding_boxes.at(num).xmax )/2;
        posTranslate[1] = (boxes->bounding_boxes.at(num).ymin+boxes->bounding_boxes.at(num).ymax )/2;
        posTranslate[2] = (boxes->bounding_boxes.at(num).zmin+boxes->bounding_boxes.at(num).zmax )/2;
        //Eigen::Vector3f boxRotation;
        //posTranslate.setZero();
        //boxRotation.setZero();

        sensor_msgs::PointCloud2 debug;
        pcl::toROSMsg(*cloud_filtered,debug);
        g_pub_test.publish(debug);

        pcl::PointXYZRGB point;
        //for(unsigned int i = 0; i < cloud_filtered->points.size();i++)
       // {
        //    point.x = cloud_filtered->points.at(i).x;
        //    point.y = cloud_filtered->points.at(i).y;
        //    point.z = cloud_filtered->points.at(i).z;

        //    if( !( (point.x < minPoint[0] || point.y < minPoint[1] || point.z < minPoint[2]) ||(point.x > maxPoint[0] || point.y > maxPoint[1] || point.z > maxPoint[2]) ) )
        //    {
        //        cloud_rcv->header = cloud_filtered->header;
        //        cloud_rcv->push_back(cloud_filtered->points.at(i));
        //    }
            //std::cerr<<"point :"<<point<<std::endl;
            //std::cerr<<"max point :"<<maxPoint<<std::endl;
            //std::cerr<<"min point :"<<minPoint<<std::endl;
        //}

        pcl::CropBox<pcl::PointXYZRGB> boxFilter;
        //boxFilter.setKeepOrganized(false);
        //boxFilter.setNegative(false);
        boxFilter.setMin(minPoint);
        boxFilter.setMax(maxPoint);
        boxFilter.setInputCloud(cloud_filtered);
        boxFilter.setTransform(T_LK);
        //boxFilter.setRotation(boxRotation);
        boxFilter.filter(*cloud_rcv);
        cloud_rcv->header = cloud_filtered->header;

        //for Debug
        point.x = boxes->bounding_boxes.at(num).xmin;
        point.y = boxes->bounding_boxes.at(num).ymin;
        point.z = boxes->bounding_boxes.at(num).zmin;
        point.r = 255;
        point.b = 0;
        point.g = 0;
        cloud_rcv->push_back(point);

        point.x = boxes->bounding_boxes.at(num).xmax;
        point.y = boxes->bounding_boxes.at(num).ymax;
        point.z = boxes->bounding_boxes.at(num).zmax;
        point.r = 0;
        point.b = 255;
        point.g = 0;
        cloud_rcv->push_back(point);

        *cloud  = *cloud + *cloud_rcv;
    }

    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*cloud,cloud_publish);
    cloud_publish.header = g_cloud.header;
    g_pub_filtered.publish(cloud_publish);
    smapore = false;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "n_sgm_node");
    ros::NodeHandle nh;

     // Create a ROS subscriber for the input point cloud
    // when you scrible some topic you should include "/"
    ros::Subscriber sub = nh.subscribe ("/camera/depth_registered/points", 1, rcv_cloud); //camera/depts/points
    ros::Subscriber sub_box = nh.subscribe ("/darknet_ros_3d/bounding_boxes", 1, roiCallback);

    // Create a ROS publisher for the output point cloud
    //g_pub_clone = nh.advertise<sensor_msgs::PointCloud2> ("kinect/PointCloud", 1);
    //pub_equ = nh.advertise<pcl_msgs::ModelCoefficients> ("Plane/Coefficient", 1);
    g_pub_filtered = nh.advertise<sensor_msgs::PointCloud2>("/ROI/FilteredCloud", 1);
    g_pub_test = nh.advertise<sensor_msgs::PointCloud2>("/DEBUG/FilteredCloud", 1);

    // Create a Ros Srv for plane Equation ax+by+cz+d = 0
    //ros::ServiceServer service = nh.advertiseService("/roiExtractor", extractRoi);
    //spin
    ros::spin();
}


// This file is part of the dune-hdd project:
//   http://users.dune-project.org/projects/dune-hdd
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_SWIPDG_HH
#define DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_SWIPDG_HH

#include <memory>
#include <vector>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/common/timer.hh>
#include <dune/common/static_assert.hh>

#include <dune/stuff/common/disable_warnings.hh>
# if HAVE_ALUGRID
#   include <dune/grid/alugrid.hh>
# endif
#include <dune/stuff/common/reenable_warnings.hh>

#if HAVE_DUNE_GRID_MULTISCALE
# include <dune/grid/multiscale/provider/interface.hh>
#endif

#include <dune/stuff/common/configuration.hh>
#include <dune/stuff/common/timedlogging.hh>
#include <dune/stuff/grid/layers.hh>
#include <dune/stuff/grid/provider.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>
#include <dune/stuff/grid/walker/functors.hh>

#include <dune/gdt/assembler/system.hh>
#include <dune/gdt/assembler/tmp-storage.hh>
#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/functionals/l2.hh>
#include <dune/gdt/operators/oswaldinterpolation.hh>
#include <dune/gdt/operators/projections.hh>
#include <dune/gdt/playground/functionals/swipdg.hh>
#include <dune/gdt/playground/operators/elliptic-swipdg.hh>
#include <dune/gdt/playground/operators/fluxreconstruction.hh>
#include <dune/gdt/playground/products/swipdgpenalty.hh>
#include <dune/gdt/spaces/fv/default.hh>
#include <dune/gdt/spaces/rt/pdelab.hh>
#include <dune/gdt/spaces/dg.hh>

#include "base.hh"

namespace Dune {
namespace HDD {
namespace LinearElliptic {
namespace Discretizations {

// forward, for friendlyness
template< class GridImp, class RangeFieldImp, int rangeDim, int polynomialOrder, Stuff::LA::ChooseBackend la_backend >
class BlockSWIPDG;

// forward, needed in the Traits
template< class GridImp, Stuff::Grid::ChooseLayer layer, class RangeFieldImp, int rangeDim, int polynomialOrder = 1,
#if HAVE_DUNE_FEM
          GDT::ChooseSpaceBackend space_backend = GDT::ChooseSpaceBackend::fem,
#else
# error No suitable space backend available!
#endif
          Stuff::LA::ChooseBackend la_backend  = Stuff::LA::default_sparse_backend >
class SWIPDG;

namespace internal {

template< class GridImp, Stuff::Grid::ChooseLayer layer, class RangeFieldImp, int rangeDim, int polynomialOrder,
          GDT::ChooseSpaceBackend space_backend,
          Stuff::LA::ChooseBackend la_backend >
class SWIPDGTraits
  : public internal::ContainerBasedDefaultTraits< typename Stuff::LA::Container< RangeFieldImp, la_backend >::MatrixType,
                                                  typename Stuff::LA::Container< RangeFieldImp, la_backend >::VectorType>
{
public:
  typedef SWIPDG< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder, space_backend, la_backend > derived_type;
  typedef GridImp GridType;
  typedef RangeFieldImp RangeFieldType;
  static const unsigned int dimRange = rangeDim;
  static const unsigned int polOrder = polynomialOrder;

private:
  typedef GDT::Spaces::DGProvider< GridType, layer, space_backend, polOrder, RangeFieldType, dimRange > SpaceProvider;

  friend class SWIPDG< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder, space_backend, la_backend >;

public:
  typedef typename SpaceProvider::Type TestSpaceType;
  typedef TestSpaceType AnsatzSpaceType;
  typedef typename TestSpaceType::GridViewType GridViewType;
}; // class SWIPDGTraits

} // namespace internal

template< class GridImp, Stuff::Grid::ChooseLayer layer, class RangeFieldImp, int rangeDim, int polynomialOrder,
          GDT::ChooseSpaceBackend space_backend,
          Stuff::LA::ChooseBackend la_backend >
class SWIPDG
  : public ContainerBasedDefault< internal::SWIPDGTraits< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder,
                                                          space_backend, la_backend > >
{
  typedef ContainerBasedDefault< internal::SWIPDGTraits< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder,
                                                         space_backend, la_backend > > BaseType;
  typedef SWIPDG< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder, space_backend, la_backend > ThisType;
public:
  typedef internal::SWIPDGTraits< GridImp, layer, RangeFieldImp, rangeDim, polynomialOrder, space_backend, la_backend >
      Traits;
  typedef GridImp GridType;
  using typename BaseType::ProblemType;
  using typename BaseType::BoundaryInfoType;
  using typename BaseType::TestSpaceType;
  using typename BaseType::AnsatzSpaceType;
  using typename BaseType::MatrixType;
  using typename BaseType::VectorType;
  using typename BaseType::GridViewType;
  using typename BaseType::RangeFieldType;

  typedef typename TestSpaceType::PatternType PatternType;

  static const unsigned int dimDomain = BaseType::dimDomain;
  static const unsigned int dimRange = BaseType::dimRange;

private:
  typedef typename Traits::SpaceProvider SpaceProvider;

  typedef Stuff::Grid::ProviderInterface< GridType >      GridProviderType;
#if HAVE_DUNE_GRID_MULTISCALE
  typedef grid::Multiscale::ProviderInterface< GridType > MsGridProviderType;
#endif
  using typename BaseType::AffinelyDecomposedMatrixType;
  using typename BaseType::AffinelyDecomposedVectorType;

  typedef typename ProblemType::DiffusionFactorType::NonparametricType DiffusionFactorType;
  typedef typename ProblemType::DiffusionTensorType::NonparametricType DiffusionTensorType;
  typedef GDT::Operators::EllipticSWIPDG< DiffusionFactorType, MatrixType
                                        , TestSpaceType, AnsatzSpaceType
                                        , GridViewType, DiffusionTensorType > EllipticOperatorType;

public:
  static std::string static_id();

  template< Stuff::Grid::ChooseLayer lr = layer >
  SWIPDG(GridProviderType& grid_provider,
         const Stuff::Common::Configuration& bound_inf_cfg,
         const ProblemType& prob,
         const int level_or_subdomain = 0,
         const std::vector< std::string >& only_these_products = {}
#if HAVE_DUNE_GRID_MULTISCALE
       , typename std::enable_if<    (lr != Stuff::Grid::ChooseLayer::local)
                                  && (lr != Stuff::Grid::ChooseLayer::local_oversampled), void >::type* /*disable_for_local_grid_parts*/ = nullptr
#endif
        );

#if HAVE_DUNE_GRID_MULTISCALE

  SWIPDG(const MsGridProviderType& grid_provider,
         const Stuff::Common::Configuration& bound_inf_cfg,
         const ProblemType& prob,
         const int level_or_subdomain = 0,
         const std::vector< std::string >& only_these_products = {});

#endif // HAVE_DUNE_GRID_MULTISCALE

  void init(const bool prune = false);

private:
  friend class BlockSWIPDG< GridImp, RangeFieldImp, rangeDim, polynomialOrder, la_backend >;

  const RangeFieldType beta_;
  using BaseType::pattern_;
  const std::vector< std::string > only_these_products_;
}; // class SWIPDG

#if HAVE_ALUGRID && HAVE_DUNE_FEM
# if HAVE_DUNE_ISTL

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::leaf,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                        Stuff::Grid::ChooseLayer::leaf,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::istl_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::leaf >(GridProviderType&,
                                               const Stuff::Common::Configuration&,
                                               const ProblemType&,
                                               const int level_or_subdomain,
                                               const std::vector< std::string >&,
                                               void*);

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::level,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                        Stuff::Grid::ChooseLayer::level,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::istl_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::level >(GridProviderType&,
                                                const Stuff::Common::Configuration&,
                                                const ProblemType&,
                                                const int level_or_subdomain,
                                                const std::vector< std::string >&,
                                                void*);

#   if HAVE_MPI

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                              Stuff::Grid::ChooseLayer::leaf,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                        Stuff::Grid::ChooseLayer::leaf,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::istl_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::leaf >(GridProviderType&,
                                               const Stuff::Common::Configuration&,
                                               const ProblemType&,
                                               const int level_or_subdomain,
                                               const std::vector< std::string >&,
                                               void*);

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                              Stuff::Grid::ChooseLayer::level,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                        Stuff::Grid::ChooseLayer::level,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::istl_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::level >(GridProviderType&,
                                                const Stuff::Common::Configuration&,
                                                const ProblemType&,
                                                const int level_or_subdomain,
                                                const std::vector< std::string >&,
                                                void*);

#   endif // HAVE_MPI
#   if HAVE_DUNE_GRID_MULTISCALE

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::local,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::local_oversampled,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;

#     if HAVE_MPI

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                              Stuff::Grid::ChooseLayer::local,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                              Stuff::Grid::ChooseLayer::local_oversampled,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::istl_sparse >;

#     endif // HAVE_MPI
#   endif // HAVE_DUNE_GRID_MULTISCALE
# endif // HAVE_DUNE_ISTL
# if HAVE_EIGEN

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::leaf,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                        Stuff::Grid::ChooseLayer::leaf,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::eigen_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::leaf >(GridProviderType&,
                                               const Stuff::Common::Configuration&,
                                               const ProblemType&,
                                               const int level_or_subdomain,
                                               const std::vector< std::string >&,
                                               void*);

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::level,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                        Stuff::Grid::ChooseLayer::level,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::eigen_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::level >(GridProviderType&,
                                                const Stuff::Common::Configuration&,
                                                const ProblemType&,
                                                const int level_or_subdomain,
                                                const std::vector< std::string >&,
                                                void*);

#   if HAVE_MPI

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                              Stuff::Grid::ChooseLayer::leaf,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                        Stuff::Grid::ChooseLayer::leaf,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::eigen_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::leaf >(GridProviderType&,
                                               const Stuff::Common::Configuration&,
                                               const ProblemType&,
                                               const int level_or_subdomain,
                                               const std::vector< std::string >&,
                                               void*);

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                              Stuff::Grid::ChooseLayer::level,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;
extern template SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm  >,
                        Stuff::Grid::ChooseLayer::level,
                        double,
                        1,
                        1,
                        GDT::ChooseSpaceBackend::fem,
                        Stuff::LA::ChooseBackend::eigen_sparse >
    ::SWIPDG< Stuff::Grid::ChooseLayer::level >(GridProviderType&,
                                                const Stuff::Common::Configuration&,
                                                const ProblemType&,
                                                const int level_or_subdomain,
                                                const std::vector< std::string >&,
                                                void*);

#   endif // HAVE_MPI
#   if HAVE_DUNE_GRID_MULTISCALE

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::local,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, No_Comm >,
                              Stuff::Grid::ChooseLayer::local_oversampled,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;

#     if HAVE_MPI

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                              Stuff::Grid::ChooseLayer::local,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;

extern template class SWIPDG< ALUGrid< 2, 2, simplex, conforming, MPI_Comm >,
                              Stuff::Grid::ChooseLayer::local_oversampled,
                              double,
                              1,
                              1,
                              GDT::ChooseSpaceBackend::fem,
                              Stuff::LA::ChooseBackend::eigen_sparse >;

#     endif // HAVE_MPI
#   endif // HAVE_DUNE_GRID_MULTISCALE
# endif // HAVE_EIGEN
#endif // HAVE_ALUGRID && HAVE_DUNE_FEM

} // namespace Discretizations
} // namespace LinearElliptic
} // namespace HDD
} // namespace Dune

#endif // DUNE_HDD_LINEARELLIPTIC_DISCRETIZATIONS_SWIPDG_HH


#include <test/test-models/good/variational/eta_should_be_big.hpp>
#include <stan/variational/advi.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <gtest/gtest.h>
#include <test/unit/util.hpp>
#include <vector>
#include <string>
#include <iostream>
#include <boost/random/additive_combine.hpp>  // L'Ecuyer RNG

typedef boost::ecuyer1988 rng_t;

class eta_adapt_big_test : public ::testing::Test {
 public:
  eta_adapt_big_test()
      : logger(log_stream_, log_stream_, log_stream_, log_stream_,
               log_stream_) {}

  void SetUp() {
    static const std::string DATA = "";
    std::stringstream data_stream(DATA);
    stan::io::dump data_var_context(data_stream);

    model_ = new stan_model(data_var_context, 0, &model_stream_);
    cont_params_ = Eigen::VectorXd::Zero(model_->num_params_r());
    base_rng_.seed(927802408);
    model_stream_.str("");
    log_stream_.str("");

    advi_meanfield_ = new stan::variational::advi<
        stan_model, stan::variational::normal_meanfield, rng_t>(
        *model_, cont_params_, base_rng_, 1, 100, 100, 1);

    advi_fullrank_ = new stan::variational::advi<
        stan_model, stan::variational::normal_fullrank, rng_t>(
        *model_, cont_params_, base_rng_, 1, 100, 100, 1);

    advi_lowrank_ = new stan::variational::advi_lowrank<stan_model, rng_t>(
        *model_, cont_params_, base_rng_, 1, 1, 100, 100, 1);
  }

  void TearDown() {
    delete advi_meanfield_;
    delete advi_fullrank_;
    delete advi_lowrank_;
    delete model_;
  }

  stan::variational::advi<stan_model, stan::variational::normal_meanfield,
                          rng_t> *advi_meanfield_;
  stan::variational::advi<stan_model, stan::variational::normal_fullrank, rng_t>
      *advi_fullrank_;
  stan::variational::advi_lowrank<stan_model, rng_t> *advi_lowrank_;
  std::stringstream model_stream_;
  std::stringstream log_stream_;
  stan::callbacks::stream_logger logger;

  stan_model *model_;
  rng_t base_rng_;
  Eigen::VectorXd cont_params_;
};

TEST_F(eta_adapt_big_test, eta_should_be_big) {
  stan::variational::normal_meanfield meanfield_init
      = stan::variational::normal_meanfield(cont_params_);
  stan::variational::normal_fullrank fullrank_init
      = stan::variational::normal_fullrank(cont_params_);
  stan::variational::normal_lowrank lowrank_init
      = stan::variational::normal_lowrank(cont_params_, 1);

  EXPECT_EQ(100.0, advi_meanfield_->adapt_eta(meanfield_init, 50, logger));
  EXPECT_EQ(100.0, advi_fullrank_->adapt_eta(fullrank_init, 50, logger));
  EXPECT_EQ(100.0, advi_lowrank_->adapt_eta(lowrank_init, 50, logger));
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

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

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

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

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

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

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

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

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

#endif


#include <iostream>
#include <sstream>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>
#include <errno.h>
#include <math.h>
#include <string.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <termios.h>
#include <stdint.h>
#include <stdbool.h>
#include <vector>
#include <algorithm>
#include <Eigen/Dense>

// ROS Headers
#include <ros/ros.h>
#include <geometry_msgs/Accel.h>
#include <geometry_msgs/Wrench.h>
#include <geometry_msgs/TransformStamped.h>
#include <gazebo/gazebo.hh>
#include <gazebo_msgs/ModelState.h>
#include <gazebo_msgs/SetModelState.h>
#include <gazebo_msgs/ApplyBodyWrench.h>
#include <gazebo_msgs/ModelStates.h>
#include <tf/transform_listener.h>

#include "ogm_ae/Robot.hpp"
#include "ogm_ae/Mapping.hpp"
#include "ogm_ae/Exploration.hpp"
#include "Mapping.cpp"// no map object in this node, but a required class for exploration class

using namespace std;

void PublishDesiredQuadrotorState(
    ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg,
    geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
    Eigen::Vector3d& b1D);

void PublishDesiredQuadrotorState(
    ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg,
    geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
    Eigen::Vector3d& b1D, geometry_msgs::Vector3& x3Dot, geometry_msgs::Vector3& x4Dot);

void DoYawRotation(ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg, bool& pubDesiredState, ros::Publisher& desPosePub, ros::Publisher& desTwistPub, ros::Publisher& desAccelPub,
                   ros::Publisher& gazeboPub, geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel, gazebo_msgs::ModelState& ModelStateMsg, ogm_ae::ModelStateStamped& ModelStateStampedMsg, double& yawRotDuration, bool& simulateGazebo);

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

  // Initializations
  ros::init(argc, argv, "paths_to_cmd");
  string objective = "exploration";

  // Private Parameters: Robot & Sensor Reference Frames
  ros::NodeHandle nh, privateNH("~"); int robotNumber(-1); double mass; bool pubDesiredState(true);
  privateNH.param("robot_number", robotNumber, robotNumber);
  privateNH.param("mass", mass, mass);
  bool runAutonomousExploration(false);

  // Robot
  Robot robot(objective, robotNumber);
  vector<Robot> robotVec(1, robot);
  int numRobots; ros::param::get("num_robots", numRobots);

  // Exploration
  Exploration explore(robotVec, numRobots);
  robot = robotVec[0];
  int trajDim;

  ros::param::get("/exploration/simulate_gazebo", explore.simulateGazebo);

  string mapFrame, modelName, sensorFrame, robotFrame;
  ros::param::get("/mapping/frame", mapFrame);

  modelName   = robot.ns         ;
  robotFrame  = robot.robotFrame ;
  sensorFrame = robot.exploreFrame;

  ROS_INFO("Robot %d (namespace %s): Body Frame = %s, Sensor Frame = %s",
           robotNumber, modelName.c_str(), robotFrame.c_str(), sensorFrame.c_str());

  // Times for Gazebo Loading & Initial Yaw Rotation
  double tSecsToLoad, yawRotDuration;
  if(explore.simulateGazebo)
    ros::param::get("/exploration/gazebo_load_time", tSecsToLoad);
  else
    tSecsToLoad = 1.0;// 1 sec minimum for map reduction parameters
  ros::param::get("/exploration/yaw_rotation_time", yawRotDuration);
  bool doYawRot(false);

  // Cancel Gravity
  ros::ServiceClient WrenchClient = nh.serviceClient<gazebo_msgs::ApplyBodyWrench>("/gazebo/apply_body_wrench");
  gazebo_msgs::ApplyBodyWrench wrenchCmdsSRV;

  // Camera Pitch
  explore.radiansCamPitch = explore.degreeCamPitch*M_PI/180;

  // Subscriber
  ros::Subscriber trajInfoSub, ModelStatesSub;
  GenSubscriber<ogm_ae::PolyLeastSquaresTraj> trajInfoSubscription   ;
  GenSubscriber<gazebo_msgs::ModelStates>     modelStatesSubscription;
  trajInfoSub = nh.subscribe
      (robot.trajInfoTopic   , 1, &GenSubscriber<ogm_ae::PolyLeastSquaresTraj>::callback, &trajInfoSubscription   );
  ModelStatesSub = nh.subscribe
      ("/gazebo/model_states", 1, &GenSubscriber<gazebo_msgs::ModelStates>::callback    , &modelStatesSubscription);

  // Publishers
  ros::Publisher desPosePub, desCamPub, desTwistPub, desAccelPub, gazeboPub, quadrotorPub;
  desPosePub  = nh.advertise
      <geometry_msgs::PoseStamped >
      (robot.desiredPoseTopic,    1, true);
  desCamPub   = nh.advertise
      <geometry_msgs::PoseStamped >
      (robot.desiredCamTopic,     1, true);

  // TODO: erase 2 below
  desTwistPub = nh.advertise
      <geometry_msgs::TwistStamped>
      (robot.desiredTwistTopic,   1, true);
  desAccelPub = nh.advertise
      <geometry_msgs::AccelStamped>
      (robot.desiredAccelTopic,   1, true);

  gazeboPub    = nh.advertise
      <ogm_ae::ModelStateStamped  >
      ("gazebo_states_topic_"+to_string(robotNumber), 1, true);

  quadrotorPub = nh.advertise
      <ogm_ae::DesiredState  >
      ("/quadrotor0/desired_traj", 1, true);

  // TODO: follow something like this:
//  if(simulation)
//    moveRobotPub   = nh.advertise
//        <ogm_ae::ModelStateStamped  >
//        ("gazebo_states_topic_"+to_string(robotNumber), 1, true);
//  else

  geometry_msgs::Point zeroPt;
  zeroPt.x = 0.0;
  zeroPt.y = 0.0;
  zeroPt.z = 0.0;
  geometry_msgs::Vector3 zeroVector3;
  zeroVector3.x = 0.0;
  zeroVector3.y = 0.0;
  zeroVector3.z = 0.0;

  geometry_msgs::PoseStamped  robotPose, robotPoseSave, cameraPose;
  robotPose.pose.position = zeroPt;
  ros::param::get("/exploration/height", explore.height);
  robotPose.pose.position.z = explore.height;

  geometry_msgs::TwistStamped robotVel;
  robotVel.twist.linear  = zeroVector3;
  robotVel.twist.angular = zeroVector3;

  geometry_msgs::AccelStamped robotAccel;
  robotAccel.accel.linear  = zeroVector3;
  robotAccel.accel.angular = zeroVector3;

  robotPose .header.frame_id = mapFrame;
  cameraPose.header.frame_id = mapFrame;
  robotVel  .header.frame_id = mapFrame;
  robotAccel.header.frame_id = mapFrame;

  ogm_ae::ModelStateStamped ModelStateStampedMsg;
  gazebo_msgs::ModelState ModelStateMsg;
  ModelStateMsg.model_name = modelName;
  ModelStateMsg.reference_frame = mapFrame;

  ogm_ae::DesiredState DesiredStateMsg;
  DesiredStateMsg.xD     .resize(3, 0.0);
  DesiredStateMsg.xD1Dot .resize(3, 0.0);
  DesiredStateMsg.xD2Dot .resize(3, 0.0);
  DesiredStateMsg.xD3Dot .resize(3, 0.0);
  DesiredStateMsg.xD4Dot .resize(3, 0.0);
  DesiredStateMsg.b1D    .resize(3, 0.0);
  DesiredStateMsg.b1D1Dot.resize(3, 0.0);
  DesiredStateMsg.b1D2Dot.resize(3, 0.0);
  Eigen::Vector3d b1D(1.0, 0.0, 0.0), b1D1Dot, b1D2Dot, omegaD;
  geometry_msgs::Vector3 x3Dot, x4Dot;

  ROS_INFO("Waiting %e seconds for Gazebo to load...", tSecsToLoad);
  usleep(tSecsToLoad*1000000);
  ROS_INFO("Beginning desired trajectory commands.");

  // Get TFs
  explore.GetRobotCameraTFs(robot);

  // Initialized Last Pose as Current
  robotPoseSave = robotPose;

  // Height Difference Between the Robot and Exploration Sensor
  tf::StampedTransform robotTF;
  robotTF = LoopUntilTransformAcquired(explore.tfListener, robotFrame, sensorFrame);
  explore.height -= robotTF.getOrigin().z();// corrected for robot COM

  wrenchCmdsSRV.request.body_name = modelName+"::base_link";
  wrenchCmdsSRV.request.reference_frame = "world";
  wrenchCmdsSRV.request.reference_point.x = 0.0;
  wrenchCmdsSRV.request.reference_point.y = 0.0;
  wrenchCmdsSRV.request.reference_point.z = 0.0;
  wrenchCmdsSRV.request.start_time = ros::Time(0.0);
  wrenchCmdsSRV.request.duration = ros::Duration(-1.0);// apply continuously until new command

  wrenchCmdsSRV.request.wrench.force.x = 0.0;
  wrenchCmdsSRV.request.wrench.force.y = 0.0;
  wrenchCmdsSRV.request.wrench.force.z = mass*9.81;

  wrenchCmdsSRV.request.wrench.torque.x = 0.0;
  wrenchCmdsSRV.request.wrench.torque.y = 0.0;
  wrenchCmdsSRV.request.wrench.torque.z = 0.0;

  WrenchClient.call(wrenchCmdsSRV);
  if(!wrenchCmdsSRV.response.success)
      cout << "Fail! Response message:\n" << wrenchCmdsSRV.response.status_message << endl;

  bool firstPass(true), msgOK(false);

  // Spin with 3 Subscribers (paths of position, velocity, & acceleration)
  while (ros::ok()){

    int modelInd(-1);
    for(int i(0); i < modelStatesSubscription.msg.name.size(); i++){
      if(modelStatesSubscription.msg.name[i] == modelName){
        modelInd = i;
        break;
      }
    }
    if(modelInd == -1)
      ROS_WARN("Model %s not published by Gazebo.", modelName.c_str());
    else{
      robotPose.pose = modelStatesSubscription.msg.pose[modelInd];
      robotVel.twist = modelStatesSubscription.msg.twist[modelInd];

      //    // Current Robot Pose
      //    robotTF = LoopUntilTransformAcquired(explore.tfListener, mapFrame, robotFrame);
      //    robotPose.pose.position.x = robotTF.getOrigin().x();
      //    robotPose.pose.position.y = robotTF.getOrigin().y();
          robotPose.pose.position.z = explore.height   ;
      //    robotPose.pose.orientation.x = robotTF.getRotation().x();
      //    robotPose.pose.orientation.y = robotTF.getRotation().y();
      //    robotPose.pose.orientation.z = robotTF.getRotation().z();
      //    robotPose.pose.orientation.w = robotTF.getRotation().w();

          if(firstPass){
            robotPoseSave = robotPose;
            firstPass = false;
          }

          // Check if Autonomous Exploration Underway (Boolean) & New Messages
          ros::param::get("run_autonomous_exploration", runAutonomousExploration);

          // Yaw Rotation at Beginning or Later on Command
          ros::param::get("do_yaw_rotation", doYawRot);
          if(doYawRot){

            // Do a Yaw Rotation
            DoYawRotation(quadrotorPub, DesiredStateMsg, pubDesiredState, desPosePub, desTwistPub, desAccelPub, gazeboPub, robotPose, robotVel, robotAccel, ModelStateMsg, ModelStateStampedMsg, yawRotDuration, explore.simulateGazebo);
            robotPoseSave = robotPose;

            // Don't Repeat This
            ros::param::set("do_yaw_rotation", false);
            ros::param::set("run_autonomous_exploration", true);

          }

          // Update Message if New
          if(trajInfoSubscription.msg.header.stamp.toSec() <= ros::Time::now().toSec()
          && trajInfoSubscription.msg.timeStartSeg.size() > 0){
            robot.trajInfo = trajInfoSubscription.msg;
            if(robot.trajInfo.zCoeff.size() > 0)
              trajDim = 3;
            else
              trajDim = 2;
            msgOK = true;
          }

          // Pose Data from Function Parameters
          if(msgOK && runAutonomousExploration){

            // Current Time
            ros::Time rosTimeNow = ros::Time::now();

            // Robot Trajectory Function
            b1D = explore.GetTrajFromCoeff(trajDim, robot, rosTimeNow, cameraPose, robotPose, robotVel, robotAccel, x3Dot, x4Dot);

            // Publish Camera
            desCamPub.publish(cameraPose);

            // Save Latest Pose
            robotPoseSave = robotPose;

          }
          else{

            // Fixed Location & Attitude
            robotPose = robotPoseSave;

            // Correct Attitude to e1/e2 Plane if Necessary
            if(robotPose.pose.orientation.x != 0.0 || robotPose.pose.orientation.y != 0.0){
              Eigen::Vector3d b1Sensor;
              robotPose.pose.orientation = MakeRollAndPitch0(robotPose.pose.orientation, b1Sensor, 1);
              robotPoseSave = robotPose;
            }

            // Zero Time Derivatives
            robotVel.twist  .linear  = zeroVector3;
            robotVel.twist  .angular = zeroVector3;
            robotAccel.accel.linear  = zeroVector3;
            robotAccel.accel.angular = zeroVector3;

            // Time Stamps
            robotPose.header.stamp  = ros::Time::now();
            robotVel.header.stamp   = ros::Time::now();
            robotAccel.header.stamp = ros::Time::now();
          }

          // Publish Robot
          desPosePub .publish(robotPose );
          desTwistPub.publish(robotVel  );
          desAccelPub.publish(robotAccel);

          // Gazebo Simulation
          if(explore.simulateGazebo){

            ModelStateMsg.pose  = robotPose.pose ;
            ModelStateMsg.twist = robotVel .twist;
            ModelStateStampedMsg.model_state = ModelStateMsg;
            ModelStateStampedMsg.header.stamp = ros::Time::now();
            gazeboPub.publish(ModelStateStampedMsg);
          }

          if(pubDesiredState)
            PublishDesiredQuadrotorState(quadrotorPub, DesiredStateMsg, robotPose, robotVel, robotAccel, b1D, x3Dot, x4Dot);
    }

    ros::spinOnce();

  }
  return 0;
}

void PublishDesiredQuadrotorState(
    ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg,
    geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
    Eigen::Vector3d& b1D){

  // Overloaded Case with 0 3rd & 4th Translational Derivatives
   geometry_msgs::Vector3 x3Dot, x4Dot;
   x3Dot.x = 0.0; x3Dot.y = 0.0; x3Dot.z = 0.0;
   x4Dot.x = 0.0; x4Dot.y = 0.0; x4Dot.z = 0.0;

  PublishDesiredQuadrotorState(quadrotorPub, DesiredStateMsg, robotPose, robotVel, robotAccel, b1D, x3Dot, x4Dot);

  return;
}

void PublishDesiredQuadrotorState(
    ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg,
    geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
    Eigen::Vector3d& b1D, geometry_msgs::Vector3& x3Dot, geometry_msgs::Vector3& x4Dot){

  // Initializations
  Eigen::Vector3d b1D1Dot, b1D2Dot, omegaD;

  // Desired Position & Time Derivatives
  ROSXYZTypeToVec(robotPose.pose.position, DesiredStateMsg.xD    );
  ROSXYZTypeToVec(robotVel.twist.linear  , DesiredStateMsg.xD1Dot);
  ROSXYZTypeToVec(robotAccel.accel.linear, DesiredStateMsg.xD2Dot);
  ROSXYZTypeToVec(x3Dot                  , DesiredStateMsg.xD3Dot);
  ROSXYZTypeToVec(x4Dot                  , DesiredStateMsg.xD4Dot);

  // Desired Direction of 1st Body-Fixed Axis & Time Derivatives
  omegaD << 0.0, 0.0, robotVel.twist.angular.z;
  b1D1Dot = omegaD.cross(b1D    );
  b1D2Dot = omegaD.cross(b1D1Dot);// note: 0 angular acceleration

  EigenVecToStdVec(b1D    , DesiredStateMsg.b1D    );
  EigenVecToStdVec(b1D1Dot, DesiredStateMsg.b1D1Dot);
  EigenVecToStdVec(b1D2Dot, DesiredStateMsg.b1D2Dot);

  quadrotorPub.publish(DesiredStateMsg);

  return;
}

void DoYawRotation(ros::Publisher& quadrotorPub, ogm_ae::DesiredState& DesiredStateMsg, bool& pubDesiredState, ros::Publisher& desPosePub, ros::Publisher& desTwistPub, ros::Publisher& desAccelPub, ros::Publisher& gazeboPub,
                   geometry_msgs::PoseStamped& robotPose, geometry_msgs::TwistStamped& robotVel, geometry_msgs::AccelStamped& robotAccel,
                   gazebo_msgs::ModelState& ModelStateMsg, ogm_ae::ModelStateStamped& ModelStateStampedMsg, double& yawRotDuration, bool& simulateGazebo){

  // Initial Values for Reseting at End
  geometry_msgs::Quaternion quatInit = robotPose.pose.orientation;
  double angVelZInit = robotVel.twist.angular.z, thetaInit;
  QuatToYawRot(thetaInit, quatInit);

  ros::Time yawRotTimeNow = ros::Time::now();
  double yawRotTimeStartSecs = yawRotTimeNow.toSec();
  robotVel.twist.angular.z = 2*M_PI/yawRotDuration;
  while(yawRotTimeNow.toSec() < yawRotTimeStartSecs+yawRotDuration){

    // Pose
    double yawRotTheta
        = 2*M_PI*(yawRotTimeNow.toSec()-yawRotTimeStartSecs)/yawRotDuration
        +thetaInit;
    geometry_msgs::Quaternion quat;
    YawRotToQuat(yawRotTheta, quat);
    Eigen::Vector3d b1D(cos(yawRotTheta), sin(yawRotTheta), 0.0);
    robotPose.pose.orientation = quat;
    robotPose.header.stamp = yawRotTimeNow;
    desPosePub.publish(robotPose);

    // Velocity
    robotVel.header.stamp = yawRotTimeNow;
    desTwistPub.publish(robotVel);

    // Acceleration
    robotAccel.header.stamp = yawRotTimeNow;
    desAccelPub.publish(robotAccel);

    // Gazebo
    if(simulateGazebo){
      ModelStateMsg.pose  = robotPose.pose ;
      ModelStateMsg.twist = robotVel .twist;
      ModelStateStampedMsg.model_state = ModelStateMsg;
      ModelStateStampedMsg.header.stamp = ros::Time::now();
      gazeboPub.publish(ModelStateStampedMsg);
    }

    // Quadrotor Flight Control
    if(pubDesiredState)
      PublishDesiredQuadrotorState(quadrotorPub, DesiredStateMsg, robotPose, robotVel, robotAccel, b1D);

    // Current Time & Spin
    yawRotTimeNow = ros::Time::now();
    ros::spinOnce();
  }

  // Reset Changed Values
  robotPose.pose.orientation = quatInit;
  robotVel.twist.angular.z = angVelZInit;

  return;
}


/*!
 * \file password.cpp
 *
 * \copyright Copyright 2013 (C) Morpheus Being
 *
 * \author Morpheus Being
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
*/

// compile as
// g++ -o password password.cpp -lboost_random

#include <iostream>                                        // provide access to cout, cin, cerr
#include <string>                                          // provide access to string
#include <vector>                                          // provide vector

#include <boost/random/random_device.hpp>
#include <boost/nondet_random.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_int_distribution.hpp>

/**
 * @brief Create a pronouncable password
 *
 *
 * @param number - the number of passwords required to create
 */
void password_pronounce(int number)
{
    // We first define the characters that we're going
    //     to allow.  This is pretty much just the characters
    //     on a standard keyboard.
	std::string chars("bcdfghjklmnpqrstvwxyzBCDFGHJKLMNPQRSTVWXYZ");
	std::string vowels("aeiouAEIOU");
	std::string numbers("1234567890");
	std::string symbols("!@#$%^&*()`~-_=+[{]}\\|;:'\",<.>/? ");

    // We use __random_device as a source of entropy, since we want
    //     passwords that are not predictable.

    boost::random::random_device rng;

    // Now develop uniform distribution
    boost::random::uniform_int_distribution<> chars_dist(0, chars.size() - 1);
    boost::random::uniform_int_distribution<> vowels_dist(0, vowels.size() - 1);
    boost::random::uniform_int_distribution<> symbols_dist(0, symbols.size() - 1);
    boost::random::uniform_int_distribution<> numbers_dist(0, numbers.size() - 1);

    for (int i = 0; i < number; ++i) {
	    // Display group of char/vowel/char to get pronouncable password
    	for (int j = 0; j < 3; ++j) {
			std::cout << chars[chars_dist(rng)];
			std::cout << vowels[vowels_dist(rng)];
			std::cout << chars[chars_dist(rng)];
		}
    	// now add a symbol
		std::cout << symbols[symbols_dist(rng)];
		// now add 3 numbers to end
		for (int i = 0; i < 3; ++i) {
			std::cout << numbers[numbers_dist(rng)];
		}
    	std::cout << std::endl;
    }
}

/**
 * @brief Create long passwords for system use - not for remembering
 *
 * @param num - the number of passwords to create
 * @param len - the number of characters in each password
 */
void password_long(int& num, int len = 20)
{
   std::string chars(
        "abcdefghijklmnopqrstuvwxyz"
        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
        "1234567890"
        "!@#$%^&*()"
        "`~-_=+[{]}\\|;:'\",<.>/?");

    boost::random_device rng;
    boost::variate_generator<boost::random_device&, boost::random::uniform_int_distribution<> >
        gen(rng, boost::random::uniform_int_distribution<>(0, chars.size()));

    // the number asked for
    for (int i = 0; i < num; ++i) {
	    // of the length requested - if none, use standard length of 20
    	for (int j = 0; j < len; ++j) {
    		std::cout << chars[gen()];
    	}
		std::cout << std::endl;
    }
}

/**
 * @brief Show how to use the program.
 *
 * This can be called or shown on error on command line.
 *
 * @param name - the name of the calling program
 */
static void show_usage(std::string name)
{
			std::cerr << "usage: " << name << "  [options] " << std::endl;
			std::cerr << " l - create [n] long password of [n] (20 is default) characters for system useage." << std::endl;
			std::cerr << " p - create [n] pronouncable passwords as cvccvccvcsnnn " << std::endl;
			std::cerr << "                where c = constanant " << std::endl;
			std::cerr << "                      v = vowel" << std::endl;
			std::cerr << "                      s = symbol" << std::endl;
			std::cerr << "                      n = digit" << std::endl;
}

static void show_version(std::string name)
{
    std::cerr << name << " version: " << "0.1" << std::endl << std::endl;
}

int main(int argc, char *argv[])
{
   int length = 20;
   int number = 1;

   if (argc < 2) {
      show_usage(argv[0]);
      return 1;
   }

   std::vector <std::string> commandline;

   for (int i = 1; i < argc; ++i) {
	   std::string arg = argv[i];

	   if ((arg == "-h" ) || (arg == "--help") || (arg == "-?")) {
    	  show_usage(argv[0]);
	   }
       else if ((arg == "-v") || (arg == "--version")) {
           show_version(argv[0]);
       }
	   else if ((arg == "-l") || (arg == "--long")) {
		   // we want a set number of long passwords
		   if (i + 1 < argc) {
			   std::string myString = argv[++i];
		   	   number = atoi(myString.c_str());
		   }
		   // if yet another argument, we want them to be of set length
		   if (i + 1 < argc) {
			   std::string myString = argv[++i];
			   length = atoi(myString.c_str());
			   password_long(number, length);
		   } else {
               password_long(number);
		   }

	   } else if ((arg == "-p") || (arg == "--pronounce")) {
		   if (i + 1 < argc) {
			   std::string myString = argv[++i];
			   number = atoi(myString.c_str());
		   }
		   password_pronounce(number);
	   } else {
		   // we have a problem
		   std::cerr << "Houston, we have a problem." << std::endl;
		   std::cerr << "Why don't you try again with -h or --help option. " << std::endl;
	   }
    }
	return 0;
}

//   EOF


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IROUND2EVEN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_IROUND2EVEN_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/valmin.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/is_less.hpp>
#include <boost/simd/function/simd/splat.hpp>
#include <boost/simd/function/fast.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/is_nan.hpp>
#include <boost/simd/function/simd/if_zero_else.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(iround2even_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::single_<A0>, X>
                          )
   {
      using result = bd::as_integer_t<A0>;
      BOOST_FORCEINLINE result operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using sr_t = bd::scalar_of_t<result>;
        static const A0 Vax =  splat<A0>(bs::Valmax<sr_t>());
        static const A0 Vix =  splat<A0>(bs::Valmin<sr_t>());
      #ifndef BOOST_SIMD_NO_NANS
        A0 aa0 = if_zero_else(is_nan(a0), a0);
        return if_else(bs::is_less(aa0, Vix), Valmin<result>(),
                       if_else(bs::is_greater(aa0, Vax), Valmax<result>(),
                               bs::fast_(iround2even)(aa0)
                              )
                      );
      #else
        return if_else(bs::is_less(a0, Vix), Valmin<result>(),
                       if_else(bs::is_greater(a0, Vax), Valmax<result>(),
                               bs::fast_(iround2even(a0))
                              )
                      );
      #endif
      }
   };

} } }

#endif


#include "itkFitFibersToImageFilter.h"

#include <boost/progress.hpp>

namespace itk{

FitFibersToImageFilter::FitFibersToImageFilter()
  : m_PeakImage(nullptr)
  , m_MaskImage(nullptr)
  , m_FitIndividualFibers(true)
  , m_GradientTolerance(1e-5)
  , m_Lambda(0.1)
  , m_MaxIterations(20)
  , m_FiberSampling(10)
  , m_Coverage(0)
  , m_Overshoot(0)
  , m_RMSE(0.0)
  , m_FilterOutliers(true)
  , m_MeanWeight(1.0)
  , m_MedianWeight(1.0)
  , m_MinWeight(1.0)
  , m_MaxWeight(1.0)
  , m_Verbose(true)
  , m_DeepCopy(true)
  , m_ResampleFibers(true)
  , m_NumUnknowns(0)
  , m_NumResiduals(0)
  , m_NumCoveredDirections(0)
  , m_SignalModel(nullptr)
  , sz_x(0)
  , sz_y(0)
  , sz_z(0)
  , TD(0)
  , FD(0)
  , fiber_count(0)
{
  this->SetNumberOfRequiredOutputs(3);
}

FitFibersToImageFilter::~FitFibersToImageFilter()
{

}

void FitFibersToImageFilter::CreateDiffSystem()
{
  sz_x = m_DiffImage->GetLargestPossibleRegion().GetSize(0);
  sz_y = m_DiffImage->GetLargestPossibleRegion().GetSize(1);
  sz_z = m_DiffImage->GetLargestPossibleRegion().GetSize(2);
  dim_four_size = m_DiffImage->GetVectorLength();
  int num_voxels = sz_x*sz_y*sz_z;

  float minSpacing = 1;
  if(m_DiffImage->GetSpacing()[0]<m_DiffImage->GetSpacing()[1] && m_DiffImage->GetSpacing()[0]<m_DiffImage->GetSpacing()[2])
    minSpacing = m_DiffImage->GetSpacing()[0];
  else if (m_DiffImage->GetSpacing()[1] < m_DiffImage->GetSpacing()[2])
    minSpacing = m_DiffImage->GetSpacing()[1];
  else
    minSpacing = m_DiffImage->GetSpacing()[2];

  if (m_ResampleFibers)
    for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
    {
      std::streambuf *old = cout.rdbuf(); // <-- save
      std::stringstream ss;
      std::cout.rdbuf (ss.rdbuf());
      if (m_DeepCopy)
        m_Tractograms[bundle] = m_Tractograms.at(bundle)->GetDeepCopy();
      m_Tractograms.at(bundle)->ResampleLinear(minSpacing/m_FiberSampling);
      std::cout.rdbuf (old);
    }

  m_NumResiduals = num_voxels * dim_four_size;

  MITK_INFO << "Num. unknowns: " << m_NumUnknowns;
  MITK_INFO << "Num. residuals: " << m_NumResiduals;
  MITK_INFO << "Creating system ...";

  A.set_size(m_NumResiduals, m_NumUnknowns);
  b.set_size(m_NumResiduals); b.fill(0.0);

  TD = 0;
  FD = 0;
  m_NumCoveredDirections = 0;
  fiber_count = 0;
  vnl_vector<int> voxel_indicator; voxel_indicator.set_size(sz_x*sz_y*sz_z); voxel_indicator.fill(0);

  for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
  {
    vtkSmartPointer<vtkPolyData> polydata = m_Tractograms.at(bundle)->GetFiberPolyData();

    for (int i=0; i<m_Tractograms.at(bundle)->GetNumFibers(); ++i)
    {
      vtkCell* cell = polydata->GetCell(i);
      int numPoints = cell->GetNumberOfPoints();
      vtkPoints* points = cell->GetPoints();

      if (numPoints<2)
        MITK_INFO << "FIBER WITH ONLY ONE POINT ENCOUNTERED!";

      for (int j=0; j<numPoints-1; ++j)
      {
        double* p1 = points->GetPoint(j);
        PointType3 p;
        p[0]=p1[0];
        p[1]=p1[1];
        p[2]=p1[2];

        itk::Index<3> idx3;
        m_DiffImage->TransformPhysicalPointToIndex(p, idx3);
        if (!m_DiffImage->GetLargestPossibleRegion().IsInside(idx3) || (m_MaskImage.IsNotNull() && m_MaskImage->GetPixel(idx3)==0))
          continue;

        double* p2 = points->GetPoint(j+1);
        mitk::DiffusionSignalModel<>::GradientType fiber_dir;
        fiber_dir[0] = p[0]-p2[0];
        fiber_dir[1] = p[1]-p2[1];
        fiber_dir[2] = p[2]-p2[2];
        fiber_dir.Normalize();

        int x = idx3[0];
        int y = idx3[1];
        int z = idx3[2];

        m_SignalModel->SetFiberDirection(fiber_dir);
        mitk::DiffusionSignalModel<>::PixelType simulated_pixel = m_SignalModel->SimulateMeasurement();
        VectorImgType::PixelType measured_pixel = m_DiffImage->GetPixel(idx3);

        double simulated_mean = 0;
        double measured_mean = 0;
        int num_nonzero_g = 0;
        for (int g=0; g<dim_four_size; ++g)
        {
          if( m_SignalModel->GetGradientDirection(g).GetNorm()<mitk::eps )
            continue;
          simulated_mean += simulated_pixel[g];
          measured_mean += (double)measured_pixel[g];
          ++num_nonzero_g;
        }
        simulated_mean /= num_nonzero_g;
        measured_mean /= num_nonzero_g;
        simulated_pixel -= simulated_mean;

        if (voxel_indicator[x + sz_x*y + sz_x*sz_y*z]==0)
          FD += measured_mean;
        TD += simulated_mean;
        voxel_indicator[x + sz_x*y + sz_x*sz_y*z] = 1;

        for (int g=0; g<dim_four_size; ++g)
        {
          unsigned int linear_index = x + sz_x*y + sz_x*sz_y*z + sz_x*sz_y*sz_z*g;

          if (m_FitIndividualFibers)
          {
            b[linear_index] = (double)measured_pixel[g] - measured_mean;
            A.put(linear_index, fiber_count, A.get(linear_index, fiber_count) + simulated_pixel[g]);
          }
          else
          {
            b[linear_index] = (double)measured_pixel[g] - measured_mean;
            A.put(linear_index, bundle, A.get(linear_index, bundle) + simulated_pixel[g]);
          }
        }

      }

      ++fiber_count;
    }
  }

  m_NumCoveredDirections = voxel_indicator.sum();
  TD /= (m_NumCoveredDirections*fiber_count);
  FD /= m_NumCoveredDirections;
  A /= TD;
  b *= 100.0/FD;  // times 100 because we want to avoid too small values for computational reasons
}

void FitFibersToImageFilter::CreatePeakSystem()
{
  sz_x = m_PeakImage->GetLargestPossibleRegion().GetSize(0);
  sz_y = m_PeakImage->GetLargestPossibleRegion().GetSize(1);
  sz_z = m_PeakImage->GetLargestPossibleRegion().GetSize(2);
  dim_four_size = m_PeakImage->GetLargestPossibleRegion().GetSize(3)/3 + 1; // +1 for zero - peak
  int num_voxels = sz_x*sz_y*sz_z;

  float minSpacing = 1;
  if(m_PeakImage->GetSpacing()[0]<m_PeakImage->GetSpacing()[1] && m_PeakImage->GetSpacing()[0]<m_PeakImage->GetSpacing()[2])
    minSpacing = m_PeakImage->GetSpacing()[0];
  else if (m_PeakImage->GetSpacing()[1] < m_PeakImage->GetSpacing()[2])
    minSpacing = m_PeakImage->GetSpacing()[1];
  else
    minSpacing = m_PeakImage->GetSpacing()[2];

  if (m_ResampleFibers)
    for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
    {
      std::streambuf *old = cout.rdbuf(); // <-- save
      std::stringstream ss;
      std::cout.rdbuf (ss.rdbuf());
      if (m_DeepCopy)
        m_Tractograms[bundle] = m_Tractograms.at(bundle)->GetDeepCopy();
      m_Tractograms.at(bundle)->ResampleLinear(minSpacing/m_FiberSampling);
      std::cout.rdbuf (old);
    }

  m_NumResiduals = num_voxels * dim_four_size;

  MITK_INFO << "Num. unknowns: " << m_NumUnknowns;
  MITK_INFO << "Num. residuals: " << m_NumResiduals;
  MITK_INFO << "Creating system ...";

  A.set_size(m_NumResiduals, m_NumUnknowns);
  b.set_size(m_NumResiduals); b.fill(0.0);

  TD = 0;
  FD = 0;
  m_NumCoveredDirections = 0;
  fiber_count = 0;

  for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
  {
    vtkSmartPointer<vtkPolyData> polydata = m_Tractograms.at(bundle)->GetFiberPolyData();

    for (int i=0; i<m_Tractograms.at(bundle)->GetNumFibers(); ++i)
    {
      vtkCell* cell = polydata->GetCell(i);
      int numPoints = cell->GetNumberOfPoints();
      vtkPoints* points = cell->GetPoints();

      if (numPoints<2)
        MITK_INFO << "FIBER WITH ONLY ONE POINT ENCOUNTERED!";

      for (int j=0; j<numPoints-1; ++j)
      {
        double* p1 = points->GetPoint(j);
        PointType4 p;
        p[0]=p1[0];
        p[1]=p1[1];
        p[2]=p1[2];
        p[3]=0;

        itk::Index<4> idx4;
        m_PeakImage->TransformPhysicalPointToIndex(p, idx4);
        itk::Index<3> idx3; idx3[0] = idx4[0]; idx3[1] = idx4[1]; idx3[2] = idx4[2];
        if (!m_PeakImage->GetLargestPossibleRegion().IsInside(idx4) || (m_MaskImage.IsNotNull() && m_MaskImage->GetPixel(idx3)==0))
          continue;

        double* p2 = points->GetPoint(j+1);
        vnl_vector_fixed<float,3> fiber_dir;
        fiber_dir[0] = p[0]-p2[0];
        fiber_dir[1] = p[1]-p2[1];
        fiber_dir[2] = p[2]-p2[2];
        fiber_dir.normalize();

        double w = 1;
        int peak_id = dim_four_size-1;
        vnl_vector_fixed<float,3> odf_peak = GetClosestPeak(idx4, m_PeakImage, fiber_dir, peak_id, w);
        float peak_mag = odf_peak.magnitude();

        int x = idx4[0];
        int y = idx4[1];
        int z = idx4[2];

        unsigned int linear_index = x + sz_x*y + sz_x*sz_y*z + sz_x*sz_y*sz_z*peak_id;

        if (b[linear_index] == 0 && peak_id<dim_four_size-1)
        {
          m_NumCoveredDirections++;
          FD += peak_mag;
        }
        TD += w;

        if (m_FitIndividualFibers)
        {
          b[linear_index] = peak_mag;
          A.put(linear_index, fiber_count, A.get(linear_index, fiber_count) + w);
        }
        else
        {
          b[linear_index] = peak_mag;
          A.put(linear_index, bundle, A.get(linear_index, bundle) + w);
        }
      }

      ++fiber_count;
    }
  }

  TD /= (m_NumCoveredDirections*fiber_count);
  FD /= m_NumCoveredDirections;
  A /= TD;
  b *= 100.0/FD;  // times 100 because we want to avoid too small values for computational reasons
}

void FitFibersToImageFilter::GenerateData()
{
  m_NumUnknowns = m_Tractograms.size();
  if (m_FitIndividualFibers)
  {
    m_NumUnknowns = 0;
    for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
      m_NumUnknowns += m_Tractograms.at(bundle)->GetNumFibers();
  }

  fiber_count = 0;
  sz_x = 0;
  sz_y = 0;
  sz_z = 0;
  TD = 0;
  FD = 0;
  if (m_PeakImage.IsNotNull())
    CreatePeakSystem();
  else if (m_DiffImage.IsNotNull())
    CreateDiffSystem();
  else
    mitkThrow() << "No input image set!";

  double init_lambda = fiber_count;  // initialization for lambda estimation

  itk::TimeProbe clock;
  clock.Start();

  cost = VnlCostFunction(m_NumUnknowns);
  cost.SetProblem(A, b, init_lambda);
  m_Weights.set_size(m_NumUnknowns); // m_Weights.fill( TD/100.0 * FD/2.0 );
  m_Weights.fill( 0.0 );
  vnl_lbfgsb minimizer(cost);
  vnl_vector<double> l; l.set_size(m_NumUnknowns); l.fill(0);
  vnl_vector<long> bound_selection; bound_selection.set_size(m_NumUnknowns); bound_selection.fill(1);
  minimizer.set_bound_selection(bound_selection);
  minimizer.set_lower_bound(l);
  minimizer.set_projected_gradient_tolerance(m_GradientTolerance);

  MITK_INFO << "Estimating regularization";
  minimizer.set_trace(false);
  minimizer.set_max_function_evals(2);
  minimizer.minimize(m_Weights);
  vnl_vector<double> dx; dx.set_size(m_NumUnknowns); dx.fill(0.0);
  cost.grad_regu_localMSE(m_Weights, dx);
  double r = dx.magnitude()/m_Weights.magnitude();
  cost.m_Lambda *= m_Lambda*55.0/r;
  MITK_INFO << r << " - " << m_Lambda*55.0/r;
  if (cost.m_Lambda>10e7)
  {
    MITK_INFO << "Regularization estimation failed. Using default value.";
    cost.m_Lambda = fiber_count;
  }
  MITK_INFO << "Using regularization factor of " << cost.m_Lambda << " (λ: " << m_Lambda << ")";

  MITK_INFO << "Fitting fibers";
  minimizer.set_trace(m_Verbose);

  minimizer.set_max_function_evals(m_MaxIterations);
  minimizer.minimize(m_Weights);

  std::vector< double > weights;
  if (m_FilterOutliers)
  {
    for (auto w : m_Weights)
      weights.push_back(w);
    std::sort(weights.begin(), weights.end());
    MITK_INFO << "Setting upper weight bound to " << weights.at(m_NumUnknowns*0.99);
    vnl_vector<double> u; u.set_size(m_NumUnknowns); u.fill(weights.at(m_NumUnknowns*0.99));
    minimizer.set_upper_bound(u);
    bound_selection.fill(2);
    minimizer.set_bound_selection(bound_selection);
    minimizer.minimize(m_Weights);
    weights.clear();
  }

  for (auto w : m_Weights)
    weights.push_back(w);
  std::sort(weights.begin(), weights.end());

  m_MeanWeight = m_Weights.mean();
  m_MedianWeight = weights.at(m_NumUnknowns*0.5);
  m_MinWeight = weights.at(0);
  m_MaxWeight = weights.at(m_NumUnknowns-1);

  MITK_INFO << "*************************";
  MITK_INFO << "Weight statistics";
  MITK_INFO << "Mean: " << m_MeanWeight;
  MITK_INFO << "Median: " << m_MedianWeight;
  MITK_INFO << "75% quantile: " << weights.at(m_NumUnknowns*0.75);
  MITK_INFO << "95% quantile: " << weights.at(m_NumUnknowns*0.95);
  MITK_INFO << "99% quantile: " << weights.at(m_NumUnknowns*0.99);
  MITK_INFO << "Min: " << m_MinWeight;
  MITK_INFO << "Max: " << m_MaxWeight;
  MITK_INFO << "*************************";
  MITK_INFO << "NumEvals: " << minimizer.get_num_evaluations();
  MITK_INFO << "NumIterations: " << minimizer.get_num_iterations();
  MITK_INFO << "Residual cost: " << minimizer.get_end_error();
  m_RMSE = cost.S->get_rms_error(m_Weights);
  MITK_INFO << "Final RMS: " << m_RMSE;

  clock.Stop();
  int h = clock.GetTotal()/3600;
  int m = ((int)clock.GetTotal()%3600)/60;
  int s = (int)clock.GetTotal()%60;
  MITK_INFO << "Optimization took " << h << "h, " << m << "m and " << s << "s";

  MITK_INFO << "Weighting fibers";
  m_RmsDiffPerFiber.set_size(m_Weights.size());
  m_RmsDiffPerBundle.set_size(m_Tractograms.size());
  std::streambuf *old = cout.rdbuf(); // <-- save
  std::stringstream ss;
  std::cout.rdbuf (ss.rdbuf());
  if (m_FitIndividualFibers)
  {
    unsigned int fiber_count = 0;
    for (unsigned int bundle=0; bundle<m_Tractograms.size(); bundle++)
    {
      vnl_vector< double > temp_weights;
      temp_weights.set_size(m_Weights.size());
      temp_weights.copy_in(m_Weights.data_block());

      for (int i=0; i<m_Tractograms.at(bundle)->GetNumFibers(); i++)
      {
        m_Tractograms.at(bundle)->SetFiberWeight(i, m_Weights[fiber_count]);
        temp_weights[fiber_count] = 0;

        ++fiber_count;
      }
      double d_rms = cost.S->get_rms_error(temp_weights) - m_RMSE;
      m_RmsDiffPerBundle[bundle] = d_rms;
      m_Tractograms.at(bundle)->Compress(0.1);
      m_Tractograms.at(bundle)->ColorFibersByFiberWeights(false, true);
    }
  }
  else
  {
    for (unsigned int i=0; i<m_Tractograms.size(); ++i)
    {
      m_Tractograms.at(i)->SetFiberWeights(m_Weights[i]);
      m_Tractograms.at(i)->Compress(0.1);
      m_Tractograms.at(i)->ColorFibersByFiberWeights(false, true);
    }
  }
  std::cout.rdbuf (old);

  // transform back
  A *= FD/100.0;
  b *= FD/100.0;

  MITK_INFO << "Generating output images ...";
  if (m_PeakImage.IsNotNull())
    GenerateOutputPeakImages();
  else if (m_DiffImage.IsNotNull())
    GenerateOutputDiffImages();

  m_Coverage = m_Coverage/FD;
  m_Overshoot = m_Overshoot/FD;

  MITK_INFO << std::fixed << "Coverage: " << setprecision(2) << 100.0*m_Coverage << "%";
  MITK_INFO << std::fixed << "Overshoot: " << setprecision(2) << 100.0*m_Overshoot << "%";
}

void FitFibersToImageFilter::GenerateOutputDiffImages()
{
  VectorImgType::PixelType pix; pix.SetSize(m_DiffImage->GetVectorLength()); pix.Fill(0);
  itk::ImageDuplicator< VectorImgType >::Pointer duplicator = itk::ImageDuplicator< VectorImgType >::New();
  duplicator->SetInputImage(m_DiffImage);
  duplicator->Update();
  m_UnderexplainedImageDiff = duplicator->GetOutput();
  m_UnderexplainedImageDiff->FillBuffer(pix);

  duplicator->SetInputImage(m_UnderexplainedImageDiff);
  duplicator->Update();
  m_OverexplainedImageDiff = duplicator->GetOutput();
  m_OverexplainedImageDiff->FillBuffer(pix);

  duplicator->SetInputImage(m_OverexplainedImageDiff);
  duplicator->Update();
  m_ResidualImageDiff = duplicator->GetOutput();
  m_ResidualImageDiff->FillBuffer(pix);

  duplicator->SetInputImage(m_ResidualImageDiff);
  duplicator->Update();
  m_FittedImageDiff = duplicator->GetOutput();
  m_FittedImageDiff->FillBuffer(pix);

  vnl_vector<double> fitted_b; fitted_b.set_size(b.size());
  cost.S->multiply(m_Weights, fitted_b);

  itk::ImageRegionIterator<VectorImgType> it1 = itk::ImageRegionIterator<VectorImgType>(m_DiffImage, m_DiffImage->GetLargestPossibleRegion());
  itk::ImageRegionIterator<VectorImgType> it2 = itk::ImageRegionIterator<VectorImgType>(m_FittedImageDiff, m_FittedImageDiff->GetLargestPossibleRegion());
  itk::ImageRegionIterator<VectorImgType> it3 = itk::ImageRegionIterator<VectorImgType>(m_ResidualImageDiff, m_ResidualImageDiff->GetLargestPossibleRegion());
  itk::ImageRegionIterator<VectorImgType> it4 = itk::ImageRegionIterator<VectorImgType>(m_UnderexplainedImageDiff, m_UnderexplainedImageDiff->GetLargestPossibleRegion());
  itk::ImageRegionIterator<VectorImgType> it5 = itk::ImageRegionIterator<VectorImgType>(m_OverexplainedImageDiff, m_OverexplainedImageDiff->GetLargestPossibleRegion());

  FD = 0;
  m_Coverage = 0;
  m_Overshoot = 0;

  while( !it2.IsAtEnd() )
  {
    itk::Index<3> idx3 = it2.GetIndex();
    VectorImgType::PixelType original_pix =it1.Get();
    VectorImgType::PixelType fitted_pix =it2.Get();
    VectorImgType::PixelType residual_pix =it3.Get();
    VectorImgType::PixelType underexplained_pix =it4.Get();
    VectorImgType::PixelType overexplained_pix =it5.Get();

    int num_nonzero_g = 0;
    double original_mean = 0;
    for (int g=0; g<dim_four_size; ++g)
    {
      if( m_SignalModel->GetGradientDirection(g).GetNorm()>=mitk::eps )
      {
        original_mean += original_pix[g];
        ++num_nonzero_g;
      }
    }
    original_mean /= num_nonzero_g;

    for (int g=0; g<dim_four_size; ++g)
    {
      unsigned int linear_index = idx3[0] + sz_x*idx3[1] + sz_x*sz_y*idx3[2] + sz_x*sz_y*sz_z*g;

      fitted_pix[g] = fitted_b[linear_index] + original_mean;
      residual_pix[g] = original_pix[g] - fitted_b[linear_index] - original_mean;

      if (residual_pix[g]<0)
      {
        overexplained_pix[g] = residual_pix[g];
        m_Coverage += b[linear_index] + original_mean;
        m_Overshoot -= residual_pix[g];
      }
      else if (residual_pix[g]>=0)
      {
        underexplained_pix[g] = residual_pix[g];
        m_Coverage += fitted_b[linear_index] + original_mean;
      }
      FD += b[linear_index] + original_mean;
    }

    it2.Set(fitted_pix);
    it3.Set(residual_pix);
    it4.Set(underexplained_pix);
    it5.Set(overexplained_pix);

    ++it1;
    ++it2;
    ++it3;
    ++it4;
    ++it5;
  }
}

void FitFibersToImageFilter::GenerateOutputPeakImages()
{
  itk::ImageDuplicator< PeakImgType >::Pointer duplicator = itk::ImageDuplicator< PeakImgType >::New();
  duplicator->SetInputImage(m_PeakImage);
  duplicator->Update();
  m_UnderexplainedImage = duplicator->GetOutput();
  m_UnderexplainedImage->FillBuffer(0.0);

  duplicator->SetInputImage(m_UnderexplainedImage);
  duplicator->Update();
  m_OverexplainedImage = duplicator->GetOutput();
  m_OverexplainedImage->FillBuffer(0.0);

  duplicator->SetInputImage(m_OverexplainedImage);
  duplicator->Update();
  m_ResidualImage = duplicator->GetOutput();
  m_ResidualImage->FillBuffer(0.0);

  duplicator->SetInputImage(m_ResidualImage);
  duplicator->Update();
  m_FittedImage = duplicator->GetOutput();
  m_FittedImage->FillBuffer(0.0);

  vnl_vector<double> fitted_b; fitted_b.set_size(b.size());
  cost.S->multiply(m_Weights, fitted_b);

  for (unsigned int r=0; r<b.size(); r++)
  {
    itk::Index<4> idx4;
    unsigned int linear_index = r;
    idx4[0] = linear_index % sz_x; linear_index /= sz_x;
    idx4[1] = linear_index % sz_y; linear_index /= sz_y;
    idx4[2] = linear_index % sz_z; linear_index /= sz_z;
    int peak_id = linear_index % dim_four_size;

    if (peak_id<dim_four_size-1)
    {
      vnl_vector_fixed<float,3> peak_dir;

      idx4[3] = peak_id*3;
      peak_dir[0] = m_PeakImage->GetPixel(idx4);
      idx4[3] += 1;
      peak_dir[1] = m_PeakImage->GetPixel(idx4);
      idx4[3] += 1;
      peak_dir[2] = m_PeakImage->GetPixel(idx4);

      peak_dir.normalize();
      peak_dir *= fitted_b[r];

      idx4[3] = peak_id*3;
      m_FittedImage->SetPixel(idx4, peak_dir[0]);

      idx4[3] += 1;
      m_FittedImage->SetPixel(idx4, peak_dir[1]);

      idx4[3] += 1;
      m_FittedImage->SetPixel(idx4, peak_dir[2]);
    }
  }

  FD = 0;
  m_Coverage = 0;
  m_Overshoot = 0;

  itk::Index<4> idx4;
  for (idx4[0]=0; idx4[0]<sz_x; ++idx4[0])
    for (idx4[1]=0; idx4[1]<sz_y; ++idx4[1])
      for (idx4[2]=0; idx4[2]<sz_z; ++idx4[2])
      {
        itk::Index<3> idx3; idx3[0] = idx4[0]; idx3[1] = idx4[1]; idx3[2] = idx4[2];
        if (m_MaskImage.IsNotNull() && m_MaskImage->GetPixel(idx3)==0)
          continue;

        vnl_vector_fixed<float,3> peak_dir;
        vnl_vector_fixed<float,3> fitted_dir;
        vnl_vector_fixed<float,3> overshoot_dir;
        for (idx4[3]=0; idx4[3]<(itk::IndexValueType)m_PeakImage->GetLargestPossibleRegion().GetSize(3); ++idx4[3])
        {
          peak_dir[idx4[3]%3] = m_PeakImage->GetPixel(idx4);
          fitted_dir[idx4[3]%3] = m_FittedImage->GetPixel(idx4);
          m_ResidualImage->SetPixel(idx4, m_PeakImage->GetPixel(idx4) - m_FittedImage->GetPixel(idx4));

          if (idx4[3]%3==2)
          {
            FD += peak_dir.magnitude();

            itk::Index<4> tidx= idx4;
            if (peak_dir.magnitude()>fitted_dir.magnitude())
            {
              m_Coverage += fitted_dir.magnitude();
              m_UnderexplainedImage->SetPixel(tidx, peak_dir[2]-fitted_dir[2]); tidx[3]--;
              m_UnderexplainedImage->SetPixel(tidx, peak_dir[1]-fitted_dir[1]); tidx[3]--;
              m_UnderexplainedImage->SetPixel(tidx, peak_dir[0]-fitted_dir[0]);
            }
            else
            {
              overshoot_dir[0] = fitted_dir[0]-peak_dir[0];
              overshoot_dir[1] = fitted_dir[1]-peak_dir[1];
              overshoot_dir[2] = fitted_dir[2]-peak_dir[2];
              m_Coverage += peak_dir.magnitude();
              m_Overshoot += overshoot_dir.magnitude();
              m_OverexplainedImage->SetPixel(tidx, overshoot_dir[2]); tidx[3]--;
              m_OverexplainedImage->SetPixel(tidx, overshoot_dir[1]); tidx[3]--;
              m_OverexplainedImage->SetPixel(tidx, overshoot_dir[0]);
            }
          }
        }
      }
}

vnl_vector_fixed<float,3> FitFibersToImageFilter::GetClosestPeak(itk::Index<4> idx, PeakImgType::Pointer peak_image , vnl_vector_fixed<float,3> fiber_dir, int& id, double& w )
{
  int m_NumDirs = peak_image->GetLargestPossibleRegion().GetSize()[3]/3;
  vnl_vector_fixed<float,3> out_dir; out_dir.fill(0);
  float angle = 0.9;

  for (int i=0; i<m_NumDirs; i++)
  {
    vnl_vector_fixed<float,3> dir;
    idx[3] = i*3;
    dir[0] = peak_image->GetPixel(idx);
    idx[3] += 1;
    dir[1] = peak_image->GetPixel(idx);
    idx[3] += 1;
    dir[2] = peak_image->GetPixel(idx);

    float mag = dir.magnitude();
    if (mag<mitk::eps)
      continue;

    dir.normalize();

    float a = dot_product(dir, fiber_dir);
    if (fabs(a)>angle)
    {
      angle = fabs(a);
      w = angle;
      if (a<0)
        out_dir = -dir;
      else
        out_dir = dir;
      out_dir *= mag;
      id = i;
    }
  }

  return out_dir;
}

std::vector<mitk::FiberBundle::Pointer> FitFibersToImageFilter::GetTractograms() const
{
  return m_Tractograms;
}

void FitFibersToImageFilter::SetTractograms(const std::vector<mitk::FiberBundle::Pointer> &tractograms)
{
  m_Tractograms = tractograms;
}

void FitFibersToImageFilter::SetSignalModel(mitk::DiffusionSignalModel<> *SignalModel)
{
  m_SignalModel = SignalModel;
}

}


/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */

/*
 * File:   AccountGenerator.cpp
 * Author: ubuntu
 *
 * Created on March 8, 2018, 6:14 PM
 */

#include <iostream>
#include <boost/filesystem/path.hpp>
#include <boost/filesystem/operations.hpp>
#include <nlohmann/json.hpp>
#include <botan/hex.h>

#include "keto/account_utils/AccountGenerator.hpp"
#include "keto/account_utils/Exceptions.hpp"
#include "include/keto/account_utils/AccountGenerator.hpp"

namespace keto {
namespace account_utils {

AccountGenerator::AccountGenerator() : generator(new Botan::AutoSeeded_RNG()) {
    std::cout << "Generate a new key " << std::endl;
    this->privateKey = std::shared_ptr<Botan::Private_Key>(
            new Botan::RSA_PrivateKey(*generator, 2056));
    Botan::RSA_PrivateKey* privateKey = (Botan::RSA_PrivateKey*)this->privateKey.get();
    this->publicKey = std::shared_ptr<Botan::Public_Key>(
            new Botan::RSA_PublicKey(*privateKey));
}

AccountGenerator::AccountGenerator(const std::string& privateKeyPath) : generator(new Botan::AutoSeeded_RNG()){
        // setup the paths using the environmental variables
    boost::filesystem::path keyPath = privateKeyPath;
    std::cout << "Key path is : " << keyPath.string() << std::endl;
    if (!boost::filesystem::exists(keyPath)) {
        BOOST_THROW_EXCEPTION(keto::account_utils::InvalidPrivateKeyPathException());
    }
    this->privateKey = std::shared_ptr<Botan::Private_Key>(
            Botan::PKCS8::load_key(keyPath.string(), *generator));
    Botan::RSA_PrivateKey* privateKey = (Botan::RSA_PrivateKey*)this->privateKey.get();
    this->publicKey = std::shared_ptr<Botan::Public_Key>(
            new Botan::RSA_PublicKey(*privateKey));
}

AccountGenerator::~AccountGenerator() {
}

keto::crypto::SecureVector AccountGenerator::getAccountHash() {
    std::unique_ptr<Botan::HashFunction> hash256(Botan::HashFunction::create("SHA-256"));
    return hash256->process(this->privateKey->private_key_bits());
}

keto::crypto::SecureVector AccountGenerator::getPrivateKey() {
    return Botan::PKCS8::BER_encode(*this->privateKey);
}

std::vector<uint8_t> AccountGenerator::getPublicKey() {
    return this->publicKey->public_key_bits();
}

AccountGenerator::operator std::string() {
    nlohmann::json json = {
        {"account_hash", Botan::hex_encode(getAccountHash(),true)},
        {"public_key", Botan::hex_encode(getPublicKey(),true)},
        {"private_key", Botan::hex_encode(getPrivateKey(),true)}
      };
    return json.dump();
}

}
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * *  Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */
#include <stdexcept>

#include <boost/lexical_cast.hpp>

#include <proj_api.h>

#include "dbglog/dbglog.hpp"

#include "math/geometry.hpp"

#include "csconvertor.hpp"

namespace geo {

class CsConvertor::Impl : boost::noncopyable
{
public:
    //typedef std::shared_ptr<Impl> pointer;
    virtual ~Impl() {}
    virtual math::Point2 convert(const math::Point2 &p) const = 0;
    virtual math::Point3 convert(const math::Point3 &p) const = 0;
};

namespace {

class NoOpConvertor : public CsConvertor::Impl
{
public:
    NoOpConvertor()
    {
        LOG(info1) << "Coordinate system transformation: no-op.";
    }

    virtual math::Point2 convert(const math::Point2 &p) const { return p; }
    virtual math::Point3 convert(const math::Point3 &p) const { return p; }
};

class ProjInst
{
public:
    projPJ pj;

    ProjInst(const SrsDefinition &srs, projCtx ctx) : pj(nullptr)
    {
        pj = pj_init_plus_ctx(ctx, srs.c_str());
        if (!pj)
        {
            LOGTHROW(err1, std::runtime_error)
                << "Failed to initialize proj converter: <"
                << srs << ">";
        }
    }

    ~ProjInst()
    {
        pj_free(pj);
    }
};

class ProjImpl : public CsConvertor::Impl
{
public:
    ProjImpl(const SrsDefinition &from, const SrsDefinition &to, projCtx ctx)
        : preMult(1), postMult(1), gridNotFoundReported(false)
    {
        LOG(info1) << "Coordinate system transformation ("
            << from << " -> " << to << ").";

        if (!from.is(SrsDefinition::Type::proj4)
                || !to.is(SrsDefinition::Type::proj4))
        {
            LOGTHROW(fatal, std::logic_error)
                << "No GDAL: only proj to proj conversion is available.";
        }

        this->from = std::make_unique<ProjInst>(from, ctx);
        this->to = std::make_unique<ProjInst>(to, ctx);

        if (pj_is_latlong(this->from->pj))
            preMult = DEG_TO_RAD;
        if (pj_is_latlong(this->to->pj))
            postMult = RAD_TO_DEG;
    }

    virtual math::Point2 convert(const math::Point2 &p) const {
        math::Point3 p3(p[0], p[1], 0);
        p3 = convert(p3);
        return math::Point2(p3[0], p3[1]);
    }

    virtual math::Point3 convert(const math::Point3 &p) const {
        assert(!std::isnan(p[0]) && !std::isnan(p[1]) && !std::isnan(p[2]));
        math::Point3 r(p);
        r[0] *= preMult;
        r[1] *= preMult;
        auto err = pj_transform(from->pj, to->pj, 1, 1, &r[0], &r[1], &r[2]);
        if (err == -38)
        {
            // prevent repeatedly logging the same message
            if (gridNotFoundReported)
            {
                throw std::runtime_error(
                        "Error in coordinate transformation <-38>:"
                        "<failed to load datum shift file>");
            }
            gridNotFoundReported = true;
        }
        if (err != 0)
        {
            LOGTHROW(err2, std::runtime_error)
                << "Error in coordinate transformation <"
                << err << ">:<" << pj_strerrno(err) << ">, point <"
                << p << ">";
        }
        r[0] *= postMult;
        r[1] *= postMult;
        return r;
    }

    std::unique_ptr<ProjInst> from, to;
    double preMult, postMult;
    mutable bool gridNotFoundReported;
};

static volatile struct Initializer {
    Initializer() {
        // initializes locks and default context
        ::pj_get_default_ctx();
    }
} initializer;

} // namespace

CsConvertor::CsConvertor(const SrsDefinition &from,
    const SrsDefinition &to)
    : trans_(std::make_shared<ProjImpl>(from, to, pj_get_default_ctx()))
{}

CsConvertor::CsConvertor(const SrsDefinition &from,
    const SrsDefinition &to, projCtx ctx)
    : trans_(std::make_shared<ProjImpl>(from, to, ctx))
{}

CsConvertor::CsConvertor()
    : trans_(std::make_shared<NoOpConvertor>())
{}

CsConvertor::CsConvertor(const std::shared_ptr<Impl> &trans)
    : trans_(trans)
{}

math::Point2 CsConvertor::operator()(const math::Point2 &p) const
{
    return trans_->convert(p);
}

math::Point3 CsConvertor::operator()(const math::Point3 &p) const
{
    return trans_->convert(p);
}

math::Point4 CsConvertor::operator()(const math::Point4 &p) const
{
    const auto pp(trans_->convert
                  (math::Point3(p(0) / p(3), p(1) / p(3), p(2) / p(3))));
    return math::Point4(pp(0), pp(1), pp(2), 1.0);
}

} // namespace geo