//          Copyright Gavin Band 2008 - 2012.
// 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)

#ifndef QCTOOL_HAPLOTYPE_FREQUENCY_COMPONENT_HPP
#define QCTOOL_HAPLOTYPE_FREQUENCY_COMPONENT_HPP

#include <string>
#include <boost/function.hpp>
#include <boost/optional.hpp>
#include <boost/signals2/signal.hpp>
#include <Eigen/Core>
#include "genfile/SNPDataSourceProcessor.hpp"
#include "genfile/VariantIdentifyingDataTest.hpp"
#include "genfile/CohortIndividualSource.hpp"
#include "genfile/SampleStratification.hpp"
#include "appcontext/OptionProcessor.hpp"
#include "appcontext/UIContext.hpp"
#include "HaplotypeFrequencyLogLikelihood.hpp"
#include "FlatTableDBOutputter.hpp"

struct HaplotypeFrequencyComponent: public genfile::SNPDataSourceProcessor::Callback {
public:
	typedef std::auto_ptr< HaplotypeFrequencyComponent > UniquePtr ;

	static void declare_options( appcontext::OptionProcessor& options ) ;
	static UniquePtr create(
		genfile::CohortIndividualSource const& samples,
		std::string const& source_sample_id_column,
		genfile::CohortIndividualSource::UniquePtr ld_samples,
		std::string const& ld_sample_id_column,
		genfile::SNPDataSource::UniquePtr source,
		appcontext::OptionProcessor const& options,
		appcontext::UIContext& ui_context
	) ;

	void set_max_distance( uint64_t distance ) ;
	void set_min_r2( double min_r2 ) ;
	void set_stratification( genfile::SampleStratification stratification ) ;
	void set_prior( Eigen::Matrix2d const& matrix ) ;

public:
	HaplotypeFrequencyComponent( genfile::SNPDataSource::UniquePtr source, appcontext::UIContext& ui_context ) ;

public:
	void begin_processing_snps( std::size_t number_of_samples, genfile::SNPDataSource::Metadata const& ) ;
	void processed_snp( genfile::VariantIdentifyingData const& target_snp, genfile::VariantDataReader& target_data_reader ) ;

	void compute_ld_measures(
		genfile::VariantIdentifyingData const& source_snp,
		genfile::VariantDataReader& source_data_reader,
		genfile::VariantIdentifyingData const& target_snp,
		genfile::VariantDataReader& target_data_reader
	) ;

	bool compute_dosage_ld_measures(
		genfile::VariantIdentifyingData const& source_snp,
		std::vector< uint32_t > const& source_ploidy,
		genfile::VariantIdentifyingData const& target_snp,
		std::vector< uint32_t > const& target_ploidy,
		Eigen::MatrixXd const& dosages,
		Eigen::MatrixXd const& nonmissingness
	) ;

	double compute_dosage_correlation(
		genfile::VariantIdentifyingData const& source_snp,
		genfile::VariantIdentifyingData const& target_snp,
		Eigen::MatrixXd const& dosages,
		Eigen::MatrixXd const& nonmissingness,
		std::vector< genfile::SampleRange > const& sample_set
	) ;

	bool compute_em_ld_measures(
		genfile::VariantIdentifyingData const& source_snp,
		std::vector< uint32_t > const& source_ploidy,
		std::vector< int > const& source_calls,
		genfile::VariantIdentifyingData const& target_snp,
		std::vector< uint32_t > const& target_ploidy,
		std::vector< int > const& target_calls,
		Eigen::MatrixXd const& nonmissingness
	) ;

	bool compute_em_ld_measures(
		genfile::VariantIdentifyingData const& source_snp,
		std::vector< uint32_t > const& source_ploidy,
		std::vector< int > const& source_calls,
		genfile::VariantIdentifyingData const& target_snp,
		std::vector< uint32_t > const& target_ploidy,
		std::vector< int > const& target_calls,
		std::string const& variable_name_stub,
		std::vector< genfile::SampleRange > const& sample_set,
		bool const alwaysOutput
	) ;

	void end_processing_snps() ;

	void send_results_to( haplotype_frequency_component::FlatTableDBOutputter::UniquePtr ) ;

private:
	genfile::SNPDataSource::UniquePtr m_source ;
	appcontext::UIContext& m_ui_context ;
	boost::optional< genfile::SampleStratification > m_stratification ;
	double const m_threshhold ;
	int64_t m_max_distance ;
	double m_min_r2 ;
	haplotype_frequency_component::FlatTableDBOutputter::UniquePtr m_sink ;
	Eigen::Matrix2d m_prior ;
} ;

#endif


#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/g2o_types.h"
namespace myslam
{

VisualOdometry::VisualOdometry()
    : state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0), matcher_flann_(new cv::flann::LshIndexParams(5, 10, 2))
{
    num_of_features_ = Config::get<int>("number_of_features");
    scale_factor_ = Config::get<double>("scale_factor");
    level_pyramid_ = Config::get<int>("level_pyramid");
    match_ratio_ = Config::get<float>("match_ratio");
    max_num_lost_ = Config::get<float>("max_num_lost");
    min_inliers_ = Config::get<int>("min_inliers");
    key_frame_min_rot = Config::get<double>("keyframe_rotation");
    key_frame_min_trans = Config::get<double>("keyframe_translation");
    map_point_erase_ratio_ = Config::get<double>("map_point_erase_ratio");
    orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
}

VisualOdometry::~VisualOdometry()
{
}

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
    switch (state_)
    {
    case INITIALIZING:
    {
        state_ = OK;
        ref_ = curr_ = frame;
        //map_->insertKeyFrame(frame);
        extractKeyPoints();
        computeDescriptors();
        addKeyFrame();
        break;
    }
    case OK:
    {
        curr_ = frame;
        curr_->T_c_w_ = ref_->T_c_w_;
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP();
        if (checkEstimatedPose())
        {
            curr_->T_c_w_ = T_c_w_estimated_; //直接估计T_cw
            optimizeMap();
            num_lost_ = 0;
            if (checkKeyFrame())
                addKeyFrame();
        }
        else
        {
            num_lost_++;
            if (num_lost_ > max_num_lost_)
                state_ = LOST;
            return false;
        }
        break;
    }
    case LOST:
    {
        cout << "vo has lost" << endl;
        break;
    }
    }
    return true;
}

void VisualOdometry::extractKeyPoints()
{
    boost::timer timer;
    orb_->detect(curr_->color_, keypoints_curr_);
    cout << "Extract keypoitns cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::computeDescriptors()
{
    boost::timer timer;
    orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
    cout << "Compute descriptor cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::featureMatching()
{
    boost::timer timer;
    vector<cv::DMatch> matches;
    Mat desp_map;
    vector<MapPoint::Ptr> candidate;
    for (auto &allpoints : map_->map_points_)
    {
        // <ID,map_point::Ptr
        MapPoint::Ptr &p = allpoints.second;
        // pos：特征点p在世界坐标系中的坐标，看它在不在当前帧视野里
        if (curr_->isInFrame(p->pos_))
        {
            // 取的是地图中特征点的引用，直接++
            p->visible_times_++;
            // 存在于当前帧中的地图特征点和其描述子集合
            candidate.push_back(p);
            desp_map.push_back(p->descriptor_);
        }
    }

    // 当前帧特征点的描述子，与地图特征点的描述子匹配
    matcher_flann_.match(desp_map, descriptors_curr_, matches);

    //定义lambda函数，从matches里，按照其中元素的cv::DMatch.distance找到最小距离的元素
    //得到它的最小距离
    float min_dis = std::min_element(
                        matches.begin(), matches.end(),
                        [](const cv::DMatch &m1, const cv::DMatch &m2) {
                            return m1.distance < m2.distance;
                        })
                        ->distance;
    match_3dpts_.clear();
    match_2dkp_index_.clear();

    // 找到距离合适的匹配
    // 距离小于30或min_dis的match_ratio倍，算匹配上
    for (cv::DMatch &m : matches)
    {
        if (m.distance < max<float>(min_dis * match_ratio_, 30.0))
        {
            // 当前帧在地图中匹配到的特征点（地图中的特征点的三维坐标）
            match_3dpts_.push_back(candidate[m.queryIdx]);
            // 当前帧在地图中匹配到的特征点的id（当前帧上2d点的id）
            match_2dkp_index_.push_back(m.trainIdx);
        }
    }
    cout << "good matches:" << match_3dpts_.size() << endl;
    cout << "match cost time: " << timer.elapsed() << endl;
}

void VisualOdometry::poseEstimationPnP()
{
    vector<cv::Point3f> pts3d;
    vector<cv::Point2f> pts2d;
    // 2d特征点
    for (int index : match_2dkp_index_)
    {
        pts2d.push_back(keypoints_curr_[index].pt);
    }
    //3d坐标点
    for (MapPoint::Ptr pt : match_3dpts_)
    {
        pts3d.push_back(pt->getPositionCV());
    }
    // 内参
    Mat K = (cv::Mat_<double>(3, 3) << ref_->camera_->fx_, 0, ref_->camera_->cx_,
             0, ref_->camera_->fy_, ref_->camera_->cy_,
             0, 0, 1);
    //输出
    // rvec ，tvec 估计的旋转矩阵和平移矩阵
    // inliers ： 内点。 feature_matches_里存在误匹配，内点是正确匹配的点
    Mat rvec, tvec, inliers;
    cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
    num_inliers_ = inliers.rows;
    cout << "pnp inliers:" << num_inliers_ << endl;
    T_c_w_estimated_ = SE3(
        SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
        Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0)));
    // 使用bundle adjestment优化位姿
    typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> Block;
    Block::LinearSolverType *linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
    Block *solver_ptr = new Block(linearSolver);
    g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm(solver);

    // 优化变量，一个顶点，位姿
    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
    pose->setId(0);
    // 设定初值
    pose->setEstimate(g2o::SE3Quat(
        T_c_w_estimated_.rotation_matrix(),
        T_c_w_estimated_.translation()));
    optimizer.addVertex(pose);

    // 边
    for (int i = 0; i < inliers.rows; i++)
    {
        // 对每一个内点
        int index = inliers.at<int>(i, 0);
        EdgeProjectXYZ2UVPoseOnly *edge = new EdgeProjectXYZ2UVPoseOnly();
        edge->setId(i);
        // 一元边，只连接位姿顶点
        edge->setVertex(0, pose);
        edge->camera_ = curr_->camera_.get();
        // 三维点坐标，把它重投影和下面的measurement计算误差
        edge->point_ = Vector3d(pts3d[index].x, pts3d[index].y, pts3d[index].z);
        // 观测到的值
        edge->setMeasurement(Vector2d(pts2d[index].x, pts2d[index].y));
        edge->setInformation(Eigen::Matrix2d::Identity());
        optimizer.addEdge(edge);
        // 内点才算匹配上了
        match_3dpts_[index]->matched_times_++;
    }

    optimizer.initializeOptimization();
    optimizer.optimize(10);

    T_c_w_estimated_ = SE3(
        pose->estimate().rotation(),
        pose->estimate().translation());
    cout<<"T_c_w_estimated_: "<<endl<<T_c_w_estimated_.matrix()<<endl;

}

bool VisualOdometry::checkEstimatedPose()
{
    if (num_inliers_ < min_inliers_)
    {
        cout << "reject because inlier is too small:" << num_inliers_ << endl;
        return false;
    }
    // 估计的是T_c_w，ref_->T_c_w_是参考帧的位姿(T_r_w)，T_r_c*T_c_w=T_r_w。
    // 要计算参考帧到当前帧的转换
    // 参考帧转到当前，当前在转到世界，等于参考帧到世界坐标系的转换
    SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
    Sophus::Vector6d d = T_r_c.log();
    if (d.norm() > 5.0)
    {
        cout << "reject because motion is too large:" << d.norm() << endl;
        return false;
    }
    return true;
}

bool VisualOdometry::checkKeyFrame()
{
    SE3 T_r_c = ref_->T_c_w_ * T_c_w_estimated_.inverse();
    Sophus::Vector6d d = T_c_w_estimated_.log();
    Vector3d trans = d.head<3>();
    Vector3d rot = d.tail<3>();
    if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
        return true;
    return false;
}

void VisualOdometry::addKeyFrame()
{
    // 对于第一帧，插入所有3d点
    if (map_->keyframes_.empty())
    {
        for (size_t i = 0; i < keypoints_curr_.size(); i++)
        {
            double d = curr_->findDepth(keypoints_curr_[i]);
            if (d < 0)
                continue;
            Vector3d p_world = ref_->camera_->pixel2world(
                Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), curr_->T_c_w_, d);
            // 可是上面得到的已经是世界坐标了啊？
            // 把坐标点挪到世界坐标系下。减去相机坐标系光心的平移，不是以第一帧为世界坐标。
            Vector3d n = p_world - ref_->getCamCenter();
            n.normalize();
            MapPoint::Ptr map_point = MapPoint::createMapPoint(
                p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
            map_->insertMapPoint(map_point);
        }
    }
    map_->insertKeyFrame(curr_);
    ref_ = curr_;
}

// 向地图中添加新的特征点
// 只有地图中的特征点太少了才调用
void VisualOdometry::addMapPoints()
{
    vector<bool> matched(keypoints_curr_.size(), false);
    for (int index : match_2dkp_index_)
        matched[index] = true;
    for (int i = 0; i < keypoints_curr_.size(); i++)
    {
        // 当前帧的特征点，已经和地图中的特征点匹配上，说明已经在地图里，不需要添加
        if (matched[i] == true)
            continue;
        // 对于没在地图里的特征点，都加进去
        // 仍然是上一帧到世界的T已经估计好，从上一帧里面找深度
        double d = ref_->findDepth(keypoints_curr_[i]);
        if (d < 0)
            continue;
        Vector3d p_world = ref_->camera_->pixel2world(
            Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), curr_->T_c_w_, d);
        // 挪到世界坐标系下
        Vector3d n = p_world - ref_->getCamCenter();
        n.normalize();
        MapPoint::Ptr map_point = MapPoint::createMapPoint(
            p_world, n, descriptors_curr_.row(i).clone(), curr_.get());
        map_->insertMapPoint(map_point);
    }
}

void VisualOdometry::optimizeMap()
{
    for (auto iter = map_->map_points_.begin(); iter != map_->map_points_.end();)
    {
        // 删除当前帧看不见的点
        if (!curr_->isInFrame(iter->second->pos_))
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        // 删除总被看见却总匹配不上的点
        float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
        if (match_ratio < map_point_erase_ratio_)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        // 如果太偏了
        double angle = getViewAngle(curr_, iter->second);
        if (angle > M_PI / 6.)
        {
            iter = map_->map_points_.erase(iter);
            continue;
        }
        if (iter->second->good_ == false)
        {
            // ?? todo try triangulate this map point
        }
        iter++;
    }
    if (match_2dkp_index_.size() < 100)
        addMapPoints();
    if (map_->map_points_.size() > 1000)
    {
        map_point_erase_ratio_ += 0.05;
    }
    else
        map_point_erase_ratio_ = 0.1;
    cout << "map points: " << map_->map_points_.size() << endl;
}

double VisualOdometry::getViewAngle(Frame::Ptr frame, MapPoint::Ptr point)
{
    Vector3d n = point->pos_ - frame->getCamCenter();
    n.normalize();
    return acos(n.transpose() * point->norm_);
}

} // namespace myslam


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_DISTANCE_ASYMMETRIC_HPP
#define BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_DISTANCE_ASYMMETRIC_HPP

// Buffer strategies

#include <boost/geometry/extensions/strategies/buffer_side.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace buffer
{

template<typename CoordinateType>
class distance_asymmetric
{
public :
    distance_asymmetric(CoordinateType const& left,
                CoordinateType const& right)
        : m_left(left)
        , m_right(right)
    {}

    template <typename Point>
    inline CoordinateType apply(Point const& , Point const& ,
                buffer_side_selector side)  const
    {
        return side == buffer_side_left ? m_left : m_right;
    }

    inline int factor() const
    {
        return m_left < 0 ? -1 : 1;
    }

private :
    CoordinateType m_left;
    CoordinateType m_right;
};

}} // namespace strategy::buffer

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_STRATEGIES_BUFFER_DISTANCE_ASYMMETRIC_HPP


#include "CalibTracker/SiStripLorentzAngle/plugins/MeasureLA.h"
#include "CalibTracker/SiStripLorentzAngle/interface/LA_Filler_Fitter.h"
#include "CalibTracker/SiStripCommon/interface/SiStripDetInfoFileReader.h"
#include "CalibTracker/StandaloneTrackerTopology/interface/StandaloneTrackerTopology.h"

#include <boost/lexical_cast.hpp>
#include <TChain.h>
#include <TFile.h>
#include <regex>

namespace sistrip {

  void MeasureLA::store_methods_and_granularity(const edm::VParameterSet& vpset) {
    for (auto const& p : vpset) {
      methods |= p.getParameter<int32_t>("Method");
      byModule = byModule || p.getParameter<int32_t>("Granularity");
      byLayer = byLayer || !p.getParameter<int32_t>("Granularity");
    }
  }

  MeasureLA::MeasureLA(const edm::ParameterSet& conf)
      : inputFiles(conf.getParameter<std::vector<std::string> >("InputFiles")),
        inFileLocation(conf.getParameter<std::string>("InFileLocation")),
        fp_(conf.getParameter<edm::FileInPath>("SiStripDetInfo")),
        reports(conf.getParameter<edm::VParameterSet>("Reports")),
        measurementPreferences(conf.getParameter<edm::VParameterSet>("MeasurementPreferences")),
        calibrations(conf.getParameter<edm::VParameterSet>("Calibrations")),
        methods(0),
        byModule(false),
        byLayer(false),
        localybin(conf.getUntrackedParameter<double>("LocalYBin", 0.0)),
        stripsperbin(conf.getUntrackedParameter<unsigned>("StripsPerBin", 0)),
        maxEvents(conf.getUntrackedParameter<unsigned>("MaxEvents", 0)),
        tTopo_(StandaloneTrackerTopology::fromTrackerParametersXMLFile(
            conf.getParameter<edm::FileInPath>("TrackerParameters").fullPath())) {
    store_methods_and_granularity(reports);
    store_methods_and_granularity(measurementPreferences);
    store_calibrations();

    TChain* const chain = new TChain("la_data");
    for (auto const& file : inputFiles)
      chain->Add((file + inFileLocation).c_str());

    LA_Filler_Fitter laff(methods, byLayer, byModule, localybin, stripsperbin, maxEvents, &tTopo_);
    laff.fill(chain, book);
    laff.fit(book);
    summarize_module_muH_byLayer(laff);
    process_reports();

    setWhatProduced(this, &MeasureLA::produce);
  }

  std::unique_ptr<SiStripLorentzAngle> MeasureLA::produce(const SiStripLorentzAngleRcd&) {
    auto lorentzAngle = std::make_unique<SiStripLorentzAngle>();
    /*
  std::map<uint32_t,LA_Filler_Fitter::Result>
    module_results = LA_Filler_Fitter::module_results(book, LA_Filler_Fitter::SQRTVAR);

  BOOST_FOREACH(const uint32_t& detid, SiStripDetInfoFileReader(fp_.fullPath()).getAllDetIds()) {
    float la = module_results[detid].measure / module_results[detid].field ;
    lorentzAngle->putLorentzAngle( detid, la );
  }
  */
    return lorentzAngle;
  }

  void MeasureLA::summarize_module_muH_byLayer(const LA_Filler_Fitter& laff) {
    for (int m = LA_Filler_Fitter::FIRST_METHOD; m <= LA_Filler_Fitter::LAST_METHOD; m <<= 1) {
      const LA_Filler_Fitter::Method method = (LA_Filler_Fitter::Method)m;
      for (auto& result : LA_Filler_Fitter::module_results(book, method)) {
        calibrate(calibration_key(result.first, method), result.second);
        std::string label =
            laff.layerLabel(result.first) + granularity(MODULESUMMARY) + LA_Filler_Fitter::method(method);
        label = std::regex_replace(label, std::regex("layer"), "");

        const double mu_H = -result.second.calMeasured.first / result.second.field;
        const double sigma_mu_H = result.second.calMeasured.second / result.second.field;
        const double weight = pow(1. / sigma_mu_H, 2);

        book.fill(mu_H, label, 150, -0.05, 0.1, weight);
      }
      for (Book::iterator it = book.begin(".*" + granularity(MODULESUMMARY) + ".*"); it != book.end(); ++it) {
        if (it->second->GetEntries())
          it->second->Fit("gaus", "LLQ");
      }
    }
  }

  void MeasureLA::process_reports() const {
    for (auto const& p : reports) {
      const GRANULARITY gran = (GRANULARITY)p.getParameter<int32_t>("Granularity");
      const std::string name = p.getParameter<std::string>("ReportName");
      const LA_Filler_Fitter::Method method = (LA_Filler_Fitter::Method)p.getParameter<int32_t>("Method");

      write_report_plots(name, method, gran);
      switch (gran) {
        case LAYER:
          write_report_text(name, method, LA_Filler_Fitter::layer_results(book, method));
          break;
        case MODULE:
          write_report_text(name, method, LA_Filler_Fitter::module_results(book, method));
          break;
        case MODULESUMMARY:
          write_report_text_ms(name, method);
          break;
      }
    }

    {
      TFile widthsFile("widths.root", "RECREATE");
      for (Book::const_iterator it = book.begin(".*_width"); it != book.end(); it++)
        if (it->second)
          it->second->Write();
      widthsFile.Close();
    }
  }

  void MeasureLA::write_report_plots(std::string name, LA_Filler_Fitter::Method method, GRANULARITY gran) const {
    TFile file((name + ".root").c_str(), "RECREATE");
    const std::string key = ".*" + granularity(gran) + ".*(" + LA_Filler_Fitter::method(method) + "|" +
                            LA_Filler_Fitter::method(method, false) + ".*)";
    for (Book::const_iterator hist = book.begin(key); hist != book.end(); ++hist)
      if (hist->second)
        hist->second->Write();
    file.Close();
  }

  template <class T>
  void MeasureLA::write_report_text(std::string name,
                                    const LA_Filler_Fitter::Method& _method,
                                    const std::map<T, LA_Filler_Fitter::Result>& _results) const {
    LA_Filler_Fitter::Method method = _method;
    std::map<T, LA_Filler_Fitter::Result> results = _results;
    std::fstream file((name + ".dat").c_str(), std::ios::out);
    for (auto& result : results) {
      calibrate(calibration_key(result.first, method), result.second);
      file << result.first << "\t" << result.second << std::endl;
    }
    file.close();
  }

  void MeasureLA::write_report_text_ms(std::string name, LA_Filler_Fitter::Method method) const {
    std::fstream file((name + ".dat").c_str(), std::ios::out);
    const std::string key = ".*" + granularity(MODULESUMMARY) + LA_Filler_Fitter::method(method);
    for (Book::const_iterator it = book.begin(key); it != book.end(); ++it) {
      const TF1* const f = it->second->GetFunction("gaus");
      if (f) {
        file << it->first << "\t" << f->GetParameter(1) << "\t" << f->GetParError(1) << "\t" << f->GetParameter(2)
             << "\t" << f->GetParError(2) << std::endl;
      }
    }
    file.close();
  }

  void MeasureLA::store_calibrations() {
    for (auto const& p : calibrations) {
      LA_Filler_Fitter::Method method = (LA_Filler_Fitter::Method)p.getParameter<int32_t>("Method");
      std::vector<double> slopes(p.getParameter<std::vector<double> >("Slopes"));
      assert(slopes.size() == 14);
      std::vector<double> offsets(p.getParameter<std::vector<double> >("Offsets"));
      assert(offsets.size() == 14);
      std::vector<double> pulls(p.getParameter<std::vector<double> >("Pulls"));
      assert(pulls.size() == 14);

      for (unsigned i = 0; i < 14; i++) {
        const std::pair<unsigned, LA_Filler_Fitter::Method> key(i, method);
        offset[key] = offsets[i];
        slope[key] = slopes[i];
        error_scaling[key] = pulls[i];
      }
    }
  }

  inline void MeasureLA::calibrate(const std::pair<unsigned, LA_Filler_Fitter::Method> key,
                                   LA_Filler_Fitter::Result& result) const {
    result.calMeasured = std::make_pair<float, float>(
        (result.measured.first - offset.find(key)->second) / slope.find(key)->second,
        result.measured.second * error_scaling.find(key)->second / slope.find(key)->second);
  }

  std::pair<uint32_t, LA_Filler_Fitter::Method> MeasureLA::calibration_key(
      const std::string layer, const LA_Filler_Fitter::Method method) const {
    std::regex format(".*(T[IO]B)_layer(\\d)([as]).*");
    const bool isTIB = "TIB" == std::regex_replace(layer, format, "\\1");
    const bool stereo = "s" == std::regex_replace(layer, format, "\\3");
    const unsigned layerNum = boost::lexical_cast<unsigned>(std::regex_replace(layer, format, "\\2"));
    return std::make_pair(LA_Filler_Fitter::layer_index(isTIB, stereo, layerNum), method);
  }

  std::pair<uint32_t, LA_Filler_Fitter::Method> MeasureLA::calibration_key(
      const uint32_t detid, const LA_Filler_Fitter::Method method) const {
    const bool isTIB = SiStripDetId(detid).subDetector() == SiStripDetId::TIB;
    const bool stereo = isTIB ? tTopo_.tibStereo(detid) : tTopo_.tobStereo(detid);
    const unsigned layer = isTIB ? tTopo_.tibLayer(detid) : tTopo_.tobStereo(detid);

    return std::make_pair(LA_Filler_Fitter::layer_index(isTIB, stereo, layer), method);
  }

}  // namespace sistrip


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

#ifndef SOURCE_DIRECTORY__APPLICATIONS_REGSEG_ENERGYTEST_HXX_
#define SOURCE_DIRECTORY__APPLICATIONS_REGSEG_ENERGYTEST_HXX_

#include <stdio.h>
#include <string>
#include <vector>
#include <iostream>
#include <fstream>
#include <iomanip>

#include <boost/program_options.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/optional.hpp>
#include <boost/filesystem.hpp>

#include <itkArray.h>
#include <itkVector.h>
#include <itkVectorImage.h>
#include <itkQuadEdgeMesh.h>
#include <itkVTKPolyDataReader.h>
#include <itkImageFileReader.h>
#include <itkOrientImageFilter.h>
#include <itkComposeImageFilter.h>

#include "DownsampleAveragingFilter.h"
#include "MultilabelBinarizeMeshFilter.h"
#include "rstkVTKPolyDataWriter.h"
#include "ComponentsFileWriter.h"
#include "EnergyCalculatorFilter.h"
#include "MahalanobisDistanceModel.h"

#include <jsoncpp/json/json.h>

namespace bpo = boost::program_options;
namespace bfs = boost::filesystem;

const static unsigned int Dimension = 3u;

typedef double                                                    MeasureType;
typedef float                                                     ChannelPixelType;
typedef itk::Image<ChannelPixelType, Dimension>                   ChannelType;
typedef itk::VectorImage<ChannelPixelType, Dimension>             ReferenceImageType;
typedef typename ReferenceImageType::PixelType                    VectorPixelType;
typedef itk::ComposeImageFilter<ChannelType, ReferenceImageType>  InputToVectorFilterType;

typedef float                                                     PointValueType;
typedef itk::Vector< PointValueType, Dimension >                  VectorType;
typedef itk::QuadEdgeMesh< VectorType, Dimension >                VectorContourType;
typedef itk::VectorImage< PointValueType, Dimension >             ProbmapType;

typedef typename ReferenceImageType::Pointer                      ReferencePointer;
typedef typename ReferenceImageType::DirectionType                DirectionType;
typedef typename ReferenceImageType::SizeType                     SizeType;
typedef typename ReferenceImageType::PointType                    PointType;
typedef typename ReferenceImageType::SpacingType                  SpacingType;
typedef itk::ContinuousIndex<PointValueType, Dimension>           ContinuousIndex;

typedef rstk::MultilabelBinarizeMeshFilter< VectorContourType >   BinarizeMeshFilterType;
typedef typename BinarizeMeshFilterType::Pointer                  BinarizeMeshFilterPointer;
typedef typename BinarizeMeshFilterType::OutputImageType          BinarizationImageType;
typedef typename BinarizeMeshFilterType::InputMeshContainer       InputMeshContainer;
typedef typename BinarizeMeshFilterType::OutputComponentType      SegmentationType;

typedef rstk::DownsampleAveragingFilter
		< BinarizationImageType, ProbmapType >                    DownsampleFilter;
typedef typename DownsampleFilter::Pointer                        DownsamplePointer;

typedef itk::VTKPolyDataReader< VectorContourType >               ReaderType;
typedef rstk::VTKPolyDataWriter< VectorContourType >              WriterType;
typedef itk::ImageFileReader<ChannelType>                         ImageReader;
typedef itk::ImageFileWriter<SegmentationType>                    SegmentationWriter;
typedef rstk::ComponentsFileWriter<ProbmapType>                   ImageWriter;

typedef itk::OrientImageFilter< ReferenceImageType, ReferenceImageType >  Orienter;
typedef itk::OrientImageFilter< SegmentationType, SegmentationType >      SegmentationOrienter;
typedef itk::OrientImageFilter< ProbmapType, ProbmapType >                ProbmapsOrienter;
typedef itk::OrientImageFilter< ChannelType, ChannelType >                ChannelOrienter;

typedef rstk::MahalanobisDistanceModel< ReferenceImageType >      EnergyModelType;
typedef typename EnergyModelType::Pointer                         EnergyModelPointer;

typedef rstk::EnergyCalculatorFilter<ReferenceImageType>          EnergyFilter;
typedef typename EnergyFilter::Pointer                            EnergyFilterPointer;
typedef typename EnergyFilter::PriorsImageType                    PriorsImageType;
typedef itk::Array< MeasureType >                                 MeasureArray;
typedef typename PriorsImageType::Pointer                         PriorsImagePointer;
typedef typename PriorsImageType::PixelType                       PriorsPixelType;
typedef typename PriorsImageType::InternalPixelType               PriorsValueType;

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

#endif /* SOURCE_DIRECTORY__APPLICATIONS_REGSEG_ENERGYTEST_HXX_ */


//
// Copyright Jason Rice 2017
// 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)
//
#ifndef NBDL_SQL_DB_DETAIL_STRING_SUM_HPP
#define NBDL_SQL_DB_DETAIL_STRING_SUM_HPP

#include <mpdef/list.hpp>

#include <boost/hana/plus.hpp>
#include <boost/hana/string.hpp>
#include <boost/hana/sum.hpp>

namespace nbdl::sql_db::detail
{
  namespace hana = boost::hana;

  constexpr auto string_sum = [](auto ...xs)
  {
    return hana::sum<hana::string_tag>(mpdef::make_list(xs...));
  };

  template <typename Delimeter = hana::string<' '>>
  constexpr auto string_sum_join = [](auto x1, auto ...xs)
  {
    return string_sum(x1, hana::plus(Delimeter{}, xs)...);
  };

  using identifier_quote_string = hana::string<'"'>; // SQL implementation specific (ANSI)

  template <typename Identifier>
  using identifier_quote = decltype(
    hana::plus(identifier_quote_string{},
    hana::plus(Identifier{},
      identifier_quote_string{}
    ))
  );
}

#endif


//! 2D形状の定義
#pragma once
#include "geom.hpp"
#include "spinner/resmgr.hpp"
#include "spinner/assoc.hpp"
#include <boost/pool/object_pool.hpp>
#include <memory>
#include "spinner/structure/treenode.hpp"
#include "spinner/rflag.hpp"
#include "boundary.hpp"

namespace boom {
	namespace geo2d {
		using Int_OP = spn::Optional<int>;
		struct Convex;
		struct Poly;
		struct Capsule;
		struct Circle;
		struct AABB;
		struct Segment;
		struct Ray;
		struct Line;
		struct Point;
		using CTGeo = spn::CType<Convex, Poly, Capsule, Circle, AABB, Segment, Ray, Line, Point>;
		template <class T>
		using ITagP = ITagP_base<T, CTGeo>;

		using PointL = std::vector<Vec2>;
		using SegL = std::vector<Segment>;
		using Convex2 = std::pair<Convex, Convex>;
		struct Line;
		struct Ray;
		struct IModel;
		struct Circle : ITagP<Circle> {
			Vec2	vCenter;
			float	fRadius;

			Circle() = default;
			Circle(const Vec2& c, float r);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;
			bool hit(const Circle& c, float t=NEAR_THRESHOLD) const;

			void getArcPoints(PointL& dst, float ang0, float ang1, float deep) const;
			Circle operator * (const AMat32& m) const;
			Circle operator * (float s) const;
			Circle& operator += (const Vec2& ofs);
			void distend(float width, float mindist);

			// ---- for MakeBoundary ----
			void setBoundary(const IModel* p);
			void appendBoundary(const IModel* p);

			friend std::ostream& operator << (std::ostream& os, const Circle& c);
		};
		//! AxisAlignedBox
		struct AABB : ITagP<AABB> {
			Vec2	minV, maxV;

			int _getAreaNumX(float p) const;
			int _getAreaNumY(float p) const;
			void _makeSegmentX(Segment& s, int num) const;
			void _makeSegmentY(Segment& s, int num) const;
			bool _checkHitX(const Segment& s0, int from, int to) const;
			bool _checkHitY(const Segment& s0, int from, int to) const;
			std::pair<int,int> _getAreaNum(const Vec2& p) const;

			AABB() = default;
			AABB(const Vec2& min_v, const Vec2& max_v);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;
			Vec2 nearest(const Vec2& pos) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& l, float t=NEAR_THRESHOLD) const;
			bool hit(const AABB& ab, float t=NEAR_THRESHOLD) const;
			AABB operator * (const AMat32& m) const;
			AABB& operator += (const Vec2& ofs);
			void distend(float width, float mindist);

			// ---- for MakeBoundary ----
			void setBoundary(const IModel* p);
			void appendBoundary(const IModel* p);

			friend std::ostream& operator << (std::ostream& os, const AABB& a);
		};
		//! IModelインタフェースの子ノードイテレータ
		struct MdlItr {
			TfBase_SP	_sp;

			MdlItr() = default;
			MdlItr(const TfBase_SP& sp);

			MdlItr& operator ++ ();
			bool operator == (const MdlItr& m) const;
			bool operator != (const MdlItr& m) const;
			explicit operator bool () const;
			const TfBase* get() const;
		};
		struct IModel : ::boom::IModelNode {
			// -----------------------------
			virtual Vec2 im_getCenter() const = 0;
			virtual float im_getArea() const = 0;
			virtual float im_getInertia() const = 0;
			virtual void im_getBVolume(Circle& c) const = 0;
			virtual void im_getBVolume(AABB& a) const = 0;
			// -----------------------------
			virtual void im_transform[[noreturn]](void* dst, const AMat32& m) const = 0;
			//! サポート射像
			/*! 均等でないスケーリングは対応しない、移動は後でオフセット、回転はdirを逆にすれば代用可
				・・との理由で行列変換後の物体に対する射像は無し */
			virtual Vec2 im_support(const Vec2& dir) const = 0;
			//! 図形と点の判定
			virtual bool im_hitPoint(const Vec2& p, float t=NEAR_THRESHOLD) const = 0;
			virtual uint32_t getCID() const = 0;
			virtual MdlItr getInner() const;
			virtual Model_SP im_clone() const = 0;

			virtual Vec2 toLocal(const Vec2& v) const { return v; }
			virtual Vec2 toLocalDir(const Vec2& v) const { return v; }
			virtual Vec2 toWorld(const Vec2& v) const { return v; }
			virtual Vec2 toWorldDir(const Vec2& v) const { return v; }
			virtual spn::Optional<const AMat32&> getToLocal() const { return spn::none; }
			virtual spn::Optional<const AMat32&> getToWorld() const { return spn::none; }
			const static AMat32 cs_idMat;
			const AMat32& getToLocalI() const;
			const AMat32& getToWorldI() const;
		};

		//! TにIModelインタフェースを付加
		template <class T>
		struct Model : IModelP_base<T, IModel> {
			using base_t = IModelP_base<T, IModel>;
			using base_t::base_t;

			Model() = default;
			Model(const T& t): base_t(t) {}
			Model(T&& t): base_t(std::move(t)) {}
			// 各種ルーチンの中継
			Model_SP im_clone() const override {
				return std::make_shared<Model<T>>(static_cast<const T&>(*this));
			}
			void im_transform(void* dst, const AMat32& m) const override { *reinterpret_cast<T*>(dst) = *this * m; }
			void im_getBVolume(Circle& c) const override { c = T::bs_getBCircle(); }
			void im_getBVolume(AABB& a) const override { a = T::bs_getBBox(); }
			float im_getInertia() const override { return T::bs_getInertia(); }
			float im_getArea() const override { return T::bs_getArea(); }
			Vec2 im_getCenter() const override { return T::bs_getCenter(); }
			Vec2 im_support(const Vec2& dir) const override { return T::support(dir); }
			bool im_hitPoint(const Vec2& pos, float t=NEAR_THRESHOLD) const override { return T::hit(pos, t); }
			std::ostream& print(std::ostream& os) const override { return os << static_cast<const T&>(*this); }
		};
		template <class T>
		std::ostream& operator << (std::ostream& os, const Model<T>& m) {
			return m.print(os);
		}
		using CircleM = Model<Circle>;

		class TfBase : public spn::TreeNode<TfBase>,
						virtual public IModel
		{
			private:
				friend class spn::TreeNode<TfBase>;
				using base_t = spn::TreeNode<TfBase>;
			protected:
				void* _getUserData(void*, std::true_type);
				void* _getUserData(void* udata, std::false_type);
				virtual void onChildAdded(const SP& node);
				virtual void onChildRemove(const SP& node);
				virtual void onParentChange(const SP& from, const SP& to);
				virtual void _setAsChanged();
			public:
				//! 子ノードの取得
				MdlItr getInner() const override;
				bool hasInner() const override;
				//! 下層オブジェクトの形状を変更した時に手動で呼ぶ
				virtual void setAsChanged();
				virtual bool isLeaf() const { return false; }
				virtual spn::Pose2D& tf_refPose[[noreturn]]();
				virtual const spn::Pose2D& tf_getPose() const;
				Model_SP im_clone() const override;
				virtual TfBase_SP clone() const = 0;
		};
		template <class Boundary, class Ud>
		class TfNode_base : public TfBase,
							public Model<Boundary>
		{
			protected:
				using model_t = Model<Boundary>;
				Ud					_udata;
			public:
				using model_t::model_t;
				Model_SP im_clone() const override {
					return TfBase::im_clone();
				}
				/*! ユーザーデータがvoidの時は親ノードのデータを返す */
				void* getUserData() override {
					return _getUserData(&_udata, std::is_same<spn::none_t, Ud>());
				}
		};
		template <class Boundary, class Ud=spn::none_t>
		class TfNode_Static : public TfNode_base<Boundary, Ud> {
			private:
				using base_t = TfNode_base<Boundary, Ud>;
				using SP = typename base_t::SP;
				using Time_t = typename base_t::Time_t;
				mutable bool	_bChanged = true,
								_bValid;
			protected:
				void onChildAdded(const TfBase::SP& node) override {
					_bChanged = true;
					base_t::onChildAdded(node);
				}
				void onChildRemove(const TfBase::SP& node) override {
					_bChanged = true;
					base_t::onChildRemove(node);
				}
				void _setAsChanged() override {
					_bChanged = true;
				}
			public:
				SP clone() const override {
					return std::make_shared<TfNode_Static>(*this);
				}
				bool imn_refresh(Time_t t) const override {
					// 更新条件はフラグ
					if(_bChanged) {
						_bChanged = false;
						std::vector<const IModel*> pm;
						// 直下の子ノードをリストアップ
						this->template iterateDepthFirst<false>([&pm](auto& node, int depth){
							if(depth == 0)
								return spn::Iterate::StepIn;
							pm.push_back(&node);
							return spn::Iterate::Next;
						});
						if((_bValid = !pm.empty())) {
							// 境界ボリュームの更新
							auto* core = const_cast<Boundary*>(reinterpret_cast<const Boundary*>(base_t::getCore()));
							auto op = MakeBoundary<Boundary>(&pm[0], pm.size(), t);
							if((_bValid = op))
								*core = *op;
						}
						// 子ノードが無い場合は常にヒットしない
					}
					return _bValid;
				}
		};
		template <class Boundary, class Ud=spn::none_t>
		class TfNode_Dynamic : public TfNode_base<Boundary, Ud> {
			private:
				using base_t = TfNode_base<Boundary, Ud>;
				using Time_t = typename base_t::Time_t;
				using SP = typename base_t::SP;
				using Time_OP = spn::Optional<Time_t>;
				mutable Time_OP		_opTime = 0;
				mutable bool		_bValid;
			protected:
				void onChildAdded(const SP& node) override {
					_opTime = spn::none;
					base_t::onChildAdded(node);
				}
				void onChildRemove(const SP& node) override {
					_opTime = spn::none;
					base_t::onChildRemove(node);
				}
			public:
				SP clone() const override {
					return std::make_shared<TfNode_Dynamic>(*this);
				}
				bool imn_refresh(Time_t t) const override {
					// 更新時刻が古い時のみ処理を行う
					if(!_opTime || *_opTime < t) {
						_opTime = t;

						std::vector<const IModel*> pm;
						// 子ノードをリストアップ
						this->template iterateDepthFirst<false>([&pm](auto& node, int depth){
							if(depth == 0)
								return spn::Iterate::StepIn;
							pm.push_back(&node);
							return spn::Iterate::Next;
						});
						if((_bValid = !pm.empty())) {
							// 境界ボリュームの更新
							auto* core = const_cast<Boundary*>(reinterpret_cast<const Boundary*>(base_t::getCore()));
							auto op = MakeBoundary<Boundary>(&pm[0], pm.size(), t);
							if((_bValid = op))
								*core = *op;
						}
						// 子ノードが無い場合は常にヒットしない
					}
					return _bValid;
				}
		};

		#define mgr_tf2d (::boom::geo2d::TfMgr::_ref())
		class TfMgr : public spn::ResMgrA<TfBase_SP, TfMgr> {};

		using LNear = std::pair<Vec2, LinePos>;
		struct Point : Vec2, ITagP<Point> {
			// -----------------------------
			const float& bs_getArea() const;
			const float& bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			using Vec2::Vec2;
			using Vec2::distance;
			float distance(const Segment& s) const;
			LNear nearest(const Segment& s) const;
			const Vec2& support(const Vec2& dir) const;
			Point operator * (const AMat32& m) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Point& p);
		};
		using PointM = Model<Point>;

		//! 直線
		struct Line : ITagP<Line> {
			Vec2	pos, dir;

			Line() = default;
			Line(const Vec2& p, const Vec2& d);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------

			Vec2x2 nearest(const Line& st) const;
			Vec2 nearest(const Vec2& p) const;
			float distance(const Vec2& p) const;
			//! 点を線分上に置く
			Vec2 placeOnLine(const Vec2& p) const;
			//! 基準位置に対する方向ベクトルとの内積
			float posDot(const Vec2& p) const;
			Line operator * (const AMat32& m) const;
			LineDivision checkSide(const Vec2& p, float t=DOT_THRESHOLD) const;

			Vec2 support(const Vec2& dir) const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Line& l);
		};
		using LineM = Model<Line>;
		//! 半直線
		struct Ray : ITagP<Ray> {
			Vec2	pos, dir;

			Ray() = default;
			Ray(const Vec2& p, const Vec2& d);

			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			const Vec2& bs_getCenter() const;
			const Circle& bs_getBCircle() const;
			const AABB& bs_getBBox() const;
			// -----------------------------

			const Line& asLine() const;
			Vec2x2 nearest(const Ray& r) const;
			Vec2 nearest(const Vec2& p) const;
			Ray operator * (const AMat32& m) const;

			Vec2 support(const Vec2& dir) const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Ray& r);
		};
		using RayM = Model<Ray>;
		//! 線分
		struct Segment : ITagP<Segment> {
			Vec2	from, to;

			Segment() = default;
			Segment(const Vec2& v0, const Vec2& v1);
			// -----------------------------
			const float& bs_getArea() const {INVOKE_ERROR}
			const float& bs_getInertia() const {INVOKE_ERROR}
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			float distance(const Segment& l) const;
			float length() const;
			float len_sq() const;
			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;

			/*! \return Vec2(最寄り座標),LINEPOS(最寄り線分位置) */
			LNear nearest(const Vec2& p) const;
			//! 線分が交差する位置を調べる
			/*! \return first: 交差座標
						second: 交差しているライン位置 (ONLINE or NOHIT) */
			Vec2_OP crossPoint(const Segment& l) const;
			Vec2_OP crossPoint(const Line& l) const;
			float ratio(const Vec2& p) const;
			Vec2 getDir() const;
			Line asLine() const;
			bool online(const Vec2& p) const;

			Segment operator * (const AMat32& m) const;
			friend std::ostream& operator << (std::ostream& os, const Segment& s);
		};
		using SegmentM = Model<Segment>;
		using AABBM = Model<AABB>;

		struct Capsule : ITagP<Capsule> {
			Vec2	from, to;
			float	radius;

			Capsule() = default;
			Capsule(const Vec2& f, const Vec2& t, float r);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			Vec2 support(const Vec2& dir) const;

			Capsule operator * (const AMat32& m) const;
			Capsule& operator += (const Vec2& ofs);

			spn::none_t hit(...) const;
			bool hit(const Vec2& p , float t=NEAR_THRESHOLD) const;
			bool hit(const Segment& s, float t=NEAR_THRESHOLD) const;
			bool hit(const Capsule& c, float t=NEAR_THRESHOLD) const;

			friend std::ostream& operator << (std::ostream& os, const Capsule& c);
		};
		using CapsuleM = Model<Capsule>;

		struct Poly : ITagP<Poly> {
			Vec2		point[3];
			bool _isInTriangle(const Vec2& p, float threshold) const;

			Poly() = default;
			Poly(const Vec2& p0, const Vec2& p1, const Vec2& p2);
			// -----------------------------
			float bs_getArea() const;
			float bs_getInertia() const;
			Vec2 bs_getCenter() const;
			Circle bs_getBCircle() const;
			AABB bs_getBBox() const;
			// -----------------------------
			//! 座標が三角形の内部に含まれるかを判定
			/*! ポリゴンは時計回りを想定
				辺上は含まない */
			bool isInTriangle(const Vec2& p) const;
			std::pair<Vec2,int> nearest(const Vec2& p) const;

			Vec2 support(const Vec2& dir) const;
			void addOffset(const Vec2& ofs);
			static float CalcArea(const Vec2& p0, const Vec2& p1, const Vec2& p2);
			static float CalcArea(const Vec2& p0, const Vec2& p1);
			//! 鈍角を探す
			/*! \return 鈍角の番号 (負数は該当なし) */
			int getObtuseCorner() const;
			//! 頂点の並びが時計回りかを判定
			bool isCW() const;
			//! 頂点の並びを反転
			void invert();
			LineDivision checkSide(const Line& l, float t=DOT_THRESHOLD) const;

			spn::none_t hit(...) const;
			//! 座標が三角形と衝突するか判定
			/*! isInTriangleとは異なり辺上もHitとみなす */
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			bool hit(const Poly& p, float t=NEAR_THRESHOLD) const;
			Poly operator * (const AMat32& m) const;
			Poly& operator += (const Vec2& ofs);
			void distend(float width, float mindist);
			friend std::ostream& operator << (std::ostream& os, const Poly& p);
		};
		using PolyM = Model<Poly>;
		//! 多角形基本クラス
		struct Convex : ITagP<Convex> {
			using AreaL = std::vector<float>;
			struct AreaSum {
				float result;

				AreaSum(): result(0) {}
				void operator()(int /*n*/, const Vec2& p0, const Vec2& p1) {
					result += Poly::CalcArea(p0,p1);
				}
			};
			struct AreaList {
				AreaL	areaL;
				float	sum;

				AreaList(int n): areaL(n), sum(0) {}
				void operator()(int n, const Vec2& p0, const Vec2& p1) {
					float a = Poly::CalcArea(p0,p1);
					areaL[n] = a;
					sum += a;
				}
			};
			PointL	point;
			const static uint8_t cs_index[1<<8];

			Convex() = default;
			/*! \param[in] v 凸包と分かっている頂点 */
			Convex(std::initializer_list<Vec2> v);
			Convex(const PointL& pl);
			Convex(PointL&& pl);

			using CBPoints = std::function<void (PointL&&)>;
			using CBConvex = std::function<void (Convex&&)>;
			static void MonotoneToConvex(const PointL& pts, const Vec2& dir, const CBConvex& cb);
			static void ConcaveToMonotone(const PointL& pts, const Vec2& dir, const CBPoints& cb);
			static bool IsMonotone(const PointL& pts, const Vec2& dir);
			static bool IsConvex(const PointL& pts);
			//! 凹ポリゴンを内包するような凸形状を求める
			static Convex FromConcave(const PointL& src);
			bool addPoint(const Vec2& p);
			// -----------------------------
			float bs_getArea() const;
			Circle bs_getBCircle() const;
			Vec2 bs_getCenter() const;
			float bs_getInertia() const;
			AABB bs_getBBox() const;
			// -----------------------------
			/*! 同時に求めると少し効率が良い為 */
			std::tuple<float,float,Vec2> area_inertia_center() const;
			Vec2 support(const Vec2& dir) const;
			//! 2つに分割
			/*! \param[out] c0 線分の進行方向左側
				\param[out] c1 線分の進行方向右側 */
			Convex2 splitTwo(const Line& l) const;
			//! 2つに分割して左側を自身に適用
			Convex split(const Line& l);
			//! 2つに分割して右側は捨てる
			void splitThis(const Line& l);

			template <class CB>
			void iterate(CB cb) const {
				// 頂点数は3つ以上
				int nL = point.size();
				AssertP(Trap, nL > 2);

				// 先にブリッジの箇所を処理
				cb(nL-1, point.back(), point.front());
				for(int i=0 ; i<nL-1 ; i++)
					cb(i, point[i], point[i+1]);
			}
			void addOffset(const Vec2& ofs);

			//! 指定ポイントの内部的な領域IDと内外位置を取得
			/*! \return first=内外判定
						second=領域ID */
			std::pair<ConvexPos, int> checkPosition(const Vec2& pos, float threshold=DOT_THRESHOLD) const;
			//! 内部的な通し番号における外郭ライン
			Segment getOuterSegment(int n) const;
			Line getOuterLine(int n) const;
			std::pair<bool,PointL> getOverlappingPoints(const Convex& mdl, const Vec2& inner) const;
			static Convex GetOverlappingConvex(const Convex& m0, const Convex& m1, const Vec2& inner);
			//! 凸包が直線と交差している箇所を2点計算
			std::tuple<bool,Vec2,Vec2> checkCrossingLine(const Line& l) const;
			Convex operator * (const AMat32& m) const;
			Convex& operator *= (const AMat32& m);
			Convex& operator += (const Vec2& ofs);
			void distend(float width, float mindist);
			//! 頂点が時計回りになっているか
			bool checkCW() const;
			//! 頂点の並びを時計回りに修正
			void adjustLoop();
			int getNPoints() const;
			Vec2 getPoint(int n) const;

			spn::none_t hit(...) const;
			bool hit(const Vec2& p, float t=NEAR_THRESHOLD) const;
			friend std::ostream& operator << (std::ostream& os, const Convex& c);
		};
		using ConvexM = Model<Convex>;
		//! GJK法による衝突判定(2D)
		/*! ヒットチェックのみ。衝突時は内部点を出力可 */
		class GSimplex {
			protected:
				const IModel	&_m0, &_m1;
				Poly	_poly;			//!< 凸包を構成するポリゴン
				Vec2	_posA[3],		//!< vtx(A-B)を求める時に使ったA側の座標
						_inner;			//!< 内部点
				bool	_bHit;			//!< 衝突の有無
				int		_nVtx;			//!< 使用された頂点の数(min=1, max=3)
			private:
				void _minkowskiSub(const Vec2& dir, int n);
				void _gjkMethod();
				void _setAsHit(int nv, const Vec2& inner);
				void _setAsNotHit(int nv);
			public:
				//! 初期化 = GJKによる判定(ヒットチェックだけ)
				GSimplex(const IModel& m0, const IModel& m1);
				bool getResult() const;
				//! 衝突時: 内部点を取得
				const Vec2& getInner() const;
		};
		//! GJKで最近傍対を求める
		/*! 常に頂点リストを時計回りに保つ */
		class GEpa : public GSimplex {
			/*! ミンコフスキー差の凸形状を構成する最大頂点数(VList)
				必要ならvectorでやっても良いがとりあえず決め打ち */
			constexpr static int MAX_VERT = 0x100;
			using VPool = boost::object_pool<Vec2x2>;
			static thread_local VPool tls_vPool;
			using VList = std::array<Vec2x2*, MAX_VERT>;

			VList	_vl;
			size_t	_szVl;
			//! 新しく頂点メモリを確保
			/*! \param n 格納先のインデックス (負数なら末尾) */
			Vec2x2* _allocVert(int n=-1);
			//! 頂点ポインタからのインデックス検索
			/*! \param vp 検索する頂点ポインタ */
			Int_OP _getIndex(const Vec2x2* vp) const;

			//! 点と辺の両対応
			struct LmLen {
				float			dist;
				Vec2			dir;
				const Vec2x2	*vtx[2];	//!< index[1]==nullptrの時は単一の頂点を表す

				bool operator < (const LmLen& len) const {
					return dist < len.dist;
				}
			};
			//! 最短距離リスト
			spn::AssocVec<LmLen>	_asv;
			//! デバッグ用
			void _printASV(std::ostream& os) const;

			union {
				Vec2x2	_pvec;
				Vec2x2	_nvec;
			};
			//! v0.firstとv1.firstからなる線分候補をリストに追加
			/*!	\return 最近傍点が原点と重なっていれば0x02, 線分候補が追加されれば0x01, それ以外は0x00 */
			int _addAsv(const Vec2x2& v0, const Vec2x2& v1);

			//! 指定方向へのミンコフスキー差
			/*! \param[in] n 計算した頂点の挿入先インデックス */
			const Vec2x2& _minkowskiSub(const Vec2& dir, int n=-1);
			//! Hit時の脱出ベクトル
			/*! 最低でも3頂点以上持っている前提 */
			void _epaMethodOnHit(float threshold);
			//! NoHit時の最短ベクトル
			void _epaMethodNoHit(float threshold);

			//! NoHit: 頂点が2つしか無い時の補正
			void _recover2NoHit();
			//! Hit: 頂点が2つしか無い時の補正
			void _recover2OnHit();
			//! 頂点の並び順を時計回りに修正
			void _adjustLoop3();
			//! 頂点リスト(3つ以上)から最短距離リストを生成
			void _geneASV();
			void _clear();

			public:
				GEpa(const IModel& m0, const IModel& m1, float threshold=NEAR_THRESHOLD);
				~GEpa();
				/*! 非衝突時に有効
					\return A側の最近傍点, B側の最近傍点 */
				const Vec2x2& getNearestPair() const;
				/*! 衝突時にそれを回避するための最短移動ベクトル(A側)
					\return first=A側の最深点 second=A側の回避ベクトル */
				const Vec2x2& getPVector() const;
		};
		//! ミンコフスキー差を求める
		Vec2 MinkowskiSub(const IModel& m0, const IModel& m1, const Vec2& dir);
		//! DualTransform (point2D -> line2D)
		Line Dual(const Vec2& v);
		//! DualTransform (line2D -> point2D)
		Vec2 Dual(const Line& ls);
		Vec2 Dual2(const Vec2& v0, const Vec2& v1);
		//! DualTransform (point3D -> plane)
		Plane Dual(const Vec3& v);
		//! DualTransform (plane -> point3D)
		Vec3 Dual(const Plane& plane);

		template <class CLIP>
		inline Vec2 NearestPoint(const Line& ls, const Vec2& p, CLIP clip) {
			Vec2 toP = p - ls.pos;
			float d = ls.dir.dot(toP);
			return ls.pos + ls.dir * clip(d);
		}
		bool IsCW(const PointL& pts);
		bool IsCrossing(const Line& ls0, const Line& ls1, float len0, float len1, float t=NEAR_THRESHOLD);
		template <class CLIP0, class CLIP1>
		inline Vec2x2_OP NearestPoint(const Line& ls0, const Line& ls1, CLIP0 clip0, CLIP1 clip1) {
			float st0d = ls0.dir.len_sq(),
					st1d = ls1.dir.len_sq(),
					st01d = ls0.dir.dot(ls1.dir);
			float d = st0d * st1d - spn::Square(st01d);
			if(std::fabs(d) < 1e-5f) {
				// 2つの直線は平行
				return spn::none;
			}
			Mat22 m0(st1d, st01d,
					st01d, st0d);
			Vec2	m1((ls1.pos - ls0.pos).dot(ls0.dir),
						(ls0.pos - ls1.pos).dot(ls1.dir));
			m1 = m0 * m1;
			m1 *= spn::Rcp22Bit(d);
			return Vec2x2(ls0.pos + ls0.dir * clip0(m1.x),
							ls1.pos + ls1.dir * clip1(m1.y));
		}

		struct Types {
			using CTGeo = ::boom::geo2d::CTGeo;
			using IModel = ::boom::geo2d::IModel;
			using GJK = ::boom::geo2d::GSimplex;
			using Narrow = ::boom::Narrow<Types>;
		};
	}
}


#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <queue>
#include <cstdlib>
#include <cassert>
#include <limits>

#include <boost/foreach.hpp>
#include <boost/thread.hpp>
#include <boost/chrono.hpp>
#include <boost/random.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>

#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>
#include <pcl_ros/transforms.h>

#include <octomap/octomap.h>
#include <octomap_ros/conversions.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/boundary.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <pcl_conversions/pcl_conversions.h>

template<typename PointA, typename PointB>
double sqdist(const PointA& a, const PointB& b)
{
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    double dz = a.z - b.z;
    return dx * dx + dy * dy + dz * dz;
}

template<typename PointA, typename PointB>
double dist(const PointA& a, const PointB& b)
{
    return sqrt(sqdist(a, b));
}

class MoveBase
{
protected:
    ros::NodeHandle nh_;
    ros::NodeHandle pnh_;
    std::string frame_id_;
    std::string robot_frame_id_;
    ros::Subscriber navfn_sub_;
    ros::Subscriber goal_point_sub_;
    ros::Subscriber goal_pose_sub_;
    ros::Publisher twist_pub_;
    ros::Publisher target_pub_;
    ros::Publisher position_error_pub_;
    ros::Publisher orientation_error_pub_;
    tf::TransformListener tf_listener_;
    geometry_msgs::PoseStamped robot_pose_;
    geometry_msgs::PoseStamped goal_;
    pcl::PointCloud<pcl::PointXYZI> navfn_;
    pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr navfn_octree_ptr_;
    ros::Timer controller_timer_;
    double robot_radius_;
    double goal_reached_threshold_;
    double controller_frequency_;
    double local_target_radius_;
    double twist_linear_gain_;
    double twist_angular_gain_;
    bool reached_position_;
    int controller_repeated_failures_;
    void startController();
public:
    MoveBase();
    ~MoveBase();
    void onNavigationFunctionChange(const sensor_msgs::PointCloud2::ConstPtr& msg);
    void onGoal(const geometry_msgs::PointStamped::ConstPtr& msg);
    void onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg);
    int projectPositionToNavigationFunction(const geometry_msgs::Point& pos);
    void getNavigationFunctionNeighborhood(const geometry_msgs::Point& pos, pcl::PointCloud<pcl::PointXYZI>& neighbors);
    bool projectGoalPositionToNavigationFunction();
    bool getRobotPose();
    double positionError();
    double orientationError();
    bool generateLocalTarget(geometry_msgs::PointStamped& p_local);
    void generateTwistCommand(const geometry_msgs::PointStamped& local_target, geometry_msgs::Twist& twist);
    void controllerCallback(const ros::TimerEvent& event);
};

MoveBase::MoveBase()
    : pnh_("~"),
      frame_id_("/map"),
      robot_frame_id_("/base_link"),
      robot_radius_(0.2),
      goal_reached_threshold_(0.5),
      controller_frequency_(2.0),
      local_target_radius_(0.4),
      twist_linear_gain_(0.5),
      twist_angular_gain_(1.0),
      reached_position_(false),
      controller_repeated_failures_(0)
{
    pnh_.param("frame_id", frame_id_, frame_id_);
    pnh_.param("robot_frame_id", robot_frame_id_, robot_frame_id_);
    pnh_.param("robot_radius", robot_radius_, robot_radius_);
    pnh_.param("goal_reached_threshold", goal_reached_threshold_, goal_reached_threshold_);
    pnh_.param("controller_frequency", controller_frequency_, controller_frequency_);
    pnh_.param("local_target_radius", local_target_radius_, local_target_radius_);
    pnh_.param("twist_linear_gain", twist_linear_gain_, twist_linear_gain_);
    pnh_.param("twist_angular_gain", twist_angular_gain_, twist_angular_gain_);
    navfn_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("navfn_in", 1, &MoveBase::onNavigationFunctionChange, this);
    goal_point_sub_ = nh_.subscribe<geometry_msgs::PointStamped>("goal_point_in", 1, &MoveBase::onGoal, this);
    goal_pose_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("goal_pose_in", 1, &MoveBase::onGoal, this);
    twist_pub_ = nh_.advertise<geometry_msgs::Twist>("twist_out", 1, false);
    target_pub_ = nh_.advertise<geometry_msgs::PointStamped>("target_out", 1, false);
    position_error_pub_ = nh_.advertise<std_msgs::Float32>("position_error", 10, false);;
    orientation_error_pub_ = nh_.advertise<std_msgs::Float32>("orientation_error", 10, false);;
}

MoveBase::~MoveBase()
{
}

void MoveBase::onNavigationFunctionChange(const sensor_msgs::PointCloud2::ConstPtr& pcl_msg)
{
    pcl::PointCloud<pcl::PointXYZI> pcl;
    pcl::fromROSMsg(*pcl_msg, pcl);

    navfn_.clear();
    pcl_ros::transformPointCloud(frame_id_, pcl, navfn_, tf_listener_);

    navfn_octree_ptr_ = pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>::Ptr(new pcl::octree::OctreePointCloudSearch<pcl::PointXYZI>(0.01));
    navfn_octree_ptr_->setInputCloud(navfn_.makeShared());
    navfn_octree_ptr_->addPointsFromInputCloud();
}

void MoveBase::startController()
{
    reached_position_ = false;
    controller_repeated_failures_ = 0;
    controller_timer_ = nh_.createTimer(ros::Duration(1.0 / controller_frequency_), &MoveBase::controllerCallback, this);
}

void MoveBase::onGoal(const geometry_msgs::PointStamped::ConstPtr& msg)
{
    try
    {
        geometry_msgs::PointStamped msg2;
        tf_listener_.transformPoint(frame_id_, *msg, msg2);
        goal_.header.stamp = msg2.header.stamp;
        goal_.header.frame_id = msg2.header.frame_id;
        goal_.pose.position.x = msg2.point.x;
        goal_.pose.position.y = msg2.point.y;
        goal_.pose.position.z = msg2.point.z;
        goal_.pose.orientation.x = 0.0;
        goal_.pose.orientation.y = 0.0;
        goal_.pose.orientation.z = 0.0;
        goal_.pose.orientation.w = 0.0;

        if(projectGoalPositionToNavigationFunction())
        {
            ROS_INFO("goal set to point (%f, %f, %f)",
                goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z);

            startController();
        }
    }
    catch(tf::TransformException& ex)
    {
        ROS_ERROR("Failed to lookup robot position: %s", ex.what());
    }
}

void MoveBase::onGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    try
    {
        tf_listener_.transformPose(frame_id_, *msg, goal_);

        if(projectGoalPositionToNavigationFunction())
        {
            ROS_INFO("goal set to pose (%f, %f, %f), (%f, %f, %f, %f)",
                    goal_.pose.position.x, goal_.pose.position.y, goal_.pose.position.z,
                    goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z,
                    goal_.pose.orientation.w);

            startController();
        }
    }
    catch(tf::TransformException& ex)
    {
        ROS_ERROR("Failed to lookup robot position: %s", ex.what());
    }
}

int MoveBase::projectPositionToNavigationFunction(const geometry_msgs::Point& pos)
{
    pcl::PointXYZI p;
    p.x = pos.x;
    p.y = pos.y;
    p.z = pos.z;
    std::vector<int> pointIdx;
    std::vector<float> pointDistSq;
    if(navfn_octree_ptr_->nearestKSearch(p, 1, pointIdx, pointDistSq) < 1)
    {
        return -1;
    }
    return pointIdx[0];
}

void MoveBase::getNavigationFunctionNeighborhood(const geometry_msgs::Point& pos, pcl::PointCloud<pcl::PointXYZI>& neighbors)
{
    pcl::PointXYZI robot_position;

    robot_position.x = pos.x;
    robot_position.y = pos.y;
    robot_position.z = pos.z;

    std::vector<int> pointIdx;
    std::vector<float> pointDistSq;

    navfn_octree_ptr_->radiusSearch(robot_position, local_target_radius_, pointIdx, pointDistSq);

    neighbors.header.frame_id = navfn_.header.frame_id;
    neighbors.header.stamp = navfn_.header.stamp;
    for(std::vector<int>::iterator it = pointIdx.begin(); it != pointIdx.end(); ++it)
        neighbors.push_back(navfn_[*it]);
}

bool MoveBase::projectGoalPositionToNavigationFunction()
{
    int goal_index = projectPositionToNavigationFunction(goal_.pose.position);
    if(goal_index == -1)
    {
        ROS_ERROR("Failed to project goal position to navfn pcl");
        return false;
    }
    if(dist(goal_.pose.position, navfn_[goal_index]) > robot_radius_)
    {
        ROS_ERROR("Failed to project goal position to navfn pcl (point is too far from ground)");
        return false;
    }
    goal_.pose.position.x = navfn_[goal_index].x;
    goal_.pose.position.y = navfn_[goal_index].y;
    goal_.pose.position.z = navfn_[goal_index].z;
    return true;
}

bool MoveBase::getRobotPose()
{
    try
    {
        geometry_msgs::PoseStamped robot_pose_local;
        robot_pose_local.header.frame_id = robot_frame_id_;
        robot_pose_local.pose.position.x = 0.0;
        robot_pose_local.pose.position.y = 0.0;
        robot_pose_local.pose.position.z = 0.0;
        robot_pose_local.pose.orientation.x = 0.0;
        robot_pose_local.pose.orientation.y = 0.0;
        robot_pose_local.pose.orientation.z = 0.0;
        robot_pose_local.pose.orientation.w = 1.0;
        tf_listener_.transformPose(frame_id_, robot_pose_local, robot_pose_);
        return true;
    }
    catch(tf::TransformException& ex)
    {
        ROS_ERROR("Failed to lookup robot position: %s", ex.what());
    }
}

double MoveBase::positionError()
{
    return sqrt(
            pow(robot_pose_.pose.position.x - goal_.pose.position.x, 2) +
            pow(robot_pose_.pose.position.y - goal_.pose.position.y, 2) +
            pow(robot_pose_.pose.position.z - goal_.pose.position.z, 2)
    );
}

double MoveBase::orientationError()
{
    // check if goal is only by position:
    double qnorm = pow(goal_.pose.orientation.w, 2) +
            pow(goal_.pose.orientation.x, 2) +
            pow(goal_.pose.orientation.y, 2) +
            pow(goal_.pose.orientation.z, 2);

    // if so, we never have an orientation error:
    if(qnorm < 1e-5) return 0;

    // "Robotica - Modellistica Pianificazione e Controllo" eq. 3.88
    double nd = goal_.pose.orientation.w,
            ne = robot_pose_.pose.orientation.w;
    Eigen::Vector3d ed(goal_.pose.orientation.x, goal_.pose.orientation.y, goal_.pose.orientation.z),
            ee(robot_pose_.pose.orientation.x, robot_pose_.pose.orientation.y, robot_pose_.pose.orientation.z);
    Eigen::Vector3d eo = ne * ed - nd * ee - ed.cross(ee);
    return eo(2);
}

bool MoveBase::generateLocalTarget(geometry_msgs::PointStamped& p_local)
{
    // get navigation function neighborhood centered at robot pos:
    pcl::PointCloud<pcl::PointXYZI> neighbors, neighbors_local;
    getNavigationFunctionNeighborhood(robot_pose_.pose.position, neighbors);

    if(!pcl_ros::transformPointCloud(robot_frame_id_, neighbors, neighbors_local, tf_listener_))
    {
        ROS_ERROR("Failed to transform robot neighborhood");
        return false;
    }

    // find minimum distance point in neighborhood:
    int min_index = -1, min_index_straight = -1;
    float min_value = std::numeric_limits<float>::infinity();

    for(size_t i = 0; i < neighbors_local.size(); i++)
    {
        pcl::PointXYZI& p = neighbors_local[i];

        if(p.intensity < min_value)
        {
            min_value = p.intensity;
            min_index = i;
        }
    }

    if(min_index == -1)
    {
        ROS_ERROR("Failed to find a target in robot vicinity");
        return false;
    }

    // check if we are actually improving the value in the navigation function
    int rob_index = projectPositionToNavigationFunction(robot_pose_.pose.position);
    double delta = navfn_[rob_index].intensity - min_value;
    if(delta < 1e-6)
    {
        ROS_ERROR("Failed to generate a target: gradient is null");
        return false;
    }

    p_local.header.stamp = ros::Time::now();
    p_local.header.frame_id = neighbors_local.header.frame_id;
    p_local.point.x = neighbors_local[min_index].x;
    p_local.point.y = neighbors_local[min_index].y;
    p_local.point.z = neighbors_local[min_index].z;

    target_pub_.publish(p_local);

    return true;
}

void MoveBase::generateTwistCommand(const geometry_msgs::PointStamped& local_target, geometry_msgs::Twist& twist)
{
    if(local_target.header.frame_id != robot_frame_id_)
    {
        ROS_ERROR("generateTwistCommand: local_target must be in frame '%s'", robot_frame_id_.c_str());
        return;
    }

    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;

    const geometry_msgs::Point& p = local_target.point;

    if(p.x < 0 || fabs(p.y) > p.x)
    {
        // turn in place
        twist.angular.z = (p.y > 0 ? 1 : -1) * twist_angular_gain_;
    }
    else
    {
        // make arc
        double center_y = (pow(p.x, 2) + pow(p.y, 2)) / (2 * p.y);
        double theta = fabs(atan2(p.x, fabs(center_y) - fabs(p.y)));
        double arc_length = fabs(center_y * theta);

        twist.linear.x = twist_linear_gain_ * arc_length;
        twist.angular.z = twist_angular_gain_ * (p.y >= 0 ? 1 : -1) * theta;
    }
}

void MoveBase::controllerCallback(const ros::TimerEvent& event)
{
    if(controller_repeated_failures_ >= 5)
    {
        ROS_ERROR("controller keeps failing. giving up :-(");
        controller_timer_.stop();
        return;
    }

    if(!getRobotPose())
    {
        controller_repeated_failures_++;
        ROS_ERROR("controllerCallback: failed to get robot pose");
        return;
    }

    geometry_msgs::Twist twist;
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 0.0;

    std_msgs::Float32 ep, eo;

    ep.data = positionError();
    position_error_pub_.publish(ep);

    eo.data = orientationError();
    orientation_error_pub_.publish(eo);

    const char *status_str;

    if((!reached_position_ && ep.data > goal_reached_threshold_)
        || (reached_position_ && ep.data > 2 * goal_reached_threshold_))
    {
        // regulate position

        status_str = "REGULATING POSITION";

        reached_position_ = false;

        geometry_msgs::PointStamped local_target;

        if(!generateLocalTarget(local_target))
        {
            controller_repeated_failures_++;
            ROS_ERROR("controllerCallback: failed to generate a local target to follow");
            return;
        }

        generateTwistCommand(local_target, twist);
    }
    else
    {
        reached_position_ = true;

        if(fabs(eo.data) > 0.02)
        {
            // regulate orientation

            status_str = "REGULATING ORIENTATION";

            twist.angular.z = twist_angular_gain_ * eo.data;
        }
        else
        {
            // goal reached

            status_str = "REACHED GOAL";

            ROS_INFO("goal reached! stopping controller timer");

            controller_timer_.stop();
        }
    }

    ROS_INFO("controller: ep=%f, eo=%f, status=%s", ep.data, eo.data, status_str);

    twist_pub_.publish(twist);

    controller_repeated_failures_ = 0;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_base");

    MoveBase p;
    ros::spin();

    return 0;
}


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Ion Gaztanaga 2005-2012. 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)
//
// See http://www.boost.org/libs/interprocess for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_INTERPROCESS_DETAIL_TIMEPOINT_TO_TIMESPEC_HPP
#define BOOST_INTERPROCESS_DETAIL_TIMEPOINT_TO_TIMESPEC_HPP

#ifndef BOOST_CONFIG_HPP
#  include <boost/config.hpp>
#endif
#
#if defined(BOOST_HAS_PRAGMA_ONCE)
#  pragma once
#endif

#include <boost/interprocess/detail/mpl.hpp>
#include <boost/interprocess/detail/type_traits.hpp>
#include <boost/interprocess/detail/timed_utils.hpp>

namespace boost {

namespace interprocess {

namespace ipcdetail {

template<class TimePoint>
inline timespec timepoint_to_timespec ( const TimePoint &tm
                                      , typename enable_if_ptime<TimePoint>::type * = 0)
{
   typedef typename TimePoint::date_type            date_type;
   typedef typename TimePoint::time_duration_type   time_duration_type;

   const TimePoint epoch(date_type(1970,1,1));

   //Avoid negative absolute times
   time_duration_type duration  = (tm <= epoch) ? time_duration_type(epoch - epoch)
                                                : time_duration_type(tm - epoch);
   timespec ts;
   ts.tv_sec  = static_cast<time_t>(duration.total_seconds());
   ts.tv_nsec = static_cast<long>(duration.total_nanoseconds() % 1000000000);
   return ts;
}

inline timespec timepoint_to_timespec (const ustime &tm)
{
   timespec ts;
   ts.tv_sec  = static_cast<time_t>(tm.get_microsecs()/1000000u);
   ts.tv_nsec = static_cast<long>((tm.get_microsecs()%1000000u)*1000u);
   return ts;
}

template<class TimePoint>
inline timespec timepoint_to_timespec ( const TimePoint &tm
                                      , typename enable_if_time_point<TimePoint>::type * = 0)
{
   typedef typename TimePoint::duration duration_t;
   duration_t d(tm.time_since_epoch());

   timespec ts;
   BOOST_IF_CONSTEXPR(duration_t::period::num == 1 && duration_t::period::den == 1000000000)
   {
      ts.tv_sec  = static_cast<time_t>(d.count()/duration_t::period::den);
      ts.tv_nsec = static_cast<long>(d.count()%duration_t::period::den);
   }
   else
   {
      const double factor = double(duration_t::period::num)/double(duration_t::period::den);
      const double res = d.count()*factor;
      ts.tv_sec  = static_cast<time_t>(res);
      ts.tv_nsec = static_cast<long>(res - double(ts.tv_sec));
   }
   return ts;
}

}  //namespace ipcdetail {

}  //namespace interprocess {

}  //namespace boost {

#endif   //ifndef BOOST_INTERPROCESS_DETAIL_TIMEPOINT_TO_TIMESPEC_HPP


#include "GLWindow.h"
#include <iostream>
#include <ngl/Camera.h>
#include <ngl/Colour.h>
#include <ngl/Light.h>
#include <ngl/Mat4.h>
#include <ngl/Transformation.h>
#include <ngl/Material.h>
#include <ngl/NGLInit.h>
#include <ngl/Obj.h>
#include <ngl/Random.h>
#include <ngl/VAOPrimitives.h>
#include <ngl/ShaderLib.h>
#include <boost/foreach.hpp>

#include <iostream>

//----------------------------------------------------------------------------------------------------------------------
GLWindow::GLWindow(const QGLFormat _format, int _timer, QWidget *_parent ) : QGLWidget( _format, _parent )
{
  // set this widget to have the initial keyboard focus
  setFocus();
  // re-size the widget to that of the parent (in this case the GLFrame passed in on construction)
  this->resize( _parent->size() );
  // Now set the initial GLWindow attributes to default values
  m_rotate = false;
  m_spinXFace = 0;
  m_spinYFace = 0;

  m_timerValue = _timer;

  m_cell_count = 0;
}

//This virtual function is called once before the first call to paintGL() or resizeGL(),
//and then once whenever the widget has been assigned a new QGLContext.
//This function should set up any required OpenGL context rendering flags, defining display lists, etc.

//----------------------------------------------------------------------------------------------------------------------
void GLWindow::initializeGL()
{
  ngl::NGLInit::instance();
  glClearColor(0.05f, 0.05f, 0.05f, 1.0f);
  glEnable(GL_DEPTH_TEST);

  //Setup camera
  ngl::Vec3 from(0.f, 0.f, 1250.f);
  ngl::Vec3 to(0.f, 0.f, 0.f);
  ngl::Vec3 up(0.f, 1.f, 0.f);
  m_cam = new ngl::Camera(from, to, up);
  m_cam->setShape(30.f, 1.f, 50.f, 1600.f);

  //Construct shader
  ngl::ShaderLib *shader=ngl::ShaderLib::instance();
  shader->createShaderProgram("Diffuse");
  shader->attachShader("DiffuseVertex",ngl::VERTEX);
  shader->attachShader("DiffuseFragment",ngl::FRAGMENT);
  shader->loadShaderSource("DiffuseVertex","shaders/DiffuseVertex.glsl");
  shader->loadShaderSource("DiffuseFragment","shaders/DiffuseFragment.glsl");
  shader->compileShader("DiffuseVertex");
  shader->compileShader("DiffuseFragment");
  shader->attachShaderToProgram("Diffuse","DiffuseVertex");
  shader->attachShaderToProgram("Diffuse","DiffuseFragment");
  shader->linkProgramObject("Diffuse");
  (*shader)["Diffuse"]->use();

  //Set light information (light intensity is that of the lights area)
  shader->setShaderParam1f("Light_Intensity", 900.f * 900.f);
  shader->setShaderParam3f("Light_Colour", 1.65f, 1.6f, 1.55f);
  shader->setShaderParam3f("Light_Position", 0.f, 1200.f, 0.f);
  shader->setShaderParam3f("Light_Direction", 0.f, -1.f, 0.f);

  //Create sphere primitive
  ngl::VAOPrimitives *prim = ngl::VAOPrimitives::instance();
  prim->createSphere("Sphere", 1.f, 20);

  //Get innitial data from simulation
  m_cell_count = m_tissue->exportData(m_tissue_data);
}

//----------------------------------------------------------------------------------------------------------------------
//This virtual function is called whenever the widget has been resized.
// The new size is passed in width and height.
void GLWindow::resizeGL( int _w, int _h)
{
  glViewport(0, 0, _w, _h);
  m_cam->setShape(30.f, static_cast<float>(_w) / static_cast<float>(_h), 30.f, 1600.f);
}

//Load camera data to shaders
void GLWindow::loadMatricesToShader()
{
  ngl::ShaderLib *shader = ngl::ShaderLib::instance();
  (*shader)["Diffuse"]->use();

  ngl::Mat4 M;
  ngl::Mat4 MV;
  ngl::Mat4 MVP;

  M = m_transform.getMatrix();
  MV = M * m_mouseGlobalTX * m_cam->getViewMatrix();
  MVP = MV * m_cam->getProjectionMatrix();

  shader->setShaderParamFromMat4("MVP", MVP);
  shader->setShaderParamFromMat4("M", M);
}

//----------------------------------------------------------------------------------------------------------------------
//This virtual function is called whenever the widget needs to be painted.
// this is our main drawing routine
void GLWindow::paintGL()
{
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  //Update camera data
  ngl::Mat4 rotX;
  ngl::Mat4 rotY;
  rotX.rotateX(m_spinXFace);
  rotY.rotateY(m_spinYFace);
  m_mouseGlobalTX = rotY * rotX;

  //Find position of simulation
  Vector3f tissue_position = m_tissue->position() * -1;

  //Set colour and offset light postion with simulation position
  ngl::ShaderLib *shader = ngl::ShaderLib::instance();
  (*shader)["Diffuse"]->use();
  shader->setShaderParam4f("Colour", 0.9f, 0.9f, 0.9f, 1.f);
  shader->setShaderParam3f("Light_Position", tissue_position.x(), tissue_position.y() + 1200.f, tissue_position.z());

  ngl::VAOPrimitives *prim = ngl::VAOPrimitives::instance();

  //Draw simulation taking into account position
  for (int i = 0; i < m_cell_count; ++i)
  {
    ngl::Vec4 position = ngl::Vec4(m_tissue_data[i * 4 + 0] + tissue_position.x(), m_tissue_data[i * 4 + 1] + tissue_position.y(), m_tissue_data[i * 4 + 2] + tissue_position.z(), 0.f);
    m_transform.setScale(m_tissue_data[i * 4 + 3], m_tissue_data[i * 4 + 3], m_tissue_data[i * 4 + 3]);
    m_transform.setPosition(position);
    loadMatricesToShader();

    // draw the sphere
    prim->draw("Sphere");
  }
}

//----------------------------------------------------------------------------------------------------------------------
void GLWindow::mouseMoveEvent ( QMouseEvent * _event )
{
  // note the method buttons() is the button state when event was called
  // this is different from button() which is used to check which button was
  // pressed when the mousePress/Release event is generated
  if(m_rotate && _event->buttons() == Qt::LeftButton)
  {
    m_spinYFace = ( m_spinYFace + (_event->x() - m_origX) ) % 360 ;
    m_spinXFace = ( m_spinXFace + (_event->y() - m_origY) ) % 360 ;
    m_origX = _event->x();
    m_origY = _event->y();
  }
  // re-draw GL
  updateGL();
}

//----------------------------------------------------------------------------------------------------------------------
void GLWindow::mousePressEvent ( QMouseEvent * _event  )
{
  // this method is called when the mouse button is pressed in this case we
  // store the value where the maouse was clicked (x,y) and set the Rotate flag to true
  if(_event->button() == Qt::LeftButton)
  {
    m_origX = _event->x();
    m_origY = _event->y();
    m_rotate = true;
  }
}

//----------------------------------------------------------------------------------------------------------------------
void GLWindow::mouseReleaseEvent ( QMouseEvent * _event )
{
  // this event is called when the mouse button is released
  // we then set Rotate to false
  if (_event->button() == Qt::LeftButton)
  {
    m_rotate = false;
  }
}

void GLWindow::timerEvent( QTimerEvent *_event)
{
  m_tissue->simulate();
  m_cell_count = m_tissue->exportData(m_tissue_data);
  updateGL();
}

//Reset simulation to innitial construction
void GLWindow::reset()
{
  m_tissue->construction();
  m_cell_count = m_tissue->exportData(m_tissue_data);
  updateGL();
}

//Return camera position for offline renderer
Vector3f GLWindow::getCameraPos() const
{
  ngl::Mat4 rotation = m_mouseGlobalTX;
  ngl::Vec3 camera_position = rotation.inverse().getBackVector();
  return Vector3f(camera_position.m_x, camera_position.m_y, camera_position.m_z) * 1250.f;
}

void GLWindow::startSimTimer()
{
  m_timer=startTimer(m_timerValue);
}

void GLWindow::stopSimTimer()
{
  killTimer(m_timer);
}

GLWindow::~GLWindow()
{
	ngl::NGLInit *init = ngl::NGLInit::instance();
	init->NGLQuit();
}


#include "vtkboneAIMWriter.h"
#include "vtkObjectFactory.h"
#include "vtkPointData.h"
#include "vtkDataArray.h"
#include "vtkMath.h"
#include "vtkImageData.h"
#include "vtkCellData.h"
#include "vtkInformation.h"
#include "vtkSmartPointer.h"
#include "AimIO/AimIO.h"
#include <sstream>
#include <string>
#include <time.h>
#include <boost/format.hpp>

using boost::format;
using std::string;
using std::ostringstream;

//-----------------------------------------------------------------------
// Private Utility Stuff

inline int roundFloat (const float x)
{
  return floor (x+0.5f);
}

vtkStandardNewMacro (vtkboneAIMWriter);

//-----------------------------------------------------------------------
vtkboneAIMWriter::vtkboneAIMWriter()
{
  this->FileName = NULL;
  this->NewProcessingLog = 1; // Default is to add the base AIM log info
  this->ProcessingLog = NULL;
}

//----------------------------------------------------------------------------
vtkboneAIMWriter::~vtkboneAIMWriter()
{
  this->SetFileName(0);   // Frees memory
  this->SetProcessingLog(0);   // Frees memory
}

//----------------------------------------------------------------------------
void vtkboneAIMWriter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);
  os << indent << "FileName: " << this->FileName << "\n";
  os << indent << "NewProcessingLog: " << this->NewProcessingLog << "\n";
  os << indent << "ProcessingLog: "
     << (this->ProcessingLog ? this->ProcessingLog : "(none)") << "\n";
}

//----------------------------------------------------------------------------
void vtkboneAIMWriter::AppendToLog (const char* s)
{
  // Could be more efficient.  But this will never segfault or leak memory.
  this->SetProcessingLog ((string(ProcessingLog) + s).c_str());
}

//----------------------------------------------------------------------------
void vtkboneAIMWriter::WriteData()
{
  AimIO::AimFile writer;

  // // Do not call any deallocation functions on image!  It does not own its data!!!
  // D3AnyImage image;
  // D3InitAny(image,D1Tundef);
  // D3int          dim,off,pos;
  // D3float        el_size_mm;
  // int            status;
  // int            dimension[3];
  // double          origin[3], spacing[3], conversionError[3];
  // time_t        rawtime;
  // struct        tm *timeinfo;
  // int position[3];

  vtkImageData *temp_input= vtkImageData::SafeDownCast(this->GetInput());

  this->UpdateProgress(.01);

  // Now we are going to make sure that the image data is on the points.
  // The offset is strange: it will be 0,0,0 for data centered at pixel
  // centers with corner offset 0,0,0 (it should be 0.5,0.5,0.5).
  // Despite this strange offset, it all works out in the end.
  int     dimension[3];
  double  origin[3];
  double  spacing[3];
  vtkSmartPointer<vtkImageData> input = vtkSmartPointer<vtkImageData>::New();
  temp_input->GetSpacing(spacing);
  temp_input->GetOrigin(origin);
  temp_input->GetDimensions(dimension);
  if (temp_input->GetPointData()->GetScalars())
    {
    input->ShallowCopy(temp_input);
    input->SetOrigin(origin[0] - 0.5*spacing[0],
                     origin[1] - 0.5*spacing[1],
                     origin[2] - 0.5*spacing[2]);
    }
  else if (temp_input->GetCellData()->GetScalars())
    {
    input->SetSpacing(spacing);
    input->SetOrigin(origin);
    input->SetDimensions(dimension[0] - 1,
                         dimension[1] - 1,
                         dimension[2] - 1);
    input->GetPointData()->SetScalars(temp_input->GetCellData()->GetScalars());
    }
  else
    {
    vtkErrorMacro(<<"Could not get data from input.");
    return;
    }

  vtkDebugMacro(<<"Writing AIM file...");

  if (!this->FileName)
    {
      vtkErrorMacro(<<"An output filename must be specified.");
      return;
    }
  writer.filename = this->FileName;

  // Do this again - they may have changed.
  input->GetDimensions(dimension);
  input->GetSpacing(spacing);
  input->GetOrigin(origin);

  writer.dimensions[0] = dimension[0];
  writer.dimensions[1] = dimension[1];
  writer.dimensions[2] = dimension[2];

  writer.position[0] = roundFloat(origin[0]/spacing[0]);
  writer.position[1] = roundFloat(origin[1]/spacing[1]);
  writer.position[2] = roundFloat(origin[2]/spacing[2]);

  double conversionError[3];
  conversionError[0] = spacing[0]*writer.position[0] - origin[0];
  conversionError[1] = spacing[1]*writer.position[1] - origin[1];
  conversionError[2] = spacing[2]*writer.position[2] - origin[2];

  writer.element_size[0] = spacing[0];
  writer.element_size[1] = spacing[1];
  writer.element_size[2] = spacing[2];

  // if (fabs(conversionError[0]) > 0.000001 ||
  //     fabs(conversionError[1]) > 0.000001 ||
  //     fabs(conversionError[2]) > 0.000001) {
  //
  //       vtkWarningMacro(<<"\n  Possible error when calculating AIM position from image origin."
  //                         "\n  SPACING * POSITION - ORIGIN = ERROR"
  //                         "\n    X: "
  //                       << spacing[0] << " * "
  //                       << position[0] << " - "
  //                       << origin[0] << " = "
  //                       << conversionError[0] << " "
  //                       << (fabs(conversionError[0]) > 0.0000001 ? "<- error?" : "")
  //                       << "\n    Y: "
  //                       << spacing[1] << " * "
  //                       << position[1] << " - "
  //                       << origin[1] << " = "
  //                       << conversionError[1] << " "
  //                       << (fabs(conversionError[1]) > 0.0000001 ? "<- error?" : "")
  //                       << "\n    Z: "
  //                       << spacing[2] << " * "
  //                       << position[2] << " - "
  //                       << origin[2] << " = "
  //                       << conversionError[2] << " "
  //                       << (fabs(conversionError[2]) > 0.0000001 ? "<- error?" : "")
  //                         );
  // }

  time_t rawtime;
  time (&rawtime);
  struct tm *timeinfo = localtime(&rawtime);
  this->UpdateProgress(.05);

  // Note: This is a dummy log, and it is important that certain parameters be set so that other
  // Scanco software can properly read the data.  For example, 'mu_scaling' and 'patient name'.
  if (this->NewProcessingLog) {
    ostringstream newLog;
    newLog << "! \n";
    newLog << "! Processing Log \n";
    newLog << "! \n";
    newLog << "!-------------------------------------------------------------------------------\n";
    newLog << "Created by                    vtkboneAIMWriter\n";
    newLog << format("%-30s%-50s\n") % "Time" % asctime(timeinfo);
    newLog << "Original file                 DK0:[MICROCT.DATA.00000062.00001080]C0001021.ISQ\n";
    newLog << "!-------------------------------------------------------------------------------\n";
    newLog << "Site                                                1\n";
    newLog << "Scanner ID                                       5001\n";
    newLog << "Scanner type                                        9\n";
    newLog << "Position Slice 1 [um]                          120000\n";
    newLog << "No. samples                                      1536\n";
    newLog << "No. projections per 180                           750\n";
    newLog << "Scan Distance [um]                             777952\n";
    newLog << "Integration time [us]                          100000\n";
    newLog << "Reference line [um]                                 0\n";
    newLog << "Reconstruction-Alg.                                 3\n";
    newLog << "Energy [V]                                      77400\n";
    newLog << "Intensity [uA]                                   7000\n";
    newLog << "!-------------------------------------------------------------------------------\n";
    newLog << "Mu_Scaling                                       8192\n";
    newLog << "Calibration Data              77kVp, BH: 1200 mg HA/ccm, Scaling 8192           \n";
    newLog << "Calib. default unit type      2 (Density)                                       \n";
    newLog << "Density: unit                 mg HA/ccm                                         \n";
    // newLog << "Density: slope                         1.44113599e+03\n";
    // newLog << "Density: intercept                    -3.56445007e+02\n";
    // newLog << "HU: mu water                                  0.24640\n";
    newLog << "Density: slope                         8.19200000e+03\n";
    newLog << "Density: intercept                     0.00000000e+00\n";
    newLog << "HU: mu water                                  0.24640\n";
    //newLog << "!-------------------------------------------------------------------------------\n";
    //newLog << "Parameter (before) name       Linear Attenuation                                \n";
    //newLog << "Parameter units               [1/cm]                                            \n";
    //newLog << "Minimum value                                 0.00000\n";
    //newLog << "Maximum value                            20000.00000\n";
    //newLog << "Average value                                 0.38143\n";
    //newLog << "Standard deviation                            0.21346\n";
    //newLog << "Scaled by factor                                 8192\n";
    //newLog << "Minimum data value                         -852.00000\n";
    //newLog << "Maximum data value                        10717.00000\n";
    //newLog << "Average data value                         3124.65381\n";
    //newLog << "Standard data deviation                    1748.67932\n";
    //newLog << "!-------------------------------------------------------------------------------\n";
    //newLog << "Procedure:                    D3P_Std_FloatNormMax_M()                          \n";
    //newLog << "! \n";
    //newLog << "max                                         127.00000\n";
    //newLog << "! \n";
    //newLog << "Parameter name                Thickness                                         \n";
    //newLog << "Parameter units               [mm]                                              \n";
    //newLog << "!    Zeros not counted. DT type assumed\n";
    //newLog << "Minimum value                                 0.00000\n";
    //newLog << "Maximum value                               127.00000\n";
    //newLog << "Average value                                 0.23753\n";
    //newLog << "Standard deviation                            0.05602\n";
    //newLog << "Scaled by factor                             12.19512\n";
    //newLog << "Minimum data value                            0.00000\n";
    //newLog << "Maximum data value                            4.00000\n";
    //newLog << "Average data value                            2.89675\n";
    //newLog << "Standard data deviation                       0.68320\n";
    // newLog << "!-------------------------------------------------------------------------------\n";

    if (ProcessingLog)
      {
      newLog << ProcessingLog;
      }

    SetProcessingLog(newLog.str().c_str());

  }

  writer.processing_log = ProcessingLog;

  this->UpdateProgress(.1);

  writer.version = AimIO::AIMFILE_VERSION_20;;

  // Write the image
  vtkDataArray* data = input->GetPointData()->GetScalars();
  switch (data->GetDataType())
    {
    case VTK_CHAR:
      writer.WriteImageData ((char*)(data->WriteVoidPointer(0,0)));
      break;
    case VTK_SHORT:
      writer.WriteImageData ((short*)(data->WriteVoidPointer(0,0)));
      break;
    case VTK_FLOAT:
      writer.WriteImageData ((float*)(data->WriteVoidPointer(0,0)));
      break;
    default:
      vtkErrorMacro(<<"Input must be of type VTK_CHAR, VTK_SHORT or VTK_FLOAT.");
      return;
    }

  this->UpdateProgress(1.0);

  return;
}

//----------------------------------------------------------------------------
vtkImageData* vtkboneAIMWriter::GetInput()
{
  return vtkImageData::SafeDownCast(this->Superclass::GetInput());
}

vtkImageData* vtkboneAIMWriter::GetInput(int port)
{
  return vtkImageData::SafeDownCast(this->Superclass::GetInput(port));
}

int vtkboneAIMWriter::FillInputPortInformation(int,
                                           vtkInformation *info)
{
  info->Set(vtkAlgorithm::INPUT_REQUIRED_DATA_TYPE(), "vtkImageData");
  return 1;
}