#include "Window.h"
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/SceneObject.h>
#include <VirtualRobot/MathTools.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>

#include <Eigen/Geometry>

#include <Inventor/actions/SoLineHighlightRenderAction.h>
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
#include <Inventor/nodes/SoMaterial.h>

#include <boost/filesystem.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/assert.hpp>

float TIMER_MS = 30.0f;

Window::Window(VirtualRobot::RobotPtr robot, Qt::WFlags flags) :
		QMainWindow(NULL), robot(robot), computeIK(true) {
	frameNames =
	{
		"LeftLeg_TCP",
		"RightLeg_TCP",
		"TorsoCenter",
		"Waist",
	};

	nodes = robot->getRobotNodeSet("CoMCompensation");

	boost::shared_ptr<Bipedal::DifferentialReferenceIK> diffIK =
	boost::make_shared<Bipedal::DifferentialReferenceIK>(
			nodes,
			robot,
			robot->getRobotNodeSet("ColModelAll"),
			robot->getRobotNode(frameNames[0]),
			robot->getRobotNode(frameNames[1]),
			robot->getRobotNode(frameNames[2]),
			robot->getRobotNode(frameNames[3]));

	referenceIK = boost::dynamic_pointer_cast<Bipedal::ReferenceIK>(diffIK);
	robotInv = diffIK->getInvertedRobot();

	for (auto& s : frameNames)
	{
		refFrames.push_back(robot->getRobotNode(s)->getGlobalPose());
	}

	sceneSep = new SoSeparator();
	sceneSep->ref();
	robotSep = new SoSeparator();
	robotInvSep = new SoSeparator();
	sceneSep->addChild(robotSep);
	sceneSep->addChild(robotInvSep);
	boost::shared_ptr<VirtualRobot::CoinVisualization> visualization =
	robot->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
	robotSep->addChild(visualization->getCoinVisualization());

	visualization =
	robotInv->getVisualization<VirtualRobot::CoinVisualization>(VirtualRobot::SceneObject::Full);
	SoMaterial* mat = new SoMaterial();
	mat->diffuseColor.setValue(0.2, 1.0, 0.2);
	mat->ambientColor.setValue(0.1, 0.8, 0.1);
	mat->setOverride(true);
	robotInvSep->addChild(mat);
	robotInvSep->addChild(visualization->getCoinVisualization());

//    boost::shared_ptr<Bipedal::PolynomialFootstepPlaner> PolyFootPlaner =
//    boost::shared_ptr<Bipedal::PolynomialFootstepPlaner>(new Bipedal::PolynomialFootstepPlaner(robot->getRobotNode(frameNames[0]),robot->getRobotNode(frameNames[1])));

//    PolynomialFootstepPlaner *PolyFootPlane = new PolynomialFootstepPlaner(robot->getRobotNode(frameNames[0]),
//            robot->getRobotNode(frameNames[1]));

    VirtualRobot::RobotNodePtr leftFootBody = robot->getRobotNode("LeftLeg_BodyAnkle2");
    VirtualRobot::RobotNodePtr rightFootBody = robot->getRobotNode("RightLeg_BodyAnkle2");

    boost::shared_ptr<Bipedal::PolynomialFootstepPlaner> PolyFootPlaner =
    boost::make_shared<Bipedal::PolynomialFootstepPlaner>(leftFootBody,rightFootBody);

    Eigen::Matrix4f leftFootPose = leftFootBody->getGlobalPose();
    Eigen::Matrix4f rightFootPose = rightFootBody->getGlobalPose();
    // Todo:
    // Should add Foot Poseture???
    // Set FootstepPlaner
    PolyFootPlaner->setParameters(3,10,double(0.3),double(0.7),double(0.1),double(0.1),double(0.0),double(1.0));
    PolyFootPlaner->generate(leftFootPose,rightFootPose);
    Eigen::Matrix6Xf LeftFootTrajectory, RightFootTrajectory;
    LeftFootTrajectory = PolyFootPlaner->getLeftFootTrajectory();
    RightFootTrajectory = PolyFootPlaner->getRightFootTrajectory();

    std::string sep = "\n----------------------------------------\n";
    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
//    std::cout << LeftFootTrajectory.AlignedMapType<< sep;

    std::cout << "LEFT FOOT POSE:" << std::endl;
    std::cout <<leftFootBody->getGlobalPose()<< std::endl;
    std::cout << "RIGHT FOOT POSE:" << std::endl;
    std::cout <<rightFootBody->getGlobalPose()<< std::endl;
	setupUI();

	viewer->viewAll();
}

Window::~Window() {
	sceneSep->unref();
}

void Window::setupUI() {
	UI.setupUi(this);
	viewer = new SoQtExaminerViewer(UI.frameViewer, "", TRUE,
			SoQtExaminerViewer::BUILD_POPUP);

	// setup
	viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
	viewer->setAccumulationBuffer(true);
	viewer->setGLRenderAction(new SoLineHighlightRenderAction());
	viewer->setTransparencyType(SoGLRenderAction::BLEND);
	viewer->setFeedbackVisibility(true);
	viewer->setSceneGraph(sceneSep);
	viewer->viewAll();

	for (unsigned i = 0; i < frameNames.size(); i++) {
		UI.comboBoxFrames->insertItem(i, frameNames[i].c_str());
	}

    connect(UI.spinBoxX, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.spinBoxY, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.spinBoxZ, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.spinBoxRoll, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.spinBoxPitch, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.spinBoxYaw, SIGNAL(valueChanged(double)), this, SLOT(frameChanged(double)));
    connect(UI.comboBoxFrames, SIGNAL(currentIndexChanged(int)), this, SLOT(updateCurrentRef()));
}

int Window::main() {
SoQt::show(this);
SoQt::mainLoop();
return 0;
}

void Window::quit() {
this->close();
SoQt::exitMainLoop();
}

void Window::closeEvent(QCloseEvent* event) {
quit();
QMainWindow::closeEvent(event);
}

void Window::frameChanged(double) {
unsigned i = UI.comboBoxFrames->currentIndex();
BOOST_ASSERT(i < refFrames.size());

VirtualRobot::MathTools::rpy2eigen4f(UI.spinBoxRoll->value(),
		UI.spinBoxPitch->value(), UI.spinBoxYaw->value(), refFrames[i]);
refFrames[i](0, 3) = UI.spinBoxX->value();
refFrames[i](1, 3) = UI.spinBoxY->value();
refFrames[i](2, 3) = UI.spinBoxZ->value();

updateIK();
}

void Window::updateIK() {
    if (!computeIK)
        return;

    std::cout << "Using the following frames for IK:" << std::endl;
    for (auto& f : refFrames) {
        std::cout << f << std::endl;
    }

    Eigen::VectorXf result;
    Bipedal::SupportPhase phase;

    if (UI.radioButtonLeftSupport->isChecked()) {
        phase = Bipedal::SUPPORT_LEFT;
    } else if (UI.radioButtonRightSupport->isChecked()) {
        phase = Bipedal::SUPPORT_RIGHT;
    } else if (UI.radioButtonDualSupport->isChecked()) {
        phase = Bipedal::SUPPORT_BOTH;
    }
    referenceIK->computeStep(refFrames[0], refFrames[1], refFrames[2], refFrames[3],
            Eigen::Vector3f::Zero(), phase, result);

    //    zmpplaner->

    for (unsigned i = 0; i < frameNames.size(); i++) {
        refFrames[i] = robot->getRobotNode(frameNames[i])->getGlobalPose();
    //        std::cout << refFrames[i] << std::endl;
    }
}

void Window::updateCurrentRef() {
unsigned i = UI.comboBoxFrames->currentIndex();

float xyzrpy[6];

VirtualRobot::MathTools::eigen4f2rpy(refFrames[i], xyzrpy);

computeIK = false;
UI.spinBoxX->setValue(xyzrpy[0]);
UI.spinBoxY->setValue(xyzrpy[1]);
UI.spinBoxZ->setValue(xyzrpy[2]);
UI.spinBoxRoll->setValue(xyzrpy[3]);
UI.spinBoxPitch->setValue(xyzrpy[4]);
UI.spinBoxYaw->setValue(xyzrpy[5]);
computeIK = true;
}


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include "source/calibration/FeatureDetector.h"

#include <boost/timer/timer.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <folly/Format.h>

#include "source/util/Camera.h"
#include "source/util/ThreadPool.h"

DEFINE_int32(deduplicate_radius, 3, "remove duplicate corners found at different octaves");
DEFINE_double(harris_parameter, 0.04, "harris parameter");
DEFINE_double(harris_window_radius, 5, "harris corner detector window radius");
DEFINE_int32(max_corners, 10000, "maximum number of corners to detect at each level");
DEFINE_int32(min_feature_distance, 10, "minimum distance between features in pixels");
DEFINE_double(min_feature_quality, 0.00001, "minimum feature quality");
DEFINE_double(
    refine_corners_epsilon,
    0.000001,
    "epsilon termiation value for refining corners to subpixel precision");
DEFINE_int32(refine_corners_radius, 5, "window radius for refining corners to subpixel precision");
DEFINE_int32(zncc_window_radius, 16, "zncc window radius in pixels");

using PixelType = uint8_t;
using Image = cv::Mat_<PixelType>;
using ImageId = std::string;

namespace fb360_dep {
namespace calibration {

bool isUniqueCorner(
    const std::vector<Keypoint>& corners,
    const int previousCornerCount,
    const Camera::Vector2& corner) {
  if (FLAGS_deduplicate_radius <= 0) {
    return true;
  }
  for (int previousCorner = 0; previousCorner < previousCornerCount; previousCorner++) {
    if ((corners[previousCorner].coords - corner).norm() < FLAGS_deduplicate_radius) {
      return false;
    }
  }
  return true;
}

std::vector<Camera::Vector2> findScaledCorners(
    const double scale,
    const cv::Mat_<uint8_t>& imageFull,
    const cv::Mat_<uint8_t>& maskFull,
    const std::string& cameraId) {
  std::vector<Camera::Vector2> cameraCorners;
  std::vector<cv::Point2f> cvCorners;
  cv::Mat_<uint8_t> gray;
  cv::resize(imageFull, gray, cv::Size(), scale, scale, cv::INTER_AREA);
  cv::Mat_<uint8_t> mask;
  if (!maskFull.empty()) {
    cv::resize(maskFull, mask, cv::Size(), scale, scale, cv::INTER_AREA);
  }

  // Find corners using the cv harris detector
  cv::goodFeaturesToTrack(
      gray,
      cvCorners,
      FLAGS_max_corners,
      FLAGS_min_feature_quality,
      FLAGS_min_feature_distance * (FLAGS_same_scale ? scale : 1),
      mask,
      FLAGS_harris_window_radius,
      true,
      FLAGS_harris_parameter);
  if (cvCorners.empty()) {
    return cameraCorners;
  }

  // Refine corners to subpixel precision
  const cv::Size windowRadius(FLAGS_refine_corners_radius, FLAGS_refine_corners_radius);
  const cv::Size zeroZone = cv::Size(-1, -1); // means "no zeroZone"
  cv::TermCriteria criteria =
      cv::TermCriteria(cv::TermCriteria::EPS, 0, FLAGS_refine_corners_epsilon);

  // If refinement fails, opencv silently leaves the inputs untouched. So use unlikely inputs
  const cv::Point2f kOffset(0.0017, 0.0013); // just some unlikely offset
  std::vector<cv::Point2f> cvRefined(cvCorners);
  for (cv::Point2f& p : cvRefined) {
    p += kOffset;
  }
  cv::cornerSubPix(gray, cvRefined, windowRadius, zeroZone, criteria);

  // Only keep refined points, convert out of opencv coordinate convention, scale
  for (ssize_t i = 0; i < ssize(cvRefined); ++i) {
    if (cvRefined[i] != cvCorners[i] + kOffset) {
      cameraCorners.emplace_back((cvRefined[i].x + 0.5f) / scale, (cvRefined[i].y + 0.5f) / scale);
    }
  }

  return cameraCorners;
}

static bool isCloseToEdge(const Camera::Vector2& point, const Image& image, const int margin) {
  if (0 <= point.x() - margin && point.x() + margin < image.cols) {
    if (0 <= point.y() - margin && point.y() + margin < image.rows) {
      return false;
    }
  }
  return true;
}

cv::Mat_<uint8_t> generateImageCircleMask(const Camera& camera) {
  double width = camera.resolution.x();
  double height = camera.resolution.y();
  cv::Mat_<uint8_t> mask(height, width);
  for (double y = 0; y < height; ++y) {
    for (double x = 0; x < width; ++x) {
      Camera::Vector2 pixel = {x + 0.5, y + 0.5};
      mask(y, x) = camera.isOutsideImageCircle(pixel) ? 0 : 255;
    }
  }
  return mask;
}

std::vector<Keypoint> findCorners(const Camera& camera, const Image& image, const bool useNearest) {
  LOG(INFO) << folly::sformat("Processing camera {}... ", camera.id);

  // Search for features at multiple scales
  int rejectedCorners = 0;
  int deduplicatedCorners = 0;
  std::vector<Keypoint> corners;

  // if we're comparing across a single scale, we don't rescale while finding corners
  int octaveCount = FLAGS_same_scale ? 1 : FLAGS_octave_count;
  const cv::Mat_<uint8_t>& mask = generateImageCircleMask(camera);
  for (int octave = 0; octave < octaveCount; ++octave) {
    double scale = std::pow(0.5, octave);
    std::vector<Camera::Vector2> octaveCorners = findScaledCorners(scale, image, mask, camera.id);
    if (FLAGS_log_verbose) {
      LOG(INFO) << folly::sformat(
          "{} found {} corners at scale {}", camera.id, octaveCorners.size(), scale);
    }
    int cornerCountBeforeOctave = corners.size();
    for (const Camera::Vector2& octaveCorner : octaveCorners) {
      if (isCloseToEdge(octaveCorner, image, FLAGS_zncc_window_radius)) {
        rejectedCorners++;
      } else if (!isUniqueCorner(corners, cornerCountBeforeOctave, octaveCorner)) {
        deduplicatedCorners++;
      } else {
        corners.emplace_back(octaveCorner, image, FLAGS_zncc_window_radius, useNearest);
      }
    }
  }

  if (FLAGS_deduplicate_radius != 0) {
    LOG(INFO) << folly::sformat(
        "{} accepted corners: {} deduplicated corners: {} rejected corners {}",
        camera.id,
        corners.size(),
        deduplicatedCorners,
        rejectedCorners);
  } else {
    LOG(INFO) << folly::sformat(
        "{} accepted corners: {} rejected corners {}", camera.id, corners.size(), rejectedCorners);
  }

  return corners;
}

std::map<ImageId, std::vector<Keypoint>>
findAllCorners(const Camera::Rig& rig, const std::vector<Image>& images, const bool useNearest) {
  std::map<ImageId, std::vector<Keypoint>> allCorners;
  boost::timer::cpu_timer featureTimer;
  ThreadPool threadPool(FLAGS_threads);
  for (int currentCamera = 0; currentCamera < ssize(rig); currentCamera++) {
    const Camera& camera = rig[currentCamera];
    std::vector<Keypoint>& keypoints = allCorners[camera.id]; // modify allCorners in main thread
    const Image& image = images[currentCamera];
    threadPool.spawn([&keypoints, &camera, &image, &useNearest] {
      keypoints = findCorners(camera, image, useNearest);
    });
  }
  threadPool.join();

  if (FLAGS_enable_timing) {
    LOG(INFO) << folly::sformat("Find corners stage time: {}", featureTimer.format());
  }

  return allCorners;
}

} // namespace calibration
} // namespace fb360_dep


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [segmented_aligned_range_t]
#include <boost/simd/range/segmented_aligned_range.hpp>
#include <boost/simd/literal.hpp>
#include <iostream>
#include <vector>

int main()
{
  using namespace boost::simd::literal;

  std::vector<float> data{1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};

  boost::simd::segmented_aligned_range_t<float*,4> r = boost::simd::segmented_aligned_range(&data[0], &data[0]+16,4_c);

  for(float v : r.head) std::cout << v << " ";
  std::cout << "\n";

  for(boost::simd::pack<float,4> v : r.body) std::cout << v << " ";
  std::cout << "\n";

  for(float v : r.tail) std::cout << v << " ";
  std::cout << "\n";

  return 0;
}
//! [segmented_aligned_range_t]


// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_INTERSECTION_AREAL_AREAL_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_INTERSECTION_AREAL_AREAL_HPP

#include <boost/core/ignore_unused.hpp>

#include <boost/geometry/algorithms/detail/intersection/interface.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace intersection
{

template
<
    typename GeometryOut,
    typename OutTag = typename detail::intersection::tag
                        <
                            typename geometry::detail::output_geometry_value
                                <
                                    GeometryOut
                                >::type
                        >::type
>
struct intersection_areal_areal_
{
    template
    <
        typename Areal1,
        typename Areal2,
        typename RobustPolicy,
        typename Strategy
    >
    static inline void apply(Areal1 const& areal1,
                             Areal2 const& areal2,
                             RobustPolicy const& robust_policy,
                             GeometryOut& geometry_out,
                             Strategy const& strategy)
    {
        geometry::dispatch::intersection_insert
            <
                Areal1, Areal2,
                typename boost::range_value<GeometryOut>::type,
                overlay_intersection
            >::apply(areal1, areal2, robust_policy,
                     geometry::range::back_inserter(geometry_out),
                     strategy);
    }
};

// TODO: Ideally this should be done in one call of intersection_insert
//   just like it's done for all other combinations
template <typename TupledOut>
struct intersection_areal_areal_<TupledOut, tupled_output_tag>
{
    template
    <
        typename Areal1,
        typename Areal2,
        typename RobustPolicy,
        typename Strategy
    >
    static inline void apply(Areal1 const& areal1,
                             Areal2 const& areal2,
                             RobustPolicy const& robust_policy,
                             TupledOut& geometry_out,
                             Strategy const& strategy)
    {
        typedef typename geometry::detail::output_geometry_value
            <
                TupledOut
            >::type single_out;

        boost::ignore_unused
            <
                detail::intersection::expect_output_pla
                    <
                        Areal1, Areal2, single_out
                    >
            >();

        typedef geometry::detail::output_geometry_access
            <
                single_out, polygon_tag, polygon_tag
            > areal;
        typedef geometry::detail::output_geometry_access
            <
                single_out, linestring_tag, linestring_tag
            > linear;
        typedef geometry::detail::output_geometry_access
            <
                single_out, point_tag, point_tag
            > pointlike;

        typedef typename geometry::tuples::element
            <
                areal::index, TupledOut
            >::type areal_out_type;
        typedef typename geometry::tuples::element
            <
                pointlike::index, TupledOut
            >::type pointlike_out_type;

        // NOTE: The same robust_policy is used in each call of
        //   intersection_insert. Is that correct?

        // A * A -> A
        call_intersection(areal1, areal2, robust_policy,
                          areal::get(geometry_out),
                          strategy);

        bool const is_areal_empty = boost::empty(areal::get(geometry_out));
        TupledOut temp_out;

        // L * L -> (L, P)
        call_intersection(geometry::detail::boundary_view<Areal1 const>(areal1),
                          geometry::detail::boundary_view<Areal2 const>(areal2),
                          robust_policy,
                          ! is_areal_empty
                            ? temp_out
                            : geometry_out,
                          strategy);

        if (! is_areal_empty)
        {
            // NOTE: the original areal geometry could be used instead of boundary here
            //   however this results in static assert failure related to rescale policy
            typedef geometry::detail::boundary_view
                <
                    areal_out_type const
                > areal_out_boundary_type;

            areal_out_boundary_type areal_out_boundary(areal::get(geometry_out));

            // L - L -> L
            call_difference(linear::get(temp_out),
                            areal_out_boundary,
                            robust_policy,
                            linear::get(geometry_out),
                            strategy);

            // P - L -> P
            call_difference(pointlike::get(temp_out),
                            areal_out_boundary,
                            robust_policy,
                            pointlike::get(geometry_out),
                            strategy.template get_point_in_geometry_strategy
                                <
                                    pointlike_out_type,
                                    areal_out_boundary_type
                                >());
        }

        return;
    }

private:
    template
    <
        typename Geometry1,
        typename Geometry2,
        typename RobustPolicy,
        typename GeometryOut,
        typename Strategy
    >
    static inline void call_intersection(Geometry1 const& geometry1,
                                         Geometry2 const& geometry2,
                                         RobustPolicy const& robust_policy,
                                         GeometryOut& geometry_out,
                                         Strategy const& strategy)
    {
        geometry::dispatch::intersection_insert
            <
                Geometry1,
                Geometry2,
                typename geometry::detail::output_geometry_value
                    <
                        GeometryOut
                    >::type,
                overlay_intersection
            >::apply(geometry1,
                     geometry2,
                     robust_policy,
                     geometry::detail::output_geometry_back_inserter(geometry_out),
                     strategy);
    }

    template
    <
        typename Geometry1,
        typename Geometry2,
        typename RobustPolicy,
        typename GeometryOut,
        typename Strategy
    >
    static inline void call_difference(Geometry1 const& geometry1,
                                       Geometry2 const& geometry2,
                                       RobustPolicy const& robust_policy,
                                       GeometryOut& geometry_out,
                                       Strategy const& strategy)
    {
        geometry::dispatch::intersection_insert
            <
                Geometry1,
                Geometry2,
                typename boost::range_value<GeometryOut>::type,
                overlay_difference
            >::apply(geometry1,
                     geometry2,
                     robust_policy,
                     geometry::range::back_inserter(geometry_out),
                     strategy);
    }
};

struct intersection_areal_areal
{
    template
    <
        typename Areal1,
        typename Areal2,
        typename RobustPolicy,
        typename GeometryOut,
        typename Strategy
    >
    static inline bool apply(Areal1 const& areal1,
                             Areal2 const& areal2,
                             RobustPolicy const& robust_policy,
                             GeometryOut& geometry_out,
                             Strategy const& strategy)
    {
        intersection_areal_areal_
            <
                GeometryOut
            >::apply(areal1, areal2, robust_policy, geometry_out, strategy);

        return true;
    }
};

}} // namespace detail::intersection
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Polygon1, typename Polygon2
>
struct intersection
    <
        Polygon1, Polygon2,
        polygon_tag, polygon_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

template
<
    typename Polygon, typename Ring
>
struct intersection
    <
        Polygon, Ring,
        polygon_tag, ring_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

template
<
    typename Ring1, typename Ring2
>
struct intersection
    <
        Ring1, Ring2,
        ring_tag, ring_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

template
<
    typename Polygon, typename MultiPolygon
>
struct intersection
    <
        Polygon, MultiPolygon,
        polygon_tag, multi_polygon_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

template
<
    typename MultiPolygon, typename Ring
>
struct intersection
    <
        MultiPolygon, Ring,
        multi_polygon_tag, ring_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

template
<
    typename MultiPolygon1, typename MultiPolygon2
>
struct intersection
    <
        MultiPolygon1, MultiPolygon2,
        multi_polygon_tag, multi_polygon_tag,
        false
    >
    : detail::intersection::intersection_areal_areal
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_INTERSECTION_AREAL_AREAL_HPP


#include <boost/python.hpp>

#include "corridor/basic_types.h"
#include "corridor/cartesian_types.h"
#include "corridor/cubic_spline/cubic_interpolation_2d.h"
#include "corridor/cubic_spline/cubic_spline_coefficients.h"
#include "corridor/cubic_spline/cubic_spline_utilities.h"
#include "corridor/frenet_types.h"

// Python API wrapper
#include "corridor_assignment_wrapper.hpp"
#include "corridor_wrapper.hpp"
#include "cubic_spline_wrapper.hpp"
#include "unscented_transformation_wrapper.hpp"

namespace py = boost::python;

BOOST_PYTHON_MODULE(PYTHON_API_MODULE_NAME) {  // NOLINT

  // ///////////////////////////////////////////////////////////////////////////
  // Cubic Spline
  // ///////////////////////////////////////////////////////////////////////////

  py::class_<CubicSplineWrapper>(
      "CubicSplineWrapper", py::init<int, const py::list&, const py::list&>())
      .def(py::init<int, const py::list&, const py::list&, const py::list&,
                    const py::list&>())
      .def("total_length", &CubicSplineWrapper::GetTotalLength)
      .def("get_polyline", &CubicSplineWrapper::getPolyline);

  // ///////////////////////////////////////////////////////////////////////////
  // Corridor Wrapper
  // ///////////////////////////////////////////////////////////////////////////
  py::class_<CorridorWrapper>("CorridorWrapper",
                              py::init<int, const py::list&, const py::list&>())
      .def(py::init<int, const py::list&, const py::list&, const py::list&,
                    const py::list&>())
      .def("get_polylines", &CorridorWrapper::GetCartesianPolylinesLines)
      .def("to_frenet_state_vector", &CorridorWrapper::ToFrenetStateVector)
      .def("to_frenet_state", &CorridorWrapper::ToFrenetState)
      .def("get_frenet_frame_dict", &CorridorWrapper::GetFrenetFrame)
      .def("length_reference_line", &CorridorWrapper::lengthReferenceLine)
      .def("curvature_at", &CorridorWrapper::curvatureAt);

  py::def("TestCorridorHandle", &TestCorridorHandle);

  // ///////////////////////////////////////////////////////////////////////////
  //  UT Wrapper
  // ///////////////////////////////////////////////////////////////////////////

  py::class_<FlatCartesianPositionAndCovMat2D>(
      "FlatCartesianPositionAndCovMat2D")
      .def_readwrite("x", &FlatCartesianPositionAndCovMat2D::x)
      .def_readwrite("y", &FlatCartesianPositionAndCovMat2D::y)
      .def_readwrite("var_x", &FlatCartesianPositionAndCovMat2D::var_x)
      .def_readwrite("var_y", &FlatCartesianPositionAndCovMat2D::var_y)
      .def_readwrite("cov_xy", &FlatCartesianPositionAndCovMat2D::cov_xy);

  py::class_<FlatPolarPositionAndCovMat2D>("FlatPolarPositionAndCovMat2D")
      .def_readwrite("r", &FlatPolarPositionAndCovMat2D::r)
      .def_readwrite("phi", &FlatPolarPositionAndCovMat2D::phi)
      .def_readwrite("var_r", &FlatPolarPositionAndCovMat2D::var_r)
      .def_readwrite("var_phi", &FlatPolarPositionAndCovMat2D::var_phi)
      .def_readwrite("cov_rphi", &FlatPolarPositionAndCovMat2D::cov_rphi);

  def("cartesian_to_polar_2d", &pyCartesianToPolarTransformation2D);
  def("polar_to_cartesian_2d", &pyPolarToCartesianTransformation2D);
  boost::python::def("ut_cartesian_to_polar_2d",
                     &UnscentedTransformationPolarCoordinate2D);

  py::class_<FlatCartesianStateAndCovMat2D>("FlatCartesianStateAndCovMat2D")
      .def_readwrite("x", &FlatCartesianStateAndCovMat2D::x)
      .def_readwrite("y", &FlatCartesianStateAndCovMat2D::y)
      .def_readwrite("vx", &FlatCartesianStateAndCovMat2D::vx)
      .def_readwrite("vy", &FlatCartesianStateAndCovMat2D::vy)
      .def_readwrite("var_x", &FlatCartesianStateAndCovMat2D::var_x)
      .def_readwrite("var_y", &FlatCartesianStateAndCovMat2D::var_y)
      .def_readwrite("var_vx", &FlatCartesianStateAndCovMat2D::var_vx)
      .def_readwrite("var_vy", &FlatCartesianStateAndCovMat2D::var_vy)
      .def_readwrite("cov_xy", &FlatCartesianStateAndCovMat2D::cov_xy)
      .def_readwrite("cov_xvx", &FlatCartesianStateAndCovMat2D::cov_xvx)
      .def_readwrite("cov_xvy", &FlatCartesianStateAndCovMat2D::cov_xvy)
      .def_readwrite("cov_yvx", &FlatCartesianStateAndCovMat2D::cov_yvx)
      .def_readwrite("cov_yvy", &FlatCartesianStateAndCovMat2D::cov_yvy)
      .def_readwrite("cov_vxvy", &FlatCartesianStateAndCovMat2D::cov_vxvy);

  py::class_<FlatFrenetStateAndCovMat2D>("FlatFrenetStateAndCovMat2D")
      .def_readwrite("l", &FlatFrenetStateAndCovMat2D::l)
      .def_readwrite("d", &FlatFrenetStateAndCovMat2D::d)
      .def_readwrite("vl", &FlatFrenetStateAndCovMat2D::vl)
      .def_readwrite("vd", &FlatFrenetStateAndCovMat2D::vd)
      .def_readwrite("var_d", &FlatFrenetStateAndCovMat2D::var_d)
      .def_readwrite("var_l", &FlatFrenetStateAndCovMat2D::var_l)
      .def_readwrite("var_vd", &FlatFrenetStateAndCovMat2D::var_vd)
      .def_readwrite("var_vl", &FlatFrenetStateAndCovMat2D::var_vl)
      .def_readwrite("cov_ld", &FlatFrenetStateAndCovMat2D::cov_ld)
      .def_readwrite("cov_lvl", &FlatFrenetStateAndCovMat2D::cov_lvl)
      .def_readwrite("cov_lvd", &FlatFrenetStateAndCovMat2D::cov_lvd)
      .def_readwrite("cov_dvl", &FlatFrenetStateAndCovMat2D::cov_dvl)
      .def_readwrite("cov_dvd", &FlatFrenetStateAndCovMat2D::cov_dvd)
      .def_readwrite("cov_vlvd", &FlatFrenetStateAndCovMat2D::cov_vlvd);

  // UnscentedStateTransformation
  boost::python::def("ut_cartesian_frenet_transformation",
                     &UnscentedStateTransformation);

  // ///////////////////////////////////////////////////////////////////////////
  // Corridor Assignment Wrapper
  // ///////////////////////////////////////////////////////////////////////////
  py::class_<FlatCorridorRelatedFeatures>("CorridorAssignmentFeature")
      .def_readwrite("l", &FlatCorridorRelatedFeatures::l)
      .def_readwrite("d", &FlatCorridorRelatedFeatures::d)
      .def_readwrite("sigma_l", &FlatCorridorRelatedFeatures::sigma_l)
      .def_readwrite("sigma_d", &FlatCorridorRelatedFeatures::sigma_d)
      .def_readwrite("obj_width_ratio",
                     &FlatCorridorRelatedFeatures::obj_width_ratio)
      .def_readwrite("obj_length_ratio",
                     &FlatCorridorRelatedFeatures::obj_length_ratio)
      .def_readwrite("corridor_width",
                     &FlatCorridorRelatedFeatures::corridor_width)
      .def_readwrite("corridor_length",
                     &FlatCorridorRelatedFeatures::corridor_length);

  //! callable functions in Python
  py::def("LateralAssignmentConfidence", &LateralAssignmentConfidence);
  py::def("LongitudinalAssignmentConfidence",
          &LongitudinalAssignmentConfidence);

  py::def("evaluateIntegralLineWidthGaussian",
          &evaluateIntegralLineWidthGaussian);

  py::def("MovingConfidence", &MovingConfidence);
  py::def("RelativeDirectionConfidence", &RelativeDirectionConfidence);
}


#include <boost/units/physical_dimensions/solid_angle.hpp>


#pragma once

#include <armadillo>
#include <memory>

class Pipeline;
class BoundaryConditions;
class InternalEnergyDiscretizer;
class MatrixEquation;
class Discretizer;

/*!
 * \brief Simple Base class to avoid having to specify template argument for
 * GoverningEquationSolver. This means that we have to use pointers to
 * GoverningEquationSolverBase instead of simple GoverningEquationSolverBase
 * instances.
 */
class GoverningEquationSolverBase
{
public:
    //! Declared to avoid the inline compiler-generated default destructor.
    virtual ~GoverningEquationSolverBase();

    //! See GoverningEquationSolver::solve().
    virtual arma::mat solve(
            const arma::uword dt,
            const Pipeline& currentState,
            const Pipeline& newState,
            const BoundaryConditions& boundaryConditions) = 0;

    //! Returns true of the equation system is over-determined given the
    //! input boundary conditions. Just returns true if there are more than
    //! three active boundary conditions, and false else.
    bool isOverDetermined(const BoundaryConditions& boundaryConditions);
};

/*!
 * \brief The GoverningEquationSolver class is a composition of Discretizer
 * and MatrixEquation with a little bit of logic, that solves the governing
 * equations for a 1d gas pipeline.
 */
template<typename T = InternalEnergyDiscretizer>
class GoverningEquationSolver : public GoverningEquationSolverBase
{
public:
    //! Declared to avoid the inline compiler-generated default destructor.
    virtual ~GoverningEquationSolver();

    /*!
     * \brief Construct with string determining the type of energy equation.
     * \param nGridPoints Number of grid points
     */
    explicit GoverningEquationSolver(const arma::uword nGridPoints);

    /*!
     * \brief Construct from Discretizer instance.
     * \param discretizer Discretizer instance
     */
    explicit GoverningEquationSolver(std::unique_ptr<Discretizer> discretizer);

    /*!
     * \brief Solve the governing equations for a given time step and boundary
     * conditions.
     * \param dt Time step [s]
     * \param currentState Current Pipeline state
     * \param newState New/guess Pipeline state
     * \param boundaryConditions Boundary conditions
     * \return Matrix containing flow, pressure and temperature columns
     */
    virtual arma::mat solve(
            const arma::uword dt,
            const Pipeline& currentState,
            const Pipeline& newState,
            const BoundaryConditions& boundaryConditions) override;

private:
    const arma::uword m_nVariables = 3; //!< Number of flow variables (flow, pressure and temperature)

    //! Discretizer used when solving the governing equations. The main
    //! difference between the two options InternalEnergyDiscretizer and
    //! EnthalpyDiscretizer is the choice of energy equation.
    //! \see InternalEnergyDiscretizer
    //! \see EnthalpyDiscretizer
    std::unique_ptr<Discretizer> m_discretizer;

    //! MatrixEquation instance used to solve the matrix equation set up when
    //! discretizing the governing equations.
    std::unique_ptr<MatrixEquation> m_matrixEquation;
};


/** @file */

#include <boost/test/unit_test.hpp>

#include <YukariMSP/MSPParsers.h>

namespace Yukari
{
namespace MSP
{
  namespace Test
  {
    BOOST_AUTO_TEST_SUITE(MSPClientTest)

    BOOST_AUTO_TEST_CASE(MSPClient_Read16)
    {
      MSPClient::Payload data = {0xc9, 0xff};
      int16_t result = MSPParsers::Read16(data.begin());
      BOOST_CHECK_EQUAL(result, -55);
    }

    BOOST_AUTO_TEST_CASE(MSPClient_Parse_IMU_Payload)
    {
      MSPClient::Payload data = {0x27, 0x0,  0x50, 0x0, 0x5, 0x2, 0xfa, 0xff, 0xfb,
                                 0xff, 0xfe, 0xff, 0x0, 0x0, 0x0, 0x0,  0x0,  0x0};
      int16_t gyro[3];
      int16_t acc[3];
      int16_t mag[3];

      bool result = MSPParsers::ParseRawIMUPayload(data, gyro, acc, mag);
      BOOST_CHECK(result);

      BOOST_CHECK_EQUAL(gyro[0], -6);
      BOOST_CHECK_EQUAL(gyro[1], -5);
      BOOST_CHECK_EQUAL(gyro[2], -2);

      BOOST_CHECK_EQUAL(acc[0], 39);
      BOOST_CHECK_EQUAL(acc[1], 80);
      BOOST_CHECK_EQUAL(acc[2], 517);

      BOOST_CHECK_EQUAL(mag[0], 0);
      BOOST_CHECK_EQUAL(mag[1], 0);
      BOOST_CHECK_EQUAL(mag[2], 0);
    }

    BOOST_AUTO_TEST_CASE(MSPClient_Parse_Attitude_Payload)
    {
      MSPClient::Payload data = {0x4d, 0x0, 0xec, 0xff, 0x76, 0x0};
      float att[3];

      bool result = MSPParsers::ParseAttitudePayload(data, att);
      BOOST_CHECK(result);

      BOOST_CHECK_EQUAL(att[0], 7.7f);
      BOOST_CHECK_EQUAL(att[1], -2.0f);
      BOOST_CHECK_EQUAL(att[2], 118.0f);
    }

    BOOST_AUTO_TEST_SUITE_END()
  }
}
}


//
// Copyright Antoine Leblanc 2010 - 2015
// Distributed under the MIT license.
//
// http://nauths.fr
// http://github.com/nicuveo
// mailto://antoine.jp.leblanc@gmail.com
//

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Includes

#include <cstdlib>
#include <iostream>
#include <boost/test/unit_test.hpp>
#include "nauths/mml/mml.hh"

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Local code

namespace
{

  typedef mml::Shape<mml::Num> Shape;
  typedef mml::Point<mml::Num> Point;
  typedef mml::Line<mml::Num> Line;

}

//HHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHHH
// Implementation

BOOST_AUTO_TEST_SUITE(line)

BOOST_AUTO_TEST_CASE(Line_0)
{
  Line l0;

  BOOST_REQUIRE(l0.p0() == Point(0, 0));
  BOOST_REQUIRE(l0.p1() == Point(0, 0));
  BOOST_REQUIRE(l0.p0() == l0.p1());
  BOOST_REQUIRE(not l0.valid());

  Line l1(10, 10, 20, 20);

  BOOST_REQUIRE(l1.p0() == Point(10, 10));
  BOOST_REQUIRE(l1.p1() == Point(20, 20));
  BOOST_REQUIRE(l1.p0() != l1.p1());
  BOOST_REQUIRE(l1.valid());

  Line l2(Point(10, 10), Point(20, 20));

  BOOST_REQUIRE(l2.p0() == Point(10, 10));
  BOOST_REQUIRE(l2.p1() == Point(20, 20));
  BOOST_REQUIRE(l2.p0() != l2.p1());
  BOOST_REQUIRE(l2.valid());
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file layer_names_test.cpp
 * @author Sreenik Seal
 *
 * Tests for testing the string representation of
 * layers in mlpack's ANN module.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/layer/layer_types.hpp>
#include <mlpack/methods/ann/layer_names.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace ann;

BOOST_AUTO_TEST_SUITE(LayerNamesTest);

/**
 * Test if the LayerNameVisitor works properly.
 */
BOOST_AUTO_TEST_CASE(LayerNameVisitorTest)
{
  LayerTypes<> atrousConvolution = new AtrousConvolution<>();
  LayerTypes<> alphaDropout = new AlphaDropout<>();
  LayerTypes<> batchNorm = new BatchNorm<>();
  LayerTypes<> constant = new Constant<>();
  LayerTypes<> convolution = new Convolution<>();
  LayerTypes<> dropConnect = new DropConnect<>();
  LayerTypes<> dropout = new Dropout<>();
  LayerTypes<> flexibleReLU = new FlexibleReLU<>();
  LayerTypes<> layerNorm = new LayerNorm<>();
  LayerTypes<> linear = new Linear<>();
  LayerTypes<> linearNoBias = new LinearNoBias<>();
  LayerTypes<> maxPooling = new MaxPooling<>();
  LayerTypes<> meanPooling = new MeanPooling<>();
  LayerTypes<> multiplyConstant = new MultiplyConstant<>();
  LayerTypes<> reLULayer = new ReLULayer<>();
  LayerTypes<> transposedConvolution = new TransposedConvolution<>();
  LayerTypes<> identityLayer = new IdentityLayer<>();
  LayerTypes<> tanHLayer = new TanHLayer<>();
  LayerTypes<> eLU = new ELU<>();
  LayerTypes<> hardTanH = new HardTanH<>();
  LayerTypes<> leakyReLU = new LeakyReLU<>();
  LayerTypes<> pReLU = new PReLU<>();
  LayerTypes<> sigmoidLayer = new SigmoidLayer<>();
  LayerTypes<> logSoftMax = new LogSoftMax<>();
  // Bilinear interpolation is not yet supported by the string converter.
  LayerTypes<> unsupportedLayer = new BilinearInterpolation<>();

  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      atrousConvolution) == "atrousconvolution");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      alphaDropout) == "alphadropout");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      batchNorm) == "batchnorm");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      constant) == "constant");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      convolution) == "convolution");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      dropConnect) == "dropconnect");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      dropout) == "dropout");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      flexibleReLU) == "flexiblerelu");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      layerNorm) == "layernorm");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      linear) == "linear");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      linearNoBias) == "linearnobias");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      maxPooling) == "maxpooling");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      meanPooling) == "meanpooling");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      multiplyConstant) == "multiplyconstant");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      reLULayer) == "relu");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      transposedConvolution) == "transposedconvolution");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      identityLayer) == "identity");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      tanHLayer) == "tanh");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      eLU) == "elu");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      hardTanH) == "hardtanh");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      leakyReLU) == "leakyrelu");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      pReLU) == "prelu");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      sigmoidLayer) == "sigmoid");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      logSoftMax) == "logsoftmax");
  BOOST_REQUIRE(boost::apply_visitor(LayerNameVisitor(),
                      unsupportedLayer) == "unsupported");
  // Delete all instances.
  boost::apply_visitor(DeleteVisitor(), atrousConvolution);
  boost::apply_visitor(DeleteVisitor(), alphaDropout);
  boost::apply_visitor(DeleteVisitor(), batchNorm);
  boost::apply_visitor(DeleteVisitor(), constant);
  boost::apply_visitor(DeleteVisitor(), convolution);
  boost::apply_visitor(DeleteVisitor(), dropConnect);
  boost::apply_visitor(DeleteVisitor(), dropout);
  boost::apply_visitor(DeleteVisitor(), flexibleReLU);
  boost::apply_visitor(DeleteVisitor(), layerNorm);
  boost::apply_visitor(DeleteVisitor(), linear);
  boost::apply_visitor(DeleteVisitor(), linearNoBias);
  boost::apply_visitor(DeleteVisitor(), maxPooling);
  boost::apply_visitor(DeleteVisitor(), meanPooling);
  boost::apply_visitor(DeleteVisitor(), multiplyConstant);
  boost::apply_visitor(DeleteVisitor(), reLULayer);
  boost::apply_visitor(DeleteVisitor(), transposedConvolution);
  boost::apply_visitor(DeleteVisitor(), identityLayer);
  boost::apply_visitor(DeleteVisitor(), tanHLayer);
  boost::apply_visitor(DeleteVisitor(), eLU);
  boost::apply_visitor(DeleteVisitor(), hardTanH);
  boost::apply_visitor(DeleteVisitor(), leakyReLU);
  boost::apply_visitor(DeleteVisitor(), pReLU);
  boost::apply_visitor(DeleteVisitor(), sigmoidLayer);
  boost::apply_visitor(DeleteVisitor(), logSoftMax);
  boost::apply_visitor(DeleteVisitor(), unsupportedLayer);
}

BOOST_AUTO_TEST_SUITE_END();


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/kinematics/skeleton/skeleton_types.h>
#include <OpenTissue/kinematics/inverse/inverse_chain.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

typedef float																             real_type;
typedef OpenTissue::math::default_math_types		  			 math_types;

/**
* This is a dummy bone traits class implementation created just for the unit-test compile.
*/
class MyBoneTraits : public OpenTissue::skeleton::DefaultBoneTraits<math_types>
{
public:

};

typedef OpenTissue::skeleton::Types<math_types,MyBoneTraits>      skeleton_types;
typedef skeleton_types::skeleton_type                             skeleton_type;
typedef skeleton_types::bone_type                                 bone_type;
typedef skeleton_types::bone_traits                               bone_traits;
typedef OpenTissue::kinematics::inverse::Chain< skeleton_type >   chain_type;

BOOST_AUTO_TEST_SUITE(opentissue_kinematics_inverse_chain);

BOOST_AUTO_TEST_CASE(test_cases)
{
  skeleton_type m_skeleton;

  bone_type * b0 = m_skeleton.create_bone();
  bone_type * b1 = m_skeleton.create_bone(b0);
  bone_type * b2 = m_skeleton.create_bone(b1);
  bone_type * b4 = m_skeleton.create_bone(b0);

  // Test if chain methods return meaningfull data if invoked on an empty chain (ie. a non-initialzied chain)
  {
    chain_type chain;
    bone_type * root         = chain.get_root();
    bone_type * end_effector = chain.get_end_effector();
    BOOST_CHECK( root == 0 );
    BOOST_CHECK( end_effector == 0 );

    chain_type::bone_iterator         bone      = chain.bone_begin();
    chain_type::bone_iterator         bone_end  = chain.bone_end();
    BOOST_CHECK( bone==bone_end );
    chain_type::reverse_bone_iterator rbone     = chain.rbone_begin();
    chain_type::reverse_bone_iterator rbone_end = chain.rbone_end();
    BOOST_CHECK( rbone==rbone_end );
  }

  // Test chain initialization with incorrect input
  {
    chain_type chain;
    bone_type * null = 0;
    BOOST_CHECK_THROW( chain.init( b0, null ), std::invalid_argument );
  }
  {
    chain_type chain;
    bone_type * null = 0;
    BOOST_CHECK_THROW( chain.init( null, b2 ), std::invalid_argument );
  }
  {
    chain_type chain;
    bone_type * null = 0;
    BOOST_CHECK_THROW( chain.init( null, null ), std::invalid_argument );
  }
  {
    chain_type chain;
    BOOST_CHECK_THROW( chain.init( b4, b2 ), std::logic_error );
  }
  {
    chain_type chain;
    BOOST_CHECK_THROW( chain.init( b2, b4 ), std::logic_error );
  }
  {
    chain_type chain;
    BOOST_CHECK_THROW( chain.init( b2, b0 ), std::logic_error );
  }

  // Initialize chain with one bone
  {
    chain_type chain;
    BOOST_CHECK_NO_THROW( chain.init( b0, b0 ) );
    chain_type::bone_iterator         bone      = chain.bone_begin();
    chain_type::bone_iterator         bone_end  = chain.bone_end();
    BOOST_CHECK( &(*bone) == b0 );
    ++bone;
    BOOST_CHECK( bone == bone_end );
    chain_type::reverse_bone_iterator rbone     = chain.rbone_begin();
    chain_type::reverse_bone_iterator rbone_end = chain.rbone_end();
    BOOST_CHECK( &(*rbone) == b0 );
    ++rbone;
    BOOST_CHECK( rbone== rbone_end );

    size_t param_size = chain.get_goal_dimension();

    BOOST_CHECK( param_size == 3 );
  }

  //Initialize chain with two bones
  {
    chain_type chain;
    BOOST_CHECK_NO_THROW( chain.init( b0, b1 ) );

    chain_type::bone_iterator         bone      = chain.bone_begin();
    chain_type::bone_iterator         bone_end  = chain.bone_end();
    BOOST_CHECK( &(*bone) == b0 );
    ++bone;
    BOOST_CHECK( &(*bone) == b1 );
    ++bone;
    BOOST_CHECK( bone == bone_end );

    chain_type::reverse_bone_iterator rbone     = chain.rbone_begin();
    chain_type::reverse_bone_iterator rbone_end = chain.rbone_end();
    BOOST_CHECK( &(*rbone) == b1 );
    ++rbone;
    BOOST_CHECK( &(*rbone) == b0 );
    ++rbone;
    BOOST_CHECK( rbone == rbone_end  );
  }

  // Initialize chain with three bones
  {
    chain_type chain;
    BOOST_CHECK_NO_THROW( chain.init( b0, b2 ) );

    chain_type::bone_iterator         bone      = chain.bone_begin();
    chain_type::bone_iterator         bone_end  = chain.bone_end();
    BOOST_CHECK( &(*bone) == b0 );
    ++bone;
    BOOST_CHECK( &(*bone) == b1 );
    ++bone;
    BOOST_CHECK( &(*bone) == b2 );
    ++bone;
    BOOST_CHECK( bone == bone_end );

    chain_type::reverse_bone_iterator rbone     = chain.rbone_begin();
    chain_type::reverse_bone_iterator rbone_end = chain.rbone_end();
    BOOST_CHECK( &(*rbone) == b2 );
    ++rbone;
    BOOST_CHECK( &(*rbone) == b1 );
    ++rbone;
    BOOST_CHECK( &(*rbone) == b0 );
    ++rbone;
    BOOST_CHECK( rbone == rbone_end );

    bone_type * root         = chain.get_root();
    bone_type * end_effector = chain.get_end_effector();
    BOOST_CHECK( root == b0 );
    BOOST_CHECK( end_effector == b2 );

  }

  // Do outboard and inboard bone methods testing
  {
    chain_type chain;
    BOOST_CHECK_NO_THROW( chain.init( b0, b2 ) );

    chain_type::bone_iterator         bone0   = chain.bone_begin();
    chain_type::bone_iterator         bone1 = bone0; ++bone1;
    chain_type::bone_iterator         bone2 = bone1; ++bone2;

    chain_type::reverse_bone_iterator         rbone0  = chain.rbone_begin();
    chain_type::reverse_bone_iterator         rbone1  = rbone0; ++rbone1;
    chain_type::reverse_bone_iterator         rbone2  = rbone1; ++rbone2;

  }
}

BOOST_AUTO_TEST_SUITE_END();


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE JPetRecoImageToolsTests
#include <boost/test/unit_test.hpp>
#include <fstream>
#include <iostream>
#include <utility>
#include <vector>

#include "JPetCommonTools/JPetCommonTools.h"
#include "./JPetFilterCosine.h"
#include "./JPetFilterHamming.h"
#include "./JPetFilterNone.h"
#include "./JPetFilterRamLak.h"
#include "./JPetFilterRidgelet.h"
#include "./JPetFilterSheppLogan.h"
#include "./JPetRecoImageTools.h"

BOOST_AUTO_TEST_SUITE(FirstSuite)

BOOST_AUTO_TEST_CASE(rescale_0)
{
  JPetRecoImageTools::Matrix2DProj matrix = {{3, 0}, {0, 0}};
  double resFact = 5;
  double minCutoff = 0;
  double epsilon = 0.00001;
  JPetRecoImageTools::rescale(matrix, minCutoff, resFact);
  BOOST_REQUIRE_CLOSE(matrix[0][0], 5, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[0][1], 0, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][0], 0, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][1], 0, epsilon);
}

BOOST_AUTO_TEST_CASE(rescale)
{
  JPetRecoImageTools::Matrix2DProj matrix = {{2, 1}, {1, 1}};
  double resFact = 2;
  double minCutoff = 0;
  double epsilon = 0.00001;
  JPetRecoImageTools::rescale(matrix, minCutoff, resFact);
  BOOST_REQUIRE_CLOSE(matrix[0][0], 2, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[0][1], 0, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][0], 0, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][1], 0, epsilon);
}

BOOST_AUTO_TEST_CASE(rescale_2)
{
  JPetRecoImageTools::Matrix2DProj matrix = {{5, 2}, {1, 2}};
  double resFact = 2;
  double minCutoff = 0;
  double epsilon = 0.00001;
  JPetRecoImageTools::rescale(matrix, minCutoff, resFact);
  BOOST_REQUIRE_CLOSE(matrix[0][0], 2, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[0][1], 0.5, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][0], 0., epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][1], 0.5, epsilon);
}

BOOST_AUTO_TEST_CASE(rescale_3)
{
  JPetRecoImageTools::Matrix2DProj matrix = {{5, 3}, {1, 2}};
  double resFact = 8;
  double minCutoff = 2; /// -> 5, 3 , 2, 2
  /// after background subtraction 3, 1, 0,0
  double epsilon = 0.00001;
  JPetRecoImageTools::rescale(matrix, minCutoff, resFact);
  BOOST_REQUIRE_CLOSE(matrix[0][0], 8, epsilon);
  BOOST_REQUIRE_CLOSE(matrix[0][1], 1. * 8. / 3., epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][0], 0 * 8. / 3., epsilon);
  BOOST_REQUIRE_CLOSE(matrix[1][1], 0, epsilon);
}
BOOST_AUTO_TEST_CASE(matrixGetterFactoryTest)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2}, {3, 4}};
  auto getter = JPetRecoImageTools::matrixGetterFactory(matrix, false);
  BOOST_REQUIRE_CLOSE(getter(0, 0), 1, epsilon);
  BOOST_REQUIRE_CLOSE(getter(0, 1), 2, epsilon);
  BOOST_REQUIRE_CLOSE(getter(1, 0), 3, epsilon);
  BOOST_REQUIRE_CLOSE(getter(1, 1), 4, epsilon);
  BOOST_REQUIRE_CLOSE(getter(3, 3), 0,
                      epsilon); /// out of range should return 0
  auto getterT =
    JPetRecoImageTools::matrixGetterFactory(matrix, true); /// transposed
  BOOST_REQUIRE_CLOSE(getterT(0, 0), 1, epsilon);
  BOOST_REQUIRE_CLOSE(getterT(0, 1), 3, epsilon);
  BOOST_REQUIRE_CLOSE(getterT(1, 0), 2, epsilon);
  BOOST_REQUIRE_CLOSE(getterT(1, 1), 4, epsilon);
  BOOST_REQUIRE_CLOSE(getterT(3, 3), 0,
                      epsilon); /// out of range should return 0
}

BOOST_AUTO_TEST_CASE(nearestNeighbour)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2}, {3, 4}};
  auto getter = JPetRecoImageTools::matrixGetterFactory(matrix, false);
  /// first argument is i second j  (i,j)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0, getter), 1,
                      epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0, getter), 3,
                      epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0.6, getter), 2,
                      epsilon); // (0,1)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0.4, getter), 1,
                      epsilon); // (0,0)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0.4, getter), 3,
                      epsilon); // (0,0)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0.8, getter), 4,
                      epsilon); // (0,0)
  auto getterT =
    JPetRecoImageTools::matrixGetterFactory(matrix, true); /// transposed
  /// first argument is j second i  (j,i)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0, getterT), 1,
                      epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0, getterT), 2,
                      epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0.6, getterT), 3,
                      epsilon); // (1,0)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(0, 0.4, getterT), 1,
                      epsilon); // (0,0)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0.4, getterT), 2,
                      epsilon); // (0,1)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::nearestNeighbour(1, 0.8, getterT), 4,
                      epsilon); // (1,1)
}

BOOST_AUTO_TEST_CASE(linear)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2}, {3, 4}};
  auto getter = JPetRecoImageTools::matrixGetterFactory(matrix, false);
  /// first argument is i second j  (i,j)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 0, getter), 1, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 1, getter), 2, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 0.9, getter), 1.9, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(1, 0, getter), 3, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 0.4, getter), 1.4,
                      epsilon); // 1 * 0.6 + 2 * 0.4
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(1, 0.4, getter), 3.4,
                      epsilon); //  3* 0.6 + 4 * 0.4
  auto getterT =
    JPetRecoImageTools::matrixGetterFactory(matrix, true); /// transposed
  /// first argument is j second i  (j,i)
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 0, getterT), 1, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(1, 0, getterT), 2, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::linear(0, 0.6, getterT), 2.2,
                      epsilon); // 1*0.4 +  3  *0.6
}
/*
BOOST_AUTO_TEST_CASE(calculateProjection)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{4}};
  //static double calculateProjection(const Matrix2D& emissionMatrix,
  //double phi,
  //int scanNumber,
  //int nScans,
  //InterpolationFunc& interpolationFunction
  //);
  /// first argument is i second j  (i,j)
  JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,0,0,1,
func), 4, epsilon);
}

BOOST_AUTO_TEST_CASE(calculateProjection2)
{
double epsilon = 0.00001;
JPetRecoImageTools::Matrix2D matrix = {{1, 2}, {3, 4}};
/// 1 2
/// 3 4
/// angle = 0
/// nScans = 2
JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
const int kNScans = 2;
auto angle = 0.0;
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,0,kNScans,
func), 6, epsilon); // 2 + 4
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,1,kNScans,
func), 4, epsilon); // 1 + 3
//BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,45,0,kNScans,
func), 5, epsilon); // 3 + 2
//BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,45,1,kNScans,
func), 4, epsilon); // 4
//BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,45,-1,kNScans,
func), 1, epsilon); // 1
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,0,kNScans,
func), 7, epsilon); // 3 + 4
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,1,kNScans,
func), 3, epsilon); // 1 + 2
//BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,125,0,kNScans,
func), 5, epsilon); // 1 + 4
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,0,kNScans,
func), 6, epsilon); // 2 + 4
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,1,kNScans,
func), 4, epsilon); // 1 + 3
}

BOOST_AUTO_TEST_CASE(calculateProjection3)
{
double epsilon = 0.00001;
JPetRecoImageTools::Matrix2D matrix = {{1, 2, 3}, {4, 5, 6}, {7,8,9}};
/// 1 2 3
/// 4 5 6
/// 7 8 9
/// angle = 0
/// nScans = 3
JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
const int kNScans = 3;
auto angle = 0.0;
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,0,kNScans,
func), 12, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,1,kNScans,
func), 15, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,2,kNScans,
func), 18, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,45,0,kNScans,
func), 15, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,0,kNScans,
func), 6, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,1,kNScans,
func), 15, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,2,kNScans,
func), 24, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,125,0,kNScans,
func), 15, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,0,kNScans,
func), 12, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,1,kNScans,
func), 15, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,2,kNScans,
func), 18, epsilon);
}

BOOST_AUTO_TEST_CASE(calculateProjection4)
{
double epsilon = 0.00001;
JPetRecoImageTools::Matrix2D matrix = {{1, 2, 3, 4}, {5, 6, 7, 8}, {9, 10, 11,
12}, {13, 14, 15, 16}};
/// 1  2  3  4
/// 5  6  7  8
/// 9  10 11 12
/// 13 14 15 16
/// angle = 0
/// nScans = 4
JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
const int kNScans = 4;
auto angle = 0.0;
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,0,kNScans,
func), 28, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,1,kNScans,
func), 32, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,2,kNScans,
func), 36, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,angle,3,kNScans,
func), 40, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,45,0,kNScans,
func), 34, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,0,kNScans,
func), 10, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,1,kNScans,
func), 26, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,2,kNScans,
func), 42, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,90,3,kNScans,
func), 58, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,125,0,kNScans,
func), 34, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,0,kNScans,
func), 28, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,1,kNScans,
func), 32, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,2,kNScans,
func), 36, epsilon);
BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection(matrix,180,3,kNScans,
func), 40, epsilon);
}

BOOST_AUTO_TEST_CASE(sinogram1)
{
double epsilon = 0.00001;
JPetRecoImageTools::Matrix2D matrix = {{1, 2, 3}, {4, 5, 6}, {7,8,9}};
/// 1 2 3
/// 4 5 6
/// 7 8 9
JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
JPetRecoImageTools::RescaleFunc func2= JPetRecoImageTools::nonRescale;
const int kNScans = 3;
const int kNViews = 2;
auto results = JPetRecoImageTools::sinogram(matrix,kNViews,kNScans, 0 , 180,
func, func2);
for (auto i: results)
for (auto j: i)
std::cout <<j <<std::endl;
}

//BOOST_AUTO_TEST_CASE(sinogram2)
//{
//double epsilon = 0.00001;
//JPetRecoImageTools::Matrix2D matrix = {{0, 0, 0}, {0, 10, 0}, {0,0,0}};
//JPetRecoImageTools::InterpolationFunc func =
JPetRecoImageTools::nearestNeighbour;
//JPetRecoImageTools::RescaleFunc func2= JPetRecoImageTools::nonRescale;
//const int kNScans = 3;
//const int kNViews = 4;
//auto results = JPetRecoImageTools::sinogram(matrix,kNViews,kNScans, 0 , 180,
func, func2);
//for (auto i: results)
//for (auto j: i)
//std::cout <<j <<std::endl;
//}
*/
BOOST_AUTO_TEST_CASE(projection1)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2, 3}, {4, 5, 6}, {7, 8, 9}};
  /// 1 2 3
  /// 4 5 6
  /// 7 8 9

  int imageSize = matrix.size();
  double center = (float)(imageSize - 1) / 2.0;
  double length = center * center;
  std::function< double(int, int) > matrixGet;
  matrixGet = JPetRecoImageTools::matrixGetterFactory(
                matrix, false); // The matrix  elements will be taken as (x,y).
  double step = (M_PI / 180);

  double cos = std::cos(step * 0); // 0 degree
  double sin = std::sin(step * 0); // 0 degree
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      12, epsilon); // 1 + 4 + 7
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      15, epsilon); // 2 + 5 + 8
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      18, epsilon); // 3 + 6 + 9

  cos = std::cos(step * 90); // 90 degree
  sin = std::sin(step * 90); // 90 degree

  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      24, epsilon); // 9 + 8 + 7
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      15, epsilon); // 6 + 5 + 3
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      6, epsilon); // 3 + 2 + 1
}

BOOST_AUTO_TEST_CASE(projection2)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2}, {3, 4}};
  /// 1 2
  /// 3 4

  int imageSize = matrix.size();
  double center = (float)(imageSize - 1) / 2.0;
  double length = center * center;
  std::function< double(int, int) > matrixGet;
  matrixGet = JPetRecoImageTools::matrixGetterFactory(
                matrix, false); // The matrix  elements will be taken as (x,y).
  double step = (M_PI / 180);

  double cos = std::cos(step * 0); // 0 degree
  double sin = std::sin(step * 0); // 0 degree
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      4, epsilon); // 1 + 3
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      6, epsilon); // 2 + 4

  cos = std::cos(step * 90); // 90 degree
  sin = std::sin(step * 90); // 90 degree

  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      7, epsilon); // 3 + 4
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      3, epsilon); // 1 + 2
}

BOOST_AUTO_TEST_CASE(projection3)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {{1, 2, 3, 4, 5},
    {6, 7, 8, 9, 10},
    {11, 12, 13, 14, 15},
    {16, 17, 18, 19, 20},
    {21, 22, 23, 24, 25}
  };
  /// 1  2  3  4  5
  /// 6  7  8  9  10
  /// 11 12 13 14 15
  /// 16 17 18 19 20
  /// 21 22 23 24 25
  /// angle = 0
  /// nScans = 4
  int imageSize = matrix.size();
  double center = (float)(imageSize - 1) / 2.0;
  double length = center * center;
  std::function< double(int, int) > matrixGet;
  matrixGet = JPetRecoImageTools::matrixGetterFactory(
                matrix, false); // The matrix  elements will be taken as (x,y).
  double step = (M_PI / 180);

  double cos = std::cos(step * 0); // 0 degree
  double sin = std::sin(step * 0); // 0 degree
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      55, epsilon); // 21 + 16 + 11 + 6 + 1
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      60, epsilon); // 22 + 17 + 12 + 7 + 2
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      65, epsilon); // 23 + 18 + 13 + 8 + 3
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        3, cos, sin, imageSize, center, length, matrixGet),
                      70, epsilon); // 24 + 19 + 14 + 9 + 4
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        4, cos, sin, imageSize, center, length, matrixGet),
                      75, epsilon); // 25 + 20 + 15 + 10 + 5

  // cos = std::cos(step * 45); //45 degree
  // sin = std::sin(step * 45); //45 degree
  //
  // BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(0,
  //               cos, sin, imageSize, center, length, matrixGet), 70,
  //               epsilon); // 24 + 19 + 14 + 9 + 4

  cos = std::cos(step * 90); // 90 degree
  sin = std::sin(step * 90); // 90 degree

  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      115, epsilon); // 25 + 24 + 23 + 22 + 21
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      90, epsilon); // 20 + 19 + 18 + 17 + 16
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      65, epsilon); // 15 + 14 + 13 + 12 + 11
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        3, cos, sin, imageSize, center, length, matrixGet),
                      40, epsilon); // 10 + 9 + 8 + 7 + 6
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        4, cos, sin, imageSize, center, length, matrixGet),
                      15, epsilon); // 5 + 4 + 3 + 2 + 1
}

BOOST_AUTO_TEST_CASE(projection4)
{
  double epsilon = 0.00001;
  JPetRecoImageTools::Matrix2D matrix = {
    {1, 10, 100, 1000, 10000, 100000, 1000000},
    {2, 20, 200, 2000, 20000, 200000, 2000000},
    {3, 30, 300, 3000, 30000, 300000, 3000000},
    {4, 40, 400, 4000, 40000, 400000, 4000000},
    {5, 50, 500, 5000, 50000, 500000, 5000000},
    {6, 60, 600, 6000, 60000, 600000, 6000000},
    {7, 70, 700, 7000, 70000, 700000, 7000000}
  };
  int imageSize = matrix.size();
  double center = (float)(imageSize - 1) / 2.0;
  double length = center * center;
  std::function< double(int, int) > matrixGet;
  matrixGet = JPetRecoImageTools::matrixGetterFactory(
                matrix, false); // The matrix  elements will be taken as (x,y).
  double step = (M_PI / 180);

  double cos = std::cos(step * 0); // 0 degree
  double sin = std::sin(step * 0); // 0 degree
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      28, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      280, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      2800, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        3, cos, sin, imageSize, center, length, matrixGet),
                      28000, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        4, cos, sin, imageSize, center, length, matrixGet),
                      280000, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        5, cos, sin, imageSize, center, length, matrixGet),
                      2800000, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        6, cos, sin, imageSize, center, length, matrixGet),
                      28000000, epsilon);

  cos = std::cos(step * 90); // 90 degree
  sin = std::sin(step * 90); // 90 degree

  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        0, cos, sin, imageSize, center, length, matrixGet),
                      7777777, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        1, cos, sin, imageSize, center, length, matrixGet),
                      6666666, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        2, cos, sin, imageSize, center, length, matrixGet),
                      5555555, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        3, cos, sin, imageSize, center, length, matrixGet),
                      4444444, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        4, cos, sin, imageSize, center, length, matrixGet),
                      3333333, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        5, cos, sin, imageSize, center, length, matrixGet),
                      2222222, epsilon);
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::calculateProjection2(
                        6, cos, sin, imageSize, center, length, matrixGet),
                      1111111, epsilon);
}

// This test takes a Shepp-Logan phantom and creates a sinogram.
BOOST_AUTO_TEST_CASE(sinogram)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "sinogram.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  int views = 180;
  int scans = 256;
  std::vector< std::vector< double > > result =
    JPetRecoImageTools::createSinogramWithSingleInterpolation(
      m, views, scans, 0, 180, JPetRecoImageTools::linear,
      JPetRecoImageTools::rescale);
  /// save sinogram
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

// This test takes a Shepp-Logan phantom and creates a sinogram.
BOOST_AUTO_TEST_CASE(sinogram2)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "sinogram2.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  std::vector< std::vector< double > > result =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  /// save sinogram
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProject)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backproject.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  int views = 180;
  int scans = 256;
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithSingleInterpolation(
      m, views, scans, 0, 180, JPetRecoImageTools::linear,
      JPetRecoImageTools::rescale);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        sinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectNone)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectNone.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterNone noneFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, noneFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectRamLak)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectRamLak.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterRamLak ramlakFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ramlakFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSheppLogan)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectSheppLogan.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterSheppLogan shepploganFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, shepploganFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectCosine)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectCosine.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterCosine cosineFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, cosineFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectHamming)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectHamming.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterHamming hammingFilter(0.45);
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, hammingFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectRidgelet)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectRidgelet.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterRidgelet ridgeletFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ridgeletFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectNoneSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectNoneSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterNone noneFilter;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, noneFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectRamLakSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectRamLakSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterRamLak ramlak;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ramlak, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSheppLoganSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectSheppLoganSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterSheppLogan shepplogan;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, shepplogan, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectCosineSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectCosineSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterCosine cosine;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, cosine, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectHammingSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectHammingSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterHamming hamming;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, hamming, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectRidgeletSlow)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/phantom.pgm";
  const auto outFile = "backprojectRidgeletSlow.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector< std::vector< int > > m(width, std::vector< int >(height));
  for (unsigned int i = 0; i < width; i++) {
    for (unsigned int j = 0; j < height; j++) {
      in >> val;
      m[i][j] = val;
    }
  }
  JPetRecoImageTools::Matrix2DProj sinogram =
    JPetRecoImageTools::createSinogramWithDoubleInterpolation(
      m, 180, JPetRecoImageTools::rescale, 0, 255);
  JPetFilterRidgelet ridgelet;
  JPetRecoImageTools::FourierTransformFunction f =
    JPetRecoImageTools::doFFTSLOW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ridgelet, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 180, JPetRecoImageTools::rescale, 0, 255);
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << "255" << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast< int >(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramNone)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/sinogramBackproject.ppm";
  const auto outFile = "backprojectSinogramNone.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector<std::vector<double>> sinogram(height, std::vector<double>(width));
  for (unsigned int i = 0; i < height; i++) {
    for (unsigned int j = 0; j < width; j++) {
      in >> val;
      sinogram[i][j] = val;
    }
  }
  JPetFilterNone noneFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, noneFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 360, JPetRecoImageTools::nonRescale, 0, 255);
  int maxValue = 0;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      if (static_cast<int>(result[i][j]) > maxValue)
        maxValue = static_cast<int>(result[i][j]);
    }
  }
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << maxValue << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast<int>(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(backProjectSinogramRamLak)
{
  const auto inFile = "unitTestData/JPetRecoImageToolsTest/sinogramBackproject.ppm";
  const auto outFile = "backprojectSinogramRamLak.ppm";
  /// read phantom
  std::ifstream in(inFile);
  BOOST_REQUIRE(in);
  std::string line;
  getline(in, line);
  unsigned int width;
  unsigned int height;
  in >> width;
  in >> height;
  int val;
  in >> val; // skip max val
  std::vector<std::vector<double>> sinogram(height, std::vector<double>(width));
  for (unsigned int i = 0; i < height; i++) {
    for (unsigned int j = 0; j < width; j++) {
      in >> val;
      sinogram[i][j] = val;
    }
  }
  JPetFilterRamLak ramLakFilter;
  JPetRecoImageTools::FourierTransformFunction f = JPetRecoImageTools::doFFTW;
  JPetRecoImageTools::Matrix2DProj filteredSinogram =
    JPetRecoImageTools::FilterSinogram(f, ramLakFilter, sinogram);
  JPetRecoImageTools::Matrix2DProj result = JPetRecoImageTools::backProject(
        filteredSinogram, 360, JPetRecoImageTools::nonRescale, 0, 255);
  int maxValue = 0;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      if (static_cast<int>(result[i][j]) > maxValue)
        maxValue = static_cast<int>(result[i][j]);
    }
  }
  std::ofstream res(outFile);
  res << "P2" << std::endl;
  res << result[0].size() << " " << result.size() << std::endl;
  res << maxValue << std::endl;
  for (unsigned int i = 0; i < result.size(); i++) {
    for (unsigned int j = 0; j < result[0].size(); j++) {
      res << static_cast<int>(result[i][j]) << " ";
    }
    res << std::endl;
  }
  res.close();
  BOOST_REQUIRE(JPetCommonTools::ifFileExisting(outFile));
}

BOOST_AUTO_TEST_CASE(test_normal_distribution_popability)
{
  float x = 0.;
  float mean = 0.;
  float stddev = 1.;

  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::normalDistributionProbability(x, mean, stddev), 0.3989422804014327, 0.00001);
  x = stddev;
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::normalDistributionProbability(x, mean, stddev), 0.24197072451914337, 0.00001);
  x = -stddev;
  BOOST_REQUIRE_CLOSE(JPetRecoImageTools::normalDistributionProbability(x, mean, stddev), 0.24197072451914337, 0.00001);
}

BOOST_AUTO_TEST_SUITE_END()