//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_multibody_parsers_urdf_model_hxx__
#define __pinocchio_multibody_parsers_urdf_model_hxx__

#include "pinocchio/math/matrix.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/multibody/model.hpp"

#include <sstream>
#include <boost/foreach.hpp>
#include <limits>

namespace pinocchio
{
  namespace urdf
  {
    namespace details
    {
      typedef double urdf_value_type;

      template<typename _Scalar, int Options>
      class UrdfVisitorBaseTpl {
        public:
          enum JointType {
            REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR
          };

          typedef _Scalar Scalar;
          typedef SE3Tpl<Scalar,Options> SE3;
          typedef InertiaTpl<Scalar,Options> Inertia;

          typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
          typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;
          typedef Eigen::Ref<Vector> VectorRef;
          typedef Eigen::Ref<const Vector> VectorConstRef;

          virtual void setName (const std::string& name) = 0;

          virtual void addRootJoint (const Inertia& Y, const std::string & body_name) = 0;

          virtual void addJointAndBody(
              JointType type,
              const Vector3& axis,
              const FrameIndex & parentFrameId,
              const SE3 & placement,
              const std::string & joint_name,
              const Inertia& Y,
              const std::string & body_name,
              const VectorConstRef& max_effort,
              const VectorConstRef& max_velocity,
              const VectorConstRef& min_config,
              const VectorConstRef& max_config) = 0;

          virtual void addFixedJointAndBody(
              const FrameIndex & parentFrameId,
              const SE3 & joint_placement,
              const std::string & joint_name,
              const Inertia& Y,
              const std::string & body_name) = 0;

          virtual void appendBodyToJoint(
              const FrameIndex fid,
              const Inertia& Y,
              const SE3 & placement,
              const std::string & body_name) = 0;

          virtual FrameIndex getBodyId (
              const std::string& frame_name) const = 0;

          UrdfVisitorBaseTpl () : log (NULL) {}

          template <typename T>
          UrdfVisitorBaseTpl& operator<< (const T& t)
          {
            if (log != NULL) *log << t;
            return *this;
          }

          std::ostream* log;
      };

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      class UrdfVisitor : public UrdfVisitorBaseTpl<Scalar, Options>
      {
        public:
          typedef UrdfVisitorBaseTpl<Scalar, Options> Base;
          typedef typename Base::JointType      JointType;
          typedef typename Base::Vector3        Vector3;
          typedef typename Base::VectorConstRef VectorConstRef;
          typedef typename Base::SE3            SE3;
          typedef typename Base::Inertia        Inertia;

          typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
          typedef typename Model::JointCollection JointCollection;
          typedef typename Model::JointModel JointModel;
          typedef typename Model::Frame Frame;

          Model& model;

          UrdfVisitor (Model& model) : model(model) {}

          void setName (const std::string& name)
          {
            model.name = name;
          }

          virtual void addRootJoint(const Inertia& Y, const std::string & body_name)
          {
            addFixedJointAndBody(0, SE3::Identity(), "root_joint", Y, body_name);
//            appendBodyToJoint(0,Y,SE3::Identity(),body_name); TODO: change for the correct behavior, see https://github.com/stack-of-tasks/pinocchio/pull/1102 for discussions on the topic

          }

          void addJointAndBody(
              JointType type,
              const Vector3& axis,
              const FrameIndex & parentFrameId,
              const SE3 & placement,
              const std::string & joint_name,
              const Inertia& Y,
              const std::string & body_name,
              const VectorConstRef& max_effort,
              const VectorConstRef& max_velocity,
              const VectorConstRef& min_config,
              const VectorConstRef& max_config)
          {
            JointIndex idx;
            const Frame & frame = model.frames[parentFrameId];

            switch (type) {
              case Base::FLOATING:
                idx = model.addJoint(frame.parent,
                    typename JointCollection::JointModelFreeFlyer(),
                    frame.placement * placement,
                    joint_name,
                    max_effort,max_velocity,min_config,max_config
                    );
                break;
              case Base::REVOLUTE:
                idx = addJoint<
                  typename JointCollection::JointModelRX,
                  typename JointCollection::JointModelRY,
                  typename JointCollection::JointModelRZ,
                  typename JointCollection::JointModelRevoluteUnaligned> (
                      axis, frame, placement, joint_name,
                      max_effort, max_velocity, min_config, max_config);
                break;
              case Base::CONTINUOUS:
                idx = addJoint<
                  typename JointCollection::JointModelRUBX,
                  typename JointCollection::JointModelRUBY,
                  typename JointCollection::JointModelRUBZ,
                  typename JointCollection::JointModelRevoluteUnboundedUnaligned> (
                      axis, frame, placement, joint_name,
                      max_effort, max_velocity, min_config, max_config);
                break;
              case Base::PRISMATIC:
                idx = addJoint<
                  typename JointCollection::JointModelPX,
                  typename JointCollection::JointModelPY,
                  typename JointCollection::JointModelPZ,
                  typename JointCollection::JointModelPrismaticUnaligned> (
                      axis, frame, placement, joint_name,
                      max_effort, max_velocity, min_config, max_config);
                break;
              case Base::PLANAR:
                idx = model.addJoint(frame.parent,
                    typename JointCollection::JointModelPlanar(),
                    frame.placement * placement,
                    joint_name,
                    max_effort,max_velocity,min_config,max_config
                    );
                break;
              default:
                PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The joint type is not correct.");
            };
            int res (model.addJointFrame(idx, (int)parentFrameId));
            if (res == -1) {
              std::ostringstream oss;
              oss << joint_name << " already inserted as a frame. Current frames "
                "are [";
              for (typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
                  model.frames.begin (); it != model.frames.end (); ++it) {
                oss << '"' << it->name << "\",";
              }
              oss << ']';
              throw std::invalid_argument(oss.str());
            }

            FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
            appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
          }

          void addFixedJointAndBody(
              const FrameIndex & parentFrameId,
              const SE3 & joint_placement,
              const std::string & joint_name,
              const Inertia& Y,
              const std::string & body_name)
          {
            const Frame & frame = model.frames[parentFrameId];

            int fid = model.addFrame(Frame(joint_name, frame.parent, parentFrameId,
                  frame.placement * joint_placement, FIXED_JOINT)
                );
            if (fid < 0)
              throw std::invalid_argument("Fixed joint " + joint_name + " could not be added.");

            appendBodyToJoint((FrameIndex)fid, Y, SE3::Identity(), body_name);
          }

          void appendBodyToJoint(
              const FrameIndex fid,
              const Inertia& Y,
              const SE3 & placement,
              const std::string & body_name)
          {
            const Frame & frame = model.frames[fid];
            const SE3 & p = frame.placement * placement;
            assert(frame.parent >= 0);
            if(!Y.isZero(Scalar(0)))
            {
              model.appendBodyToJoint(frame.parent, Y, p);
            }

            model.addBodyFrame(body_name, frame.parent, p, (int)fid);
            // Reference to model.frames[fid] can has changed because the vector
            // may have been reallocated.
            assert (model.frames[fid].parent >= 0);
            {
              assert (   !hasNaN(model.inertias[model.frames[fid].parent].lever())
                  && !hasNaN(model.inertias[model.frames[fid].parent].inertia().data()));
            }
          }

          FrameIndex getBodyId (const std::string& frame_name) const
          {
            if (model.existFrame(frame_name, BODY)) {
              FrameIndex fid = model.getFrameId (frame_name, BODY);
              assert(model.frames[fid].type == BODY);
              return fid;
            } else
              throw std::invalid_argument("Model does not have any body named "
                  + frame_name);
          }

        private:
          ///
          /// \brief The four possible cartesian types of an 3D axis.
          ///
          enum CartesianAxis { AXIS_X=0, AXIS_Y=1, AXIS_Z=2, AXIS_UNALIGNED };

          ///
          /// \brief Extract the cartesian property of a particular 3D axis.
          ///
          /// \param[in] axis The input URDF axis.
          ///
          /// \return The property of the particular axis pinocchio::urdf::CartesianAxis.
          ///
          static inline CartesianAxis extractCartesianAxis (const Vector3 & axis)
          {
            if( axis == Vector3(1., 0., 0.))
              return AXIS_X;
            else if( axis == Vector3(0., 1., 0.))
              return AXIS_Y;
            else if( axis == Vector3(0., 0., 1.))
              return AXIS_Z;
            else
              return AXIS_UNALIGNED;
          }

          template <typename TypeX, typename TypeY, typename TypeZ,
                   typename TypeUnaligned>
          JointIndex addJoint(
              const Vector3& axis,
              const Frame & frame,
              const SE3 & placement,
              const std::string & joint_name,
              const VectorConstRef& max_effort,
              const VectorConstRef& max_velocity,
              const VectorConstRef& min_config,
              const VectorConstRef& max_config)
          {
            CartesianAxis axisType = extractCartesianAxis(axis);
            switch (axisType)
            {
              case AXIS_X:
                return model.addJoint(frame.parent, TypeX(),
                    frame.placement * placement, joint_name,
                    max_effort,max_velocity,min_config,max_config);
                break;

              case AXIS_Y:
                return model.addJoint(frame.parent, TypeY(),
                    frame.placement * placement, joint_name,
                    max_effort,max_velocity,min_config,max_config);
                break;

              case AXIS_Z:
                return model.addJoint(frame.parent, TypeZ(),
                    frame.placement * placement, joint_name,
                    max_effort,max_velocity,min_config,max_config);
                break;

              case AXIS_UNALIGNED:
                return model.addJoint(frame.parent, TypeUnaligned (axis.normalized()),
                    frame.placement * placement, joint_name,
                    max_effort,max_velocity,min_config,max_config);
                break;
              default:
                PINOCCHIO_CHECK_INPUT_ARGUMENT(false, "The axis type of the joint is of wrong type.");
                break;
            }
          }
      };

      template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
      class UrdfVisitorWithRootJoint : public UrdfVisitor<Scalar, Options, JointCollectionTpl>
      {
        public:
          typedef UrdfVisitor<Scalar, Options, JointCollectionTpl> Base;
          typedef typename Base::Inertia        Inertia;
          using Base::model;
          using Base::appendBodyToJoint;

          typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
          typedef typename Model::JointCollection JointCollection;
          typedef typename Model::JointModel JointModel;

          JointModel root_joint;

          UrdfVisitorWithRootJoint (Model& model, const JointModelBase<JointModel> & root_joint)
            : Base (model), root_joint (root_joint.derived()) {}

          void addRootJoint(const Inertia & Y, const std::string & body_name)
          {
            const Frame & frame = model.frames[0];

            PINOCCHIO_THROW(!model.existJointName("root_joint"),
                            std::invalid_argument,
                            "root_joint already exists as a joint in the kinematic tree.");

            JointIndex idx = model.addJoint(frame.parent, root_joint,
                SE3::Identity(), "root_joint"
                //TODO ,max_effort,max_velocity,min_config,max_config
                );
            int res (model.addJointFrame(idx, 0));
            if (res == -1) {
              std::ostringstream oss;
              oss << "root_joint already inserted as a frame but of different type. Current frames "
                "are [";
              for (typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
                  model.frames.begin (); it != model.frames.end (); ++it) {
                oss << '"' << it->name << "\",";
              }
              oss << ']';
              throw std::invalid_argument(oss.str());
            }

            FrameIndex jointFrameId = (FrameIndex) res; // C-style cast to remove polluting compilation warning. This is Bad practice. See issue #323 (rework indexes)
            appendBodyToJoint(jointFrameId, Y, SE3::Identity(), body_name);
          }
      };

      typedef UrdfVisitorBaseTpl<double, 0> UrdfVisitorBase;

      void PINOCCHIO_DLLEXPORT parseRootTree(const ::urdf::ModelInterface * urdfTree,
                                             UrdfVisitorBase & model);

      void PINOCCHIO_DLLEXPORT parseRootTree(const std::string & filename,
                                             UrdfVisitorBase & model);

      void PINOCCHIO_DLLEXPORT parseRootTreeFromXML(const std::string & xmlString,
                                                    UrdfVisitorBase & model);
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::string & filename,
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & root_joint,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, root_joint);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(filename, visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::string & filename,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(filename, visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModelFromXML(const std::string & xmlStream,
                      const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const bool verbose)
    {
      details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint);
      if (verbose) visitor.log = &std::cout;
      parseRootTreeFromXML(xmlStream, visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModelFromXML(const std::string & xmlStream,
                      ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                      const bool verbose)
    {
      details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
      if (verbose) visitor.log = &std::cout;
      parseRootTreeFromXML(xmlStream, visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree,
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
      details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(urdfTree.get(), visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const boost::shared_ptr< ::urdf::ModelInterface> urdfTree,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
      details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(urdfTree.get(), visitor);
      return model;
    }

#ifdef PINOCCHIO_WITH_CXX11_SUPPORT
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree,
               const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointModel & rootJoint,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
      details::UrdfVisitorWithRootJoint<Scalar, Options, JointCollectionTpl> visitor (model, rootJoint);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(urdfTree.get(), visitor);
      return model;
    }

    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    ModelTpl<Scalar,Options,JointCollectionTpl> &
    buildModel(const std::shared_ptr< ::urdf::ModelInterface> urdfTree,
               ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               const bool verbose)
    {
      PINOCCHIO_CHECK_INPUT_ARGUMENT(urdfTree != NULL);
      details::UrdfVisitor<Scalar, Options, JointCollectionTpl> visitor (model);
      if (verbose) visitor.log = &std::cout;
      parseRootTree(urdfTree.get(), visitor);
      return model;
    }
#endif

  } // namespace urdf
} // namespace pinocchio

#endif // ifndef __pinocchio_multibody_parsers_urdf_model_hxx__


/*
 * Copyright (c) 2019, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// 12D quadrotor dynamics.
//
///////////////////////////////////////////////////////////////////////////////

#include <quads/quadrotor12d.h>

#include <math.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <string>

namespace quads {

Vector12d Quadrotor12D::operator()(const Vector12d& x,
                                   const Vector3d& u) const {
  ROS_ASSERT(initialized_);

  const double gx = std::sin(x(kThetaIdx)) * std::cos(x(kPhiIdx)) / m_;
  const double gy = -std::sin(x(kPhiIdx)) / m_;
  const double gz = std::cos(x(kPhiIdx)) * std::cos(x(kThetaIdx)) / m_;

  Vector12d xdot;
  xdot << x(kDxIdx), x(kDyIdx), x(kDzIdx), x(kQIdx), x(kRIdx), gx * x(kZetaIdx),
      gy * x(kZetaIdx), gz * x(kZetaIdx) - 9.81, x(kXiIdx), u(0), u(1) / Ix_,
      u(2) / Iy_;
  return xdot;
}

Matrix12x12d Quadrotor12D::StateJacobian(const Vector12d& x,
                                         const Vector3d& u) const {
  const double theta = x(kThetaIdx);
  const double phi = x(kPhiIdx);
  const double zeta = x(kZetaIdx);

  Matrix12x12d F;
  F << 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
      zeta * std::cos(phi) * std::cos(theta),
      -zeta * std::sin(phi) * std::sin(theta), 0, 0, 0,
      std::cos(phi) * std::sin(theta), 0, 0, 0, 0, 0, 0, 0,
      -zeta * std::cos(phi), 0, 0, 0, -std::sin(phi), 0, 0, 0, 0, 0, 0,
      -zeta * std::cos(phi) * std::sin(theta),
      -zeta * std::cos(theta) * std::sin(phi), 0, 0, 0,
      std::cos(phi) * std::cos(theta), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
  return F;
}

Matrix5x12d Quadrotor12D::OutputJacobian(const Vector12d& x) const {
  Matrix5x12d H(Matrix5x12d::Zero());
  H(0, 0) = 1.0;
  H(1, 1) = 1.0;
  H(2, 2) = 1.0;
  H(3, 3) = 1.0;
  H(4, 4) = 1.0;
  return H;
}

bool Quadrotor12D::Initialize(const ros::NodeHandle& n) {
  name_ = ros::names::append(n.getNamespace(), "quadrotor12d");

  if (!LoadParameters(n)) {
    ROS_ERROR("%s: Failed to load parameters.", name_.c_str());
    return false;
  }

  initialized_ = true;
  return true;
}

bool Quadrotor12D::LoadParameters(const ros::NodeHandle& n) {
  ros::NodeHandle nl(n);

  // Mass and inertia.
  if (!nl.getParam("dynamics/m", m_)) return false;
  if (!nl.getParam("dynamics/Ix", Ix_)) return false;
  if (!nl.getParam("dynamics/Iy", Iy_)) return false;

  return true;
}

}  // namespace quads


#include <kazen/camera.h>
#include <kazen/rfilter.h>
#include <kazen/warp.h>
#include <Eigen/Geometry>

NAMESPACE_BEGIN(kazen)

/**
 * \brief Perspective camera with depth of field
 *
 * This class implements a simple perspective camera model. It uses an
 * infinitesimally small aperture, creating an infinite depth of field.
 */
class PerspectiveCamera : public Camera {
public:
    PerspectiveCamera(const PropertyList &propList) {
        /* Width and height in pixels. Default: 720p */
        m_outputSize.x() = propList.getInteger("width", 1280);
        m_outputSize.y() = propList.getInteger("height", 720);
        m_invOutputSize = m_outputSize.cast<float>().cwiseInverse();

        /* Specifies an optional camera-to-world transformation. Default: none */
        m_cameraToWorld = propList.getTransform("toWorld", Transform());

        /* Horizontal field of view in degrees */
        m_fov = propList.getFloat("fov", 30.0f);

        /* Near and far clipping planes in world-space units */
        m_nearClip = propList.getFloat("nearClip", 1e-4f);
        m_farClip = propList.getFloat("farClip", 1e4f);

        m_rfilter = NULL;
    }

    void activate() {
        float aspect = m_outputSize.x() / (float) m_outputSize.y();

        /* Project vectors in camera space onto a plane at z=1:
         *
         *  xProj = cot * x / z
         *  yProj = cot * y / z
         *  zProj = (far * (z - near)) / (z * (far-near))
         *  The cotangent factor ensures that the field of view is
         *  mapped to the interval [-1, 1].
         */
        float recip = 1.0f / (m_farClip - m_nearClip),
              cot = 1.0f / std::tan(math::degToRad(m_fov / 2.0f));

        Eigen::Matrix4f perspective;
        perspective <<
            cot, 0,   0,   0,
            0, cot,   0,   0,
            0,   0,   m_farClip * recip, -m_nearClip * m_farClip * recip,
            0,   0,   1,   0;

        /**
         * Translation and scaling to shift the clip coordinates into the
         * range from zero to one. Also takes the aspect ratio into account.
         */
        m_sampleToCamera = Transform(
            Eigen::DiagonalMatrix<float, 3>(Vector3f(-0.5f, -0.5f * aspect, 1.0f)) *
            Eigen::Translation<float, 3>(-1.0f, -1.0f/aspect, 0.0f) * perspective).inverse();

        /* If no reconstruction filter was assigned, instantiate a Gaussian filter */
        if (!m_rfilter)
            m_rfilter = static_cast<ReconstructionFilter *>(
                ObjectFactory::createInstance("gaussian", PropertyList()));
    }

    Color3f sampleRay(Ray3f &ray,
            const Point2f &samplePosition,
            const Point2f &apertureSample) const {
        /* Compute the corresponding position on the
           near plane (in local camera space) */
        Point3f nearP = m_sampleToCamera * Point3f(
            samplePosition.x() * m_invOutputSize.x(),
            samplePosition.y() * m_invOutputSize.y(), 0.0f);

        /* Turn into a normalized ray direction, and
           adjust the ray interval accordingly */
        Vector3f d = nearP.normalized();
        float invZ = 1.0f / d.z();

        ray.o = m_cameraToWorld * Point3f(0, 0, 0);
        ray.d = m_cameraToWorld * d;
        ray.mint = m_nearClip * invZ;
        ray.maxt = m_farClip * invZ;
        ray.update();

        return Color3f(1.0f);
    }

    void addChild(Object *obj) {
        switch (obj->getClassType()) {
            case EReconstructionFilter:
                if (m_rfilter)
                    throw Exception("Camera: tried to register multiple reconstruction filters!");
                m_rfilter = static_cast<ReconstructionFilter *>(obj);
                break;

            default:
                throw Exception("Camera::addChild(<{}>) is not supported!",
                    classTypeName(obj->getClassType()));
        }
    }

    /// Return a human-readable summary
    std::string toString() const {
        return fmt::format(
            "PerspectiveCamera[\n"
            "  cameraToWorld = {},\n"
            "  outputSize = {},\n"
            "  fov = {},\n"
            "  clip = [{}, {}],\n"
            "  rfilter = {}\n"
            "]",
            string::indent(m_cameraToWorld.toString(), 18),
            m_outputSize.toString(),
            m_fov,
            m_nearClip, m_farClip,
            string::indent(m_rfilter->toString())
        );
    }
private:
    Vector2f m_invOutputSize;
    Transform m_sampleToCamera;
    Transform m_cameraToWorld;
    float m_fov;
    float m_nearClip;
    float m_farClip;
};

class ThinlensCamera : public Camera {
public:
    ThinlensCamera(const PropertyList &propList) {
        /* Width and height in pixels. Default: 720p */
        m_outputSize.x() = propList.getInteger("width", 1280);
        m_outputSize.y() = propList.getInteger("height", 720);
        m_apertureRadius = propList.getFloat("apertureRadius", 1.0);
        m_focusDistance = propList.getFloat("focusDistance", 0.0);
        m_invOutputSize = m_outputSize.cast<float>().cwiseInverse();

        /* Specifies an optional camera-to-world transformation. Default: none */
        m_cameraToWorld = propList.getTransform("toWorld", Transform());

        /* Horizontal field of view in degrees */
        m_fov = propList.getFloat("fov", 30.0f);

        /* Near and far clipping planes in world-space units */
        m_nearClip = propList.getFloat("nearClip", 1e-4f);
        m_farClip = propList.getFloat("farClip", 1e4f);

        m_rfilter = NULL;
    }

    void activate() {
        float aspect = m_outputSize.x() / (float) m_outputSize.y();

        /* Project vectors in camera space onto a plane at z=1:
         *
         *  xProj = cot * x / z
         *  yProj = cot * y / z
         *  zProj = (far * (z - near)) / (z * (far-near))
         *  The cotangent factor ensures that the field of view is
         *  mapped to the interval [-1, 1].
         */
        float recip = 1.0f / (m_farClip - m_nearClip),
              cot = 1.0f / std::tan(math::degToRad(m_fov / 2.0f));

        Eigen::Matrix4f perspective;
        perspective <<
            cot, 0,   0,   0,
            0, cot,   0,   0,
            0,   0,   m_farClip * recip, -m_nearClip * m_farClip * recip,
            0,   0,   1,   0;

        /**
         * Translation and scaling to shift the clip coordinates into the
         * range from zero to one. Also takes the aspect ratio into account.
         */
        m_sampleToCamera = Transform(
            Eigen::DiagonalMatrix<float, 3>(Vector3f(-0.5f, -0.5f * aspect, 1.0f)) *
            Eigen::Translation<float, 3>(-1.0f, -1.0f/aspect, 0.0f) * perspective).inverse();

        /* If no reconstruction filter was assigned, instantiate a Gaussian filter */
        if (!m_rfilter)
            m_rfilter = static_cast<ReconstructionFilter *>(
                ObjectFactory::createInstance("gaussian", PropertyList()));
    }

    Color3f sampleRay(Ray3f &ray,
            const Point2f &samplePosition,
            const Point2f &apertureSample) const {

        Point2f tmp = Warp::squareToUniformDisk(apertureSample) * m_apertureRadius;
        /* Compute the corresponding position on the
           near plane (in local camera space) */
        Point3f nearP = m_sampleToCamera * Point3f(
            samplePosition.x() * m_invOutputSize.x(),
            samplePosition.y() * m_invOutputSize.y(), 0.0f);

        /* Aperture position */
        Point3f apertureP(tmp.x(), tmp.y(), 0.0);

        /* Sampled position on the focal plane */
        Point3f focusP = nearP * (m_focusDistance / nearP.z());

        /* Turn into a normalized ray direction, and
           adjust the ray interval accordingly */

        Vector3f d = focusP - apertureP;
        d.normalize();
        float invZ = 1.0f / d.z();

        ray.o = m_cameraToWorld * apertureP;
        ray.d = m_cameraToWorld * d;
        ray.mint = m_nearClip * invZ;
        ray.maxt = m_farClip * invZ;

        ray.update();

        return Color3f(1.0f);
    }

    void addChild(Object *obj) {
        switch (obj->getClassType()) {
            case EReconstructionFilter:
                if (m_rfilter)
                    throw Exception("Camera: tried to register multiple reconstruction filters!");
                m_rfilter = static_cast<ReconstructionFilter *>(obj);
                break;

            default:
                throw Exception("Camera::addChild(<%s>) is not supported!",
                    classTypeName(obj->getClassType()));
        }
    }

    /// Return a human-readable summary
    std::string toString() const {
        return fmt::format(
            "ThinlensCamera[\n"
            "  cameraToWorld = %s,\n"
            "  outputSize = %s,\n"
            "  fov = %f,\n"
            "  clip = [%f, %f],\n"
            "  apertureRadius = %f,\n"
            "  focusDistance = %f,\n"
            "  rfilter = %s\n"
            "]",
            string::indent(m_cameraToWorld.toString(), 18),
            m_outputSize.toString(),
            m_fov,
            m_nearClip,
            m_farClip,
            m_apertureRadius,
            m_focusDistance,
            string::indent(m_rfilter->toString())
        );
    }
private:
    Vector2f m_invOutputSize;
    Transform m_sampleToCamera;
    Transform m_cameraToWorld;
    float m_fov;
    float m_nearClip;
    float m_farClip;
    float m_apertureRadius;
    float m_focusDistance;
};

KAZEN_REGISTER_CLASS(PerspectiveCamera, "perspective");
KAZEN_REGISTER_CLASS(ThinlensCamera, "thinlens");
NAMESPACE_END(kazen)


/**
 * \file pkey.cpp
 * \author Julien Kauffmann <julien.kauffmann@freelan.org>
 * \brief A PKEY sample file.
 */

#include <cryptoplus/cryptoplus.hpp>
#include <cryptoplus/pkey/pkey.hpp>
#include <cryptoplus/hash/message_digest_context.hpp>
#include <cryptoplus/error/error_strings.hpp>
#include <cryptoplus/file.hpp>

#include <boost/shared_ptr.hpp>

#include <iostream>
#include <string>
#include <cstdio>

#ifdef MSV
#include <openssl/applink.c>
#endif

namespace
{
	int pem_passphrase_callback(char* buf, int buf_len, int rwflag, void*)
	{
		std::cout << "Passphrase (max: " << buf_len << " characters): " << std::flush;
		std::string passphrase;
		std::getline(std::cin, passphrase);

		if (passphrase.empty())
		{
			std::cerr << "Passphrase cannot be empty." << std::endl;
			return 0;
		}

		if (passphrase.size() > static_cast<size_t>(buf_len))
		{
			std::cerr << "Passphrase cannot exceed " << buf_len << " characters." << std::endl;
			return 0;
		}

		if (rwflag != 0)
		{
			std::cout << "Confirm: " << std::flush;
			std::string passphrase_confirmation;
			std::getline(std::cin, passphrase_confirmation);

			if (passphrase_confirmation != passphrase)
			{
				std::cerr << "The two passphrases do not match !" << std::endl;
				return 0;
			}
		}

		std::copy(passphrase.begin(), passphrase.end(), buf);
		return passphrase.size();
	}
}

int main()
{
	cryptoplus::crypto_initializer crypto_initializer;
	cryptoplus::algorithms_initializer algorithms_initializer;
	cryptoplus::error::error_strings_initializer error_strings_initializer;

	std::cout << "PKEY sample" << std::endl;
	std::cout << "===========" << std::endl;
	std::cout << std::endl;

	const std::string private_key_filename = "private_key.pem";
	const std::string certificate_public_key_filename = "certificate_public_key.pem";

	cryptoplus::file private_key_file = cryptoplus::file::open(private_key_filename, "w");
	cryptoplus::file certificate_public_key_file = cryptoplus::file::open(certificate_public_key_filename, "w");

	if (!private_key_file)
	{
		std::cerr << "Unable to open \"" << private_key_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	if (!certificate_public_key_file)
	{
		std::cerr << "Unable to open \"" << certificate_public_key_filename << "\" for writing." << std::endl;

		return EXIT_FAILURE;
	}

	try
	{
		std::cout << "Generating DSA key. This can take some time..." << std::endl;

		cryptoplus::pkey::dsa_key dsa_key = cryptoplus::pkey::dsa_key::generate_private_key(1024, NULL, 0, NULL, NULL);

		std::cout << "Done." << std::endl;

		cryptoplus::pkey::pkey pkey = cryptoplus::pkey::pkey::from_dsa_key(dsa_key);

		std::cout << "Checking that the type is correct: " << (pkey.is_dsa() ? "OK" : "FAILURE") << std::endl;

		pkey.write_private_key_pkcs8(private_key_file, cryptoplus::cipher::cipher_algorithm("AES256"), pem_passphrase_callback);

		std::cout << "Private key written succesfully to \"" << private_key_filename << "\"." << std::endl;

		pkey.write_certificate_public_key(certificate_public_key_file);

		std::cout << "Certificate public key written succesfully to \"" << certificate_public_key_filename << "\"." << std::endl;
	}
	catch (std::exception& ex)
	{
		std::cerr << "Exception: " << ex.what() << std::endl;

		return EXIT_FAILURE;
	}

	certificate_public_key_file = cryptoplus::file();
	private_key_file = cryptoplus::file::open(private_key_filename, "r");

	if (!private_key_file)
	{
		std::cerr << "Unable to open \"" << private_key_filename << "\" for reading." << std::endl;

		return EXIT_FAILURE;
	}

	try
	{
		std::cout << "Trying to read back the private key from \"" << private_key_filename << "\"..." << std::endl;

		cryptoplus::pkey::pkey pkey = cryptoplus::pkey::pkey::from_private_key(private_key_file, pem_passphrase_callback);

		std::cout << "Done." << std::endl;
	}
	catch (std::exception& ex)
	{
		std::cerr << "Exception: " << ex.what() << std::endl;

		return EXIT_FAILURE;
	}

	return EXIT_SUCCESS;
}


// This file is part of the reference implementation for the paper
//   Bayesian Collaborative Denoising for Monte-Carlo Rendering
//   Malik Boughida and Tamy Boubekeur.
//   Computer Graphics Forum (Proc. EGSR 2017), vol. 36, no. 4, p. 137-153, 2017
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE.txt file.

// BCD headers.
#include "bcd/DeepImage.h"
#include "bcd/Denoiser.h"
#include "bcd/IDenoiser.h"
#include "bcd/ImageIO.h"
#include "bcd/MultiscaleDenoiser.h"
#include "bcd/SpikeRemovalFilter.h"
#include "bcd/Utils.h"

// Eigen headers.
#include <Eigen/Dense>

// Standard headers.
#include <cstdlib>
#include <ctime>
#include <fstream>
#include <iostream>
#include <memory>
#include <string>

using namespace std;
using namespace bcd;

static const char* g_pProgramPath;

class ProgramArguments
{
  public:
    ProgramArguments() :
        m_denoisedOutputFilePath(""),
        m_colorImage(), m_nbOfSamplesImage(), m_histogramImage(), m_covarianceImage(),
        m_histogramPatchDistanceThreshold(1.f),
        m_patchRadius(1), m_searchWindowRadius(6),
        m_minEigenValue(1.e-8f),
        m_useRandomPixelOrder(false),
        m_prefilterSpikes(false),
        m_prefilterThresholdStDevFactor(2.f),
        m_markedPixelsSkippingProbability(1.f),
        m_nbOfScales(3),
        m_nbOfCores(0)
    {
    }

    string m_denoisedOutputFilePath; // File path to the denoised image output
    Deepimf m_colorImage; // Pixel color values
    Deepimf m_nbOfSamplesImage; // Pixel number of samples
    Deepimf m_histogramImage; // Pixel histograms
    Deepimf m_covarianceImage; // Pixel covariances
    float m_histogramPatchDistanceThreshold; // Histogram patch distance threshold
    int m_patchRadius; // Patch has (1 + 2 x m_patchRadius)^2 pixels
    int m_searchWindowRadius; // Search windows (for neighbors) spreads across (1 + 2 x m_patchRadius)^2 pixels
    float m_minEigenValue; // Minimum eigen value for matrix inversion
    bool m_useRandomPixelOrder; // True means the pixel will be processed in a random order ; could be useful to remove some "grid" artifacts
    bool m_prefilterSpikes; // True means a spike removal prefiltering will be applied
    float m_prefilterThresholdStDevFactor; // See SpikeRemovalFilter::filter argument
    float m_markedPixelsSkippingProbability; // 1 means the marked centers of the denoised patches will be skipped to accelerate a lot the computations
    int m_nbOfScales;
    int m_nbOfCores; // Number of cores used by OpenMP. O means using the value defined in environment variable OMP_NUM_THREADS
};

class Callbacks
  : public ICallbacks
{
  public:
    Callbacks()
    {
    }

    void progress(const float i_progress) const override
    {
    }

    bool isAborted() const override
    {
        return false;
    }

  private:
    void logInfo(const char* msg) const override
    {
        cout << "Info: " << msg << std::endl;
    }

    void logWarning(const char* msg) const override
    {
        cout << "Warning: " << msg << std::endl;
    }

    void logError(const char* msg) const override
    {
        cout << "Error: " << msg << std::endl;
    }

    void logDebug(const char* msg) const override
    {
        cout << "Debug: " << msg << std::endl;
    }
};

Callbacks g_callbacks;

void initializeRandomSeed()
{
    srand(static_cast<unsigned int>(time(0)));
}

static void printUsage()
{
    ProgramArguments defaultProgramArgs;
    cout << "Bayesian Collaborative Denoising"<< endl << endl;
    cout << "Usage: " << g_pProgramPath << " <arguments list>" << endl;
    cout << "Only EXR images are supported." << endl << endl;
    cout << "Required arguments list:" << endl;
    cout << "    -o <output>          The file path to the output image" << endl;
    cout << "    -i <input>           The file path to the input image" << endl;
    cout << "    -h <hist>            The file path to the input histograms buffer" << endl;
    cout << "    -c <cov>             The file path to the input covariance matrices buffer" << endl;
    cout << "Optional arguments list:" << endl;
    cout << "    -d <float>           Histogram patch distance threshold (default: " << defaultProgramArgs.m_histogramPatchDistanceThreshold << ")" << endl;
    cout << "    -b <int>             Radius of search windows (default: " << defaultProgramArgs.m_searchWindowRadius << ")" << endl;
    cout << "    -w <int>             Radius of patches (default: " << defaultProgramArgs.m_patchRadius << ")" << endl;
    cout << "    -r <0/1>             1 for random pixel order (in case of grid artifacts) (default: " << (defaultProgramArgs.m_useRandomPixelOrder ? 1 : 0) << ")" << endl;
    cout << "    -p <0/1>             1 for a spike removal prefiltering (default: " << (defaultProgramArgs.m_prefilterSpikes ? 1 : 0) << ")" << endl;
    cout << "    --p-factor <float>   Factor that is multiplied by standard deviation to get the threshold for classifying spikes during prefiltering. Put lower value to remove more spikes (default: " << defaultProgramArgs.m_prefilterThresholdStDevFactor << ")" << endl;
    cout << "    -m <float in [0,1]>  Probability of skipping marked centers of denoised patches. 1 accelerates a lot the computations. 0 helps removing potential grid artifacts (default: " << defaultProgramArgs.m_markedPixelsSkippingProbability << ")" << endl;
    cout << "    -s <int>             Number of Scales for Multi-Scaling (default: " << defaultProgramArgs.m_nbOfScales << ")" << endl;
    cout << "    --ncores <nbOfCores> Number of cores used by OpenMP (default: environment variable OMP_NUM_THREADS)" << endl;
    cout << "    -e <float>           Minimum eigen value for matrix inversion (default: " << defaultProgramArgs.m_minEigenValue << ")" << endl;
}

bool parseProgramArguments(int argc, const char** argv, ProgramArguments& o_rProgramArguments)
{
    int argIndex = 0;
    bool missingColor = true, missingHist = true, missingCov = true, missingOutput = true;
    string inputColorFilePath;
    while (++argIndex < argc)
    {
        if (strcmp(argv[argIndex], "-o") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting file path to the output image after '-o'" << endl;
                return false;
            }
            o_rProgramArguments.m_denoisedOutputFilePath = string(argv[argIndex]);
            ofstream outputFile(o_rProgramArguments.m_denoisedOutputFilePath, ofstream::out | ofstream::app);
            if(!outputFile)
            {
                cout << "Error in program arguments: cannot write output file '" << o_rProgramArguments.m_denoisedOutputFilePath << "'" << endl;
                return false;
            }
            missingOutput = false;
        }
        else if (strcmp(argv[argIndex], "-i") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting file path to the input color image after '-i'" << endl;
                return false;
            }
            inputColorFilePath = argv[argIndex];
            if (!ImageIO::loadEXR(o_rProgramArguments.m_colorImage, argv[argIndex]))
            {
                cout << "Error in program arguments: couldn't load input color image file '" << argv[argIndex] << "'" << endl;
                return false;
            }
            missingColor = false;
        }
        else if (strcmp(argv[argIndex], "-h") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting file path to the input histogram image after '-h'" << endl;
                return false;
            }
            Deepimf histAndNbOfSamplesImage;
            if (!ImageIO::loadMultiChannelsEXR(histAndNbOfSamplesImage, argv[argIndex]))
            {
                cout << "Error in program arguments: couldn't load input histogram image file '" << argv[argIndex] << "'" << endl;
                return false;
            }
            Utils::separateNbOfSamplesFromHistogram(o_rProgramArguments.m_histogramImage, o_rProgramArguments.m_nbOfSamplesImage, histAndNbOfSamplesImage);
            missingHist = false;
        }
        else if (strcmp(argv[argIndex], "-c") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting file path to the input covariance matrix image after '-c'" << endl;
                return false;
            }
            if (!ImageIO::loadMultiChannelsEXR(o_rProgramArguments.m_covarianceImage, argv[argIndex]))
            {
                cout << "Error in program arguments: couldn't load input covariance matrix image file '" << argv[argIndex] << "'" << endl;
                return false;
            }
            missingCov = false;
        }
        else if (strcmp(argv[argIndex], "-d") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting histogram patch distance threshold after '-d'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_histogramPatchDistanceThreshold;
        }
        else if (strcmp(argv[argIndex], "-b") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting radius of search window after '-b'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_searchWindowRadius;
        }
        else if (strcmp(argv[argIndex], "-w") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting radius of patch after '-w'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_patchRadius;
        }
        else if (strcmp(argv[argIndex], "-e") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting minimum eigen value after '-e'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_minEigenValue;
        }
        else if (strcmp(argv[argIndex], "-r") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting 0 or 1 after '-r'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            int useRandomPixelOrder;
            iss >> useRandomPixelOrder;
            if(useRandomPixelOrder != 0 && useRandomPixelOrder != 1)
            {
                cout << "Error in program arguments: expecting 0 or 1 after '-r'" << endl;
                return false;
            }
            o_rProgramArguments.m_useRandomPixelOrder = (useRandomPixelOrder==1);
        }
        else if (strcmp(argv[argIndex], "-p") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting 0 or 1 after '-p'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            int prefilterSpikes;
            iss >> prefilterSpikes;
            if(prefilterSpikes != 0 && prefilterSpikes != 1)
            {
                cout << "Error in program arguments: expecting 0 or 1 after '-p'" << endl;
                return false;
            }
            o_rProgramArguments.m_prefilterSpikes = (prefilterSpikes==1);
        }
        else if (strcmp(argv[argIndex], "--p-factor") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting standard deviation factor for spike prefiltering threshold after '--p-factor'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_prefilterThresholdStDevFactor;
        }
        else if(strcmp(argv[argIndex], "-m") == 0)
        {
            argIndex++;
            if(argIndex == argc)
            {
                cout << "Error in program arguments: expecting float in [0,1] after '-m'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            float markedPixelsSkippingProbability;
            iss >> markedPixelsSkippingProbability;
            if(markedPixelsSkippingProbability < 0 || markedPixelsSkippingProbability > 1)
            {
                cout << "Error in program arguments: expecting float in [0,1] after '-m'" << endl;
                return false;
            }
            o_rProgramArguments.m_markedPixelsSkippingProbability = markedPixelsSkippingProbability;
        }
        else if(strcmp(argv[argIndex], "-s") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting number of scales after '-s'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_nbOfScales;
        }
        else if (strcmp(argv[argIndex], "--ncores") == 0)
        {
            argIndex++;
            if (argIndex == argc)
            {
                cout << "Error in program arguments: expecting number of cores for OpenMP after '--ncores'" << endl;
                return false;
            }
            istringstream iss(argv[argIndex]);
            iss >> o_rProgramArguments.m_nbOfCores;
        }
    }
    if(!missingColor)
    {
        if(missingHist)
        {
            string inputHistFilePath = inputColorFilePath.substr(0, inputColorFilePath.length() - 4) + "_hist.exr"; // "-4" for removing extension .exr
            cout << "Warning: input histogram file not provided by -h argument: assuming '" + inputHistFilePath + "'" << endl;
            Deepimf histAndNbOfSamplesImage;
            if (!ImageIO::loadMultiChannelsEXR(histAndNbOfSamplesImage, inputHistFilePath.c_str()))
            {
                cout << "Error in program arguments: couldn't load input histogram image file '" << inputHistFilePath << "'" << endl;
                return false;
            }
            Utils::separateNbOfSamplesFromHistogram(o_rProgramArguments.m_histogramImage, o_rProgramArguments.m_nbOfSamplesImage, histAndNbOfSamplesImage);
            missingHist = false;
        }
        if(missingCov)
        {
            string inputCovFilePath = inputColorFilePath.substr(0, inputColorFilePath.length() - 4) + "_cov.exr"; // "-4" for removing extension .exr
            cout << "Warning: input covariance file not provided by -c argument: assuming '" + inputCovFilePath + "'" << endl;
            if (!ImageIO::loadMultiChannelsEXR(o_rProgramArguments.m_covarianceImage, inputCovFilePath.c_str()))
            {
                cout << "Error in program arguments: couldn't load input covariance matrix image file '" << inputCovFilePath << "'" << endl;
                return false;
            }
            missingCov = false;
        }
    }
    if (missingColor || missingHist || missingCov || missingOutput)
    {
        cout << "Error: Missing required program argument(s):";
        if (missingColor)
            cout << " -i";
        if (missingHist)
            cout << " -h";
        if (missingCov)
            cout << " -c";
        if (missingOutput)
            cout << " -o";
        cout << endl << endl;
        printUsage();
        return false;
    }
    return true;
}

int launchBayesianCollaborativeDenoising(int argc, const char** argv)
{
    ProgramArguments programArgs;
    if(!parseProgramArguments(argc, argv, programArgs))
        return 1;

    if(programArgs.m_prefilterSpikes)
        SpikeRemovalFilter::filter(
                programArgs.m_colorImage,
                programArgs.m_nbOfSamplesImage,
                programArgs.m_histogramImage,
                programArgs.m_covarianceImage,
                programArgs.m_prefilterThresholdStDevFactor);

    DenoiserInputs inputs;
    DenoiserOutputs outputs;
    DenoiserParameters parameters;

    inputs.m_pColors = &(programArgs.m_colorImage);
    inputs.m_pNbOfSamples = &(programArgs.m_nbOfSamplesImage);
    inputs.m_pHistograms = &(programArgs.m_histogramImage);
    inputs.m_pSampleCovariances = &(programArgs.m_covarianceImage);

    Deepimf outputDenoisedColorImage(programArgs.m_colorImage);
    outputs.m_pDenoisedColors = &outputDenoisedColorImage;

    parameters.m_histogramDistanceThreshold = programArgs.m_histogramPatchDistanceThreshold;
    parameters.m_patchRadius = programArgs.m_patchRadius;
    parameters.m_searchWindowRadius = programArgs.m_searchWindowRadius;
    parameters.m_minEigenValue = programArgs.m_minEigenValue;
    parameters.m_useRandomPixelOrder = programArgs.m_useRandomPixelOrder;
    parameters.m_markedPixelsSkippingProbability = programArgs.m_markedPixelsSkippingProbability;
    parameters.m_nbOfCores = programArgs.m_nbOfCores;

    unique_ptr<IDenoiser> uDenoiser = nullptr;

    if(programArgs.m_nbOfScales > 1)
        uDenoiser.reset(new MultiscaleDenoiser(programArgs.m_nbOfScales));
    else
        uDenoiser.reset(new Denoiser());

    uDenoiser->setCallbacks(&g_callbacks);

    uDenoiser->setInputs(inputs);
    uDenoiser->setOutputs(outputs);
    uDenoiser->setParameters(parameters);

    if (!uDenoiser->inputsOutputsAreOk())
        return 1;

    uDenoiser->denoise();

    ImageIO::writeEXR(outputDenoisedColorImage, programArgs.m_denoisedOutputFilePath.c_str());

    cout << "Written denoised output in file " << programArgs.m_denoisedOutputFilePath.c_str() << std::endl;

    return 0;
}

int main(int argc, const char** argv)
{
    g_pProgramPath = argv[0];
    return launchBayesianCollaborativeDenoising(argc, argv);
}


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

#ifndef geometry_triclip_hpp_included_
#define geometry_triclip_hpp_included_

#include <boost/numeric/ublas/vector.hpp>
#include "math/geometry_core.hpp"
#include "dbglog/dbglog.hpp"

namespace ublas = boost::numeric::ublas;

namespace geometry {

/** Textured 3D triangle representation suitable for clipping algorithm.
 *
 * Triangle holds positions and tex. coordinates of its vertices.
 */
struct ClipTriangle
{
        typedef std::vector<ClipTriangle> list;
        math::Point3 pos[3]; // vertices
        math::Point2 uv[3];  // texture coordinates

        bool texCoordsAvailable;

        ClipTriangle(math::Point3 a, math::Point3 b, math::Point3 c)
            : texCoordsAvailable(false)
        {
            pos[0] = a, pos[1] = b, pos[2] = c;
        }

        ClipTriangle(math::Point3 a, math::Point3 b, math::Point3 c,
                     math::Point2 ta, math::Point2 tb, math::Point2 tc)
            : texCoordsAvailable(true)
        {
            pos[0] = a, pos[1] = b, pos[2] = c;
            uv[0] = ta, uv[1] = tb, uv[2] = tc;
        }
};

/** Clipping plane
 *
 * Plane is defined by it's normal and the shift in the direction of the normal
 */
struct ClipPlane
{
        math::Point3 normal;
        double d;

        ClipPlane(double a, double b, double c, double d)
            : normal(a, b, c), d(d)
        {}

        ClipPlane()
            : normal(0, 0, 0), d(0)
        {}
};

namespace detail {
    inline double signedDistance(const math::Point3 &point, const ClipPlane &plane)
    {
        return ublas::inner_prod(point,plane.normal) - plane.d;
    }

    inline math::Point3 intersection(const math::Point3 &p1, const math::Point3 &p2,
                       const ClipPlane &plane, double& t)
    {
        // sort points to prevent numerical inaccuracies
        math::Point3 sp1(std::max(p1,p2));
        math::Point3 sp2(std::min(p1,p2));

        double dot1 = ublas::inner_prod(sp1,plane.normal);
        double dot2 = ublas::inner_prod(sp2,plane.normal);
        double den = dot1 - dot2;

        // line parallel with plane, return the midpoint
        if (std::abs(den) < 1e-10) {
            t = 0.5;
            return (sp1 + sp2) * t;
        }

        t = (dot1 - plane.d) / den;
        return (1.0 - t)*sp1 + t*sp2;
    }
}

/** Clip triangles with given plane
 *
 * \param triangles list of triangles to be clipped
 * \param plane plane used to clip triangles
 * \param triangleInfos additional triangle informations,
                        each newly created triangle will have same triangleInfo
                        as it's origin
 * \return list of the clipped triangles
 */
template<typename TriangleInfo>
ClipTriangle::list clipTriangles( const ClipTriangle::list &triangles
                                , const ClipPlane &plane
                                , std::vector<TriangleInfo> &triangleInfos)
{
    ClipTriangle::list result;
    std::vector<TriangleInfo> resultInfo;

    if (triangleInfos.size() && triangleInfos.size()!=triangles.size()) {
        LOGTHROW(err3, std::runtime_error)
            << "Triangle count and triangle informations count mismatch.";
    }

    for ( std::size_t tid=0; tid<triangles.size(); ++tid)
    {
        auto &tri(triangles[tid]);

        bool positive[3] = {
            detail::signedDistance(tri.pos[0], plane) >= 0,
            detail::signedDistance(tri.pos[1], plane) >= 0,
            detail::signedDistance(tri.pos[2], plane) >= 0
        };

        int count = 0;
        for (int i = 0; i < 3; i++) {
            if (positive[i]) count++;
        }

        // triangle completely on negative side - do nothing
        if (count == 0) continue;

        // trinagle completely on positive side - copy to result
        if (count == 3) {
            result.push_back(tri);
            if(triangleInfos.size()){
                resultInfo.emplace_back(triangleInfos[tid]);
            }
            continue;
        }

        int a = 0, b = 0, c = 0;
        double t = 0.0;

        // case 1: one vertex on positive side, just adjust the other two
        if (count == 1)
        {
            if (positive[0]) a = 0, b = 1, c = 2;
            else if (positive[1]) a = 1, b = 2, c = 0;
            else a = 2, b = 0, c = 1;

            math::Point3 x1pos(detail::intersection(tri.pos[a], tri.pos[b], plane, t));
            math::Point2 x1uv((1.0 - t)*tri.uv[a] + t*tri.uv[b]);

            math::Point3 x2pos(detail::intersection(tri.pos[c], tri.pos[a], plane, t));
            math::Point2 x2uv((1.0 - t)*tri.uv[c] + t*tri.uv[a]);

            result.emplace_back(tri.pos[a], x1pos, x2pos,
                                tri.uv[a],  x1uv,  x2uv);
            if(triangleInfos.size()){
                resultInfo.emplace_back(triangleInfos[tid]);
            }
        }
        // case 2: two vertices on positive side, adjust triangle and add one more
        else
        {
            if (!positive[0]) a = 0, b = 1, c = 2;
            else if (!positive[1]) a = 1, b = 2, c = 0;
            else a = 2, b = 0, c = 1;

            auto tmp(1.0 - t);
            math::Point3 x1pos(detail::intersection(tri.pos[a], tri.pos[b], plane, t));
            math::Point2 x1uv(tmp*tri.uv[a] + t*tri.uv[b]);

            math::Point3 x2pos(detail::intersection(tri.pos[c], tri.pos[a], plane, t));
            math::Point2 x2uv(tmp*tri.uv[c] + t*tri.uv[a]);

            result.emplace_back(x1pos, tri.pos[b], tri.pos[c],
                                x1uv,  tri.uv[b],  tri.uv[c]);
            if(triangleInfos.size()){
                resultInfo.emplace_back(triangleInfos[tid]);
            }

            result.emplace_back(x1pos, tri.pos[c], x2pos,
                                x1uv,  tri.uv[c],  x2uv);
            if(triangleInfos.size()){
                resultInfo.emplace_back(triangleInfos[tid]);
            }
        }
    }

    triangleInfos.swap(resultInfo);
    return result;
}

} // namespace geometry
#endif // geometry_triclip_hpp_included_


/**
 * @file DynamicsState.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <array>
#include <vector>
#include <string>
#include <iostream>
#include <Eigen/Eigen>

#include <momentumopt/setting/Definitions.hpp>
#include <momentumopt/cntopt/ContactState.hpp>

namespace momentumopt {

  /**
   * This class is a container for all variables required to define a
   * dynamic state: center of mass position, linear and angular momenta
   * and its rates; forces, torques and center of pressure of the end-
   * effectors; positions, orientations, activations and contact types
   * of each end-effector.
   */
  struct DynamicsState
  {
    public:
	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  DynamicsState();
	  ~DynamicsState(){}

	  // Center of mass, linear and angular momenta
	  double& time() { return dtime_; }
	  Eigen::Vector3d& centerOfMass() { return com_; }
	  Eigen::Vector3d& linearMomentum() { return lmom_; }
	  Eigen::Vector3d& angularMomentum() { return amom_; }
	  Eigen::Vector3d& linearMomentumRate() { return lmomd_; }
	  Eigen::Vector3d& angularMomentumRate() { return amomd_; }

	  const double& time() const { return dtime_; }
	  const Eigen::Vector3d& centerOfMass() const { return com_; }
	  const Eigen::Vector3d& linearMomentum() const { return lmom_; }
	  const Eigen::Vector3d& angularMomentum() const { return amom_; }
	  const Eigen::Vector3d& linearMomentumRate() const { return lmomd_; }
	  const Eigen::Vector3d& angularMomentumRate() const { return amomd_; }

	  void time(const double& dtime) { dtime_ = dtime; }
	  void centerOfMass(const Eigen::Vector3d& com) { com_ = com; }
	  void linearMomentum(const Eigen::Vector3d& lmom) { lmom_ = lmom; }
	  void angularMomentum(const Eigen::Vector3d& amom) { amom_ = amom; }
	  void linearMomentumRate(const Eigen::Vector3d& lmomd) { lmomd_ = lmomd; }
	  void angularMomentumRate(const Eigen::Vector3d& amomd) { amomd_ = amomd; }

	  void setCenterOfMass(const Eigen::Vector3d& com) {
	      std::cout << "setting CoM to: " << std::endl;
	      com_ = com;
	      std::cout << com_ << std::endl;
	  }

	  void setCenterOfMass2(double x, double y, double z) {
	      com_ << x, y, z;
	  }

      // Endeffector forces, torques and cops
      Eigen::Vector3d& endeffectorCoP(int eff_id) { return eff_cops_[eff_id]; }
      Eigen::Vector3d& endeffectorForce(int eff_id) { return eff_forces_[eff_id]; }
      Eigen::Vector3d& endeffectorTorque(int eff_id) { return eff_torques_[eff_id]; }
      Eigen::Vector3d& endeffectorTorqueAtContactPoint(int eff_id) { return eefs_trqs_contact_point_[eff_id]; }

      const long endeffectorNum() { return eff_cops_.size(); }
      const Eigen::Vector3d& endeffectorCoP(int eff_id) const { return eff_cops_[eff_id]; }
      const Eigen::Vector3d& endeffectorForce(int eff_id) const { return eff_forces_[eff_id]; }
      const Eigen::Vector3d& endeffectorTorque(int eff_id) const { return eff_torques_[eff_id]; }
      const Eigen::Vector3d& endeffectorTorqueAtContactPoint(int eff_id) const { return eefs_trqs_contact_point_[eff_id]; }

      const std::vector<Eigen::Vector3d>& pyEndeffectorCops() const { return eff_cops_; }
      const std::vector<Eigen::Vector3d>& pyEndeffectorForces() const { return eff_forces_; }
      const std::vector<Eigen::Vector3d>& pyEndeffectorTorques() const { return eff_torques_; }

      void pyEndeffectorCops(const std::vector<Eigen::Vector3d>& eff_cops) { eff_cops_ = eff_cops; }
      void pyEndeffectorForces(const std::vector<Eigen::Vector3d>& eff_forces) { eff_forces_ = eff_forces; }
      void pyEndeffectorTorques(const std::vector<Eigen::Vector3d>& eff_torques) { eff_torques_ = eff_torques; }

	  // Endeffector activations, activation and contact ids
      int& endeffectorContactId(int eff_id) { return cnt_ids_[eff_id]; }
      int& endeffectorActivationId(int eff_id) { return eff_ids_[eff_id]; }
      bool& endeffectorActivation(int eff_id) { return eff_activations_[eff_id]; }

      const int& endeffectorContactId(int eff_id) const { return cnt_ids_[eff_id]; }
      const int& endeffectorActivationId(int eff_id) const { return eff_ids_[eff_id]; }
      bool endeffectorActivation(int eff_id) const { return eff_activations_[eff_id]; }

      // endeffector poses
      Eigen::Vector3d& endeffectorPosition(int eff_id) { return eff_positions_[eff_id]; }
      Eigen::Vector3d& endeffectorVelocity(int eff_id) { return eff_velocities_[eff_id]; }
      Eigen::Vector3d& endeffectorAcceleration(int eff_id) { return eff_accelerations_[eff_id]; }
      Eigen::Quaternion<double>& endeffectorOrientation(int eff_id) { return eff_orientations_[eff_id]; }

      const Eigen::Vector3d& endeffectorPosition(int eff_id) const { return eff_positions_[eff_id]; }
      const Eigen::Vector3d& endeffectorVelocity(int eff_id) const { return eff_velocities_[eff_id]; }
      const Eigen::Vector3d& endeffectorAcceleration(int eff_id) const { return eff_accelerations_[eff_id]; }
      const Eigen::Quaternion<double>& endeffectorOrientation(int eff_id) const { return eff_orientations_[eff_id]; }

      const std::vector<Eigen::Vector3d>& pyEndeffectorPositions() const { return eff_positions_; }
      void pyEndeffectorPositions(const std::vector<Eigen::Vector3d>& eff_positions) { eff_positions_ = eff_positions; }

	  // Helper functions
	  std::string toString() const;
	  friend std::ostream& operator<<(std::ostream &os, const DynamicsState& obj) { return os << obj.toString(); }
	  void fillInitialRobotState(const std::string cfg_file, const std::string robot_state = "initial_robot_configuration");

    private:
	  double dtime_;
	  Eigen::Vector3d com_, amom_, lmom_, amomd_, lmomd_;
      std::array<bool, Problem::n_endeffs_> eff_activations_;
      std::array<int, Problem::n_endeffs_> eff_ids_, cnt_ids_;

      std::vector<Eigen::Quaternion<double>> eff_orientations_;
      std::vector<Eigen::Vector3d> eff_positions_, eff_velocities_, eff_accelerations_;
      std::vector<Eigen::Vector3d> eff_forces_, eff_torques_, eff_cops_, eefs_trqs_contact_point_;
  };

  /**
   * This class is a container for a sequence of dynamic states,
   * for all time steps in the optimization.
   */
  class DynamicsSequence
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  DynamicsSequence(){}
	  ~DynamicsSequence(){}

	  void resize(int num_timesteps);
	  void clean() { dynamics_sequence_.clear(); }
	  int size() const { return dynamics_sequence_.size(); }

	  const std::vector<DynamicsState>& dynamicsSequence() const { return dynamics_sequence_; }
	  void dynamicsSequence(const std::vector<DynamicsState>& dynamics_sequence) { dynamics_sequence_ = dynamics_sequence; }

	  void setCoMAtTime(const Eigen::VectorXd& com, int time_id) { dynamics_sequence_[time_id].setCenterOfMass(com); }

      DynamicsState& dynamicsState(int time_id) { return dynamics_sequence_[time_id]; }
	  const DynamicsState& dynamicsState(int time_id) const { return dynamics_sequence_[time_id]; }

	  Eigen::Matrix<int, Problem::n_endeffs_, 1>& activeEndeffectorSteps() { return active_endeffector_steps_; }
	  const Eigen::Matrix<int, Problem::n_endeffs_, 1>& activeEndeffectorSteps() const { return active_endeffector_steps_; }

	  std::string toString() const;
  	  friend std::ostream& operator<<(std::ostream &os, const DynamicsSequence& obj) { return os << obj.toString(); }

    private:
      std::vector<DynamicsState> dynamics_sequence_;
      Eigen::Matrix<int, Problem::n_endeffs_, 1> active_endeffector_steps_;
  };

}


/*

Software License Agreement (BSD License)

Copyright (c) 2016--, Liana Bertoni (liana.bertoni@gmail.com)
  All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder(s) nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Contact GitHub API Training Shop Blog About
*/

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>

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

#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames_io.hpp>

#include <boost/scoped_ptr.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

#include <kdl_parser/kdl_parser.hpp>

#include <urdf/model.h>

#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>

#include <cmath>
#include <ctime>
#include <time.h>

using namespace std;
using namespace Eigen;
using namespace KDL;

int number_of_chain = 5; // 5 finger

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

	ros::init(argc, argv, "Soft_Hand_Jacobian");	// ROS node
	ros::NodeHandle nh;

  std::string root_name = "right_hand_palm_link" ;
  std::string end_chain_name[number_of_chain] ;

  end_chain_name[0] = "right_hand_thumb_distal_link";
  end_chain_name[1] = "right_hand_index_distal_link";
  end_chain_name[2] = "right_hand_middle_distal_link";
  end_chain_name[3] = "right_hand_ring_distal_link";
  end_chain_name[4] = "right_hand_little_distal_link";

  double input_joint_thumb[] = { 0.2, 0.2, 0, 0, 0 };
  double input_joint_index[] = { 0.2, 0.2, 0, 0, 0, 0, 0 };
  double input_joint_middle[] = { 0.2, 0.2, 0, 0, 0, 0, 0 };
  double input_joint_ring[] = { 0.2, 0.2, 0, 0, 0, 0, 0 };
  double input_joint_little[] = { 0.2, 0.2, 0, 0, 0, 0, 0 };

  KDL::Tree hand_tree;

  KDL::Chain chain0_thumb;
  KDL::Chain chain1_index;
  KDL::Chain chain2_middle;
  KDL::Chain chain3_ring;
  KDL::Chain chain4_little;

  KDL::Jacobian jacobian0_thumb;
  KDL::Jacobian jacobian1_index;
  KDL::Jacobian jacobian2_middle;
  KDL::Jacobian jacobian3_ring;
  KDL::Jacobian jacobian4_little;

  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_0;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_1;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_2;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_3;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_4;

  std::string robot_desc_string;
  nh.param("robot_description", robot_desc_string, string());  // robot description is the name in the launch file
  if (!kdl_parser::treeFromString(robot_desc_string, hand_tree))
  {     ROS_ERROR("Failed to construct kdl tree");
        return false;       }

  hand_tree.getChain(root_name, end_chain_name[0], chain0_thumb);      // da root a leaf
  hand_tree.getChain(root_name, end_chain_name[1], chain1_index);
  hand_tree.getChain(root_name, end_chain_name[2], chain2_middle);
  hand_tree.getChain(root_name, end_chain_name[3], chain3_ring);
  hand_tree.getChain(root_name, end_chain_name[4], chain4_little);

  int dim_hand_tree = hand_tree.getNrOfJoints();  // 34

  cout << "number_of_joint : " << dim_hand_tree << endl;

  unsigned int nj_0 = chain0_thumb.getNrOfJoints();
  unsigned int nj_1 = chain1_index.getNrOfJoints();
  unsigned int nj_2 = chain2_middle.getNrOfJoints();
  unsigned int nj_3 = chain3_ring.getNrOfJoints();
  unsigned int nj_4 = chain4_little.getNrOfJoints();

  cout << "chain0_thumb - nj_0 = "   << nj_0 << endl;
  cout << "chain1_index - nj_1 = "   << nj_1 << endl;
  cout << "chain2_middle - nj_2 = "  << nj_2 << endl;
  cout << "chain3_ring - nj_3 = "    << nj_3 << endl;
  cout << "chain4_little - nj_4 = "  << nj_4 << endl;

  KDL::JntArray jointpositions_0 = JntArray(nj_0);    // thumb
  KDL::JntArray jointpositions_1 = JntArray(nj_1);    // forefinger
  KDL::JntArray jointpositions_2 = JntArray(nj_2);    // middlefinger
  KDL::JntArray jointpositions_3 = JntArray(nj_3);    // ringfinger
  KDL::JntArray jointpositions_4 = JntArray(nj_4);    // littlefinger

  for ( int i = 0 ; i < nj_0 ; i++ )
      jointpositions_0(i) = input_joint_thumb[i];

  for ( int i = 0 ; i < nj_1 ; i++ )
      jointpositions_1(i) = input_joint_index[i];

  for ( int i = 0 ; i < nj_2 ; i++ )
      jointpositions_2(i) = input_joint_middle[i];

  for ( int i = 0 ; i < nj_3 ; i++ )
      jointpositions_3(i) = input_joint_ring[i];

  for ( int i = 0 ; i < nj_4 ; i++ )
      jointpositions_4(i) = input_joint_little[i];

  // constructs the kdl solvers in non-realtime
  jnt_to_jac_solver_0.reset(new KDL::ChainJntToJacSolver(chain0_thumb));
  jnt_to_jac_solver_1.reset(new KDL::ChainJntToJacSolver(chain1_index));
  jnt_to_jac_solver_2.reset(new KDL::ChainJntToJacSolver(chain2_middle));
  jnt_to_jac_solver_3.reset(new KDL::ChainJntToJacSolver(chain3_ring));
  jnt_to_jac_solver_4.reset(new KDL::ChainJntToJacSolver(chain4_little));

  // resizes the joint state vectors in non-realtime
  jointpositions_0.resize(chain0_thumb.getNrOfJoints());
  jointpositions_1.resize(chain1_index.getNrOfJoints());
  jointpositions_2.resize(chain2_middle.getNrOfJoints());
  jointpositions_3.resize(chain3_ring.getNrOfJoints());
  jointpositions_4.resize(chain4_little.getNrOfJoints());

  jacobian0_thumb.resize(chain0_thumb.getNrOfJoints());
  jacobian1_index.resize(chain1_index.getNrOfJoints());
  jacobian2_middle.resize(chain2_middle.getNrOfJoints());
  jacobian3_ring.resize(chain3_ring.getNrOfJoints());
  jacobian4_little.resize(chain4_little.getNrOfJoints());

  // compute Jacobian in realtime
  jnt_to_jac_solver_0->JntToJac(jointpositions_0, jacobian0_thumb);
  jnt_to_jac_solver_1->JntToJac(jointpositions_1, jacobian1_index);
  jnt_to_jac_solver_2->JntToJac(jointpositions_2, jacobian2_middle);
  jnt_to_jac_solver_3->JntToJac(jointpositions_3, jacobian3_ring);
  jnt_to_jac_solver_4->JntToJac(jointpositions_4, jacobian4_little);

  std::vector<Eigen::MatrixXd>  Jacobian_finger;// KDL::Jacobian jacobian0_thumb;
                                                // KDL::Jacobian jacobian1_forefinger;
                                                // KDL::Jacobian jacobian2_middlefinger;
                                                // KDL::Jacobian jacobian3_ringfinger;
                                                // KDL::Jacobian jacobian4_littlefinger;

  Jacobian_finger.push_back(jacobian0_thumb.data);
  Jacobian_finger.push_back(jacobian1_index.data);
  Jacobian_finger.push_back(jacobian2_middle.data);
  Jacobian_finger.push_back(jacobian3_ring.data);
  Jacobian_finger.push_back(jacobian4_little.data);

  for(int n = 0 ; n < number_of_chain ; n++)
  {
    cout << " Jacobian_" << n << endl;
    cout << " --------------------------- " << endl;
    cout <<  Jacobian_finger[n] << endl;
    cout << " --------------------------- " << endl;
  }

	cout << " STAMPATO " << endl;
  cout << "   fine   " << endl;

	ros::spin();
	return 0;

}


#include <boost/graph/boyer_myrvold_planar_test.hpp>


#define BOOST_TEST_MODULE "test_3spn2_base_stacking_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/3SPN2/ThreeSPN2BaseStackingInteraction.hpp>
#include <mjolnir/input/read_units.hpp>

#include <random>

using parameter_set_to_test = std::tuple<
    mjolnir::ThreeSPN2BaseStackingPotentialParameter<double>,
    mjolnir::ThreeSPN2CBaseStackingPotentialParameter<double>
>;

BOOST_AUTO_TEST_CASE_TEMPLATE(ThreeSPN2BaseStackingInteraction_numerical_diff,
        ParameterSet, parameter_set_to_test)
{
    namespace test = mjolnir::test;
    mjolnir::LoggerManager::set_default_logger(
            "test_3spn2_base_stacking_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using interaction_type = mjolnir::ThreeSPN2BaseStackingInteraction<traits_type>;
    using potential_type   = mjolnir::ThreeSPN2BaseStackingPotential<real_type>;
    using base_stack_kind  = typename potential_type::base_stack_kind;

    {
        using unit_type = mjolnir::unit::constants<real_type>;
        using phys_type = mjolnir::physics::constants<real_type>;
        const std::string energy = "kcal/mol";
        const std::string length = "angstrom";
        phys_type::set_kB(phys_type::kB() * (unit_type::J_to_cal() / 1000.0) *
                          unit_type::avogadro_constant());
        phys_type::set_eps0(phys_type::eps0() * (1000.0 / unit_type::J_to_cal()) /
                            unit_type::avogadro_constant());
        phys_type::set_energy_unit(energy);

        phys_type::set_eps0(phys_type::eps0() / unit_type::m_to_angstrom());

        phys_type::set_m_to_length(unit_type::m_to_angstrom());
        phys_type::set_length_to_m(unit_type::angstrom_to_m());

        phys_type::set_L_to_volume(1e-3 * std::pow(unit_type::m_to_angstrom(), 3));
        phys_type::set_volume_to_L(1e+3 * std::pow(unit_type::angstrom_to_m(), 3));

        phys_type::set_length_unit(length);
    }

    //        SBi
    //     Si --> Bi
    //    /     `-^
    //   Pj theta | rij
    //    \       |
    //     Sj --- Bj
    //
    //  rij:
    //  1. (r < r0)
    //  2. (r0 < r)
    //  theta:
    //  1. theta < pi/2K
    //  2. pi/2K < theta < pi/K
    //  3. pi/K  < theta

    std::mt19937 mt(123456789);
    for(const auto bs_kind : {base_stack_kind::AA, base_stack_kind::AT,
                              base_stack_kind::AG, base_stack_kind::AC,
                              base_stack_kind::TA, base_stack_kind::TT,
                              base_stack_kind::TG, base_stack_kind::TC,
                              base_stack_kind::GA, base_stack_kind::GT,
                              base_stack_kind::GG, base_stack_kind::GC,
                              base_stack_kind::CA, base_stack_kind::CT,
                              base_stack_kind::CG, base_stack_kind::CC})
    {
        potential_type   potential(ParameterSet{});
        interaction_type interaction("none",
                std::vector<std::pair<std::array<std::size_t, 3>, base_stack_kind>>{
                    { {{0, 1, 2}}, bs_kind }
                }, potential_type(potential), {});

        system_type sys(3, boundary_type{});
        test::clear_everything(sys);
        sys.name(0)  = "Si";
        sys.name(1)  = "Bi";
        sys.name(2)  = "Bj";
        sys.group(0) = "DNA";
        sys.group(1) = "DNA";
        sys.group(2) = "DNA";

        potential.initialize(sys);
        interaction.initialize(sys);

        const auto theta0    = potential.theta_0(bs_kind);
        const auto pi_over_K = potential.pi_over_K_BS();
        const auto theta0_1  = theta0 + 0.2 * pi_over_K; //         dtheta < pi/2K
        const auto theta0_2  = theta0 + 0.7 * pi_over_K; // pi/2K < dtheta < pi/K
        const auto theta0_3  = theta0 + 1.2 * pi_over_K; // pi/K  < dtheta
        const auto r0_1      = potential.r0(bs_kind) - 0.2;
        const auto r0_2      = potential.r0(bs_kind) + 0.5;

        for(const auto r : {r0_1, r0_2})
        {
            for(const auto theta : {theta0_1, theta0_2, theta0_3})
            {
                BOOST_TEST_MESSAGE("======================================");
                BOOST_TEST_MESSAGE("r = " << r << ", theta = " << theta);

                for(std::size_t i=0; i<100; ++i)
                {
                    // generate particle configuration in the following way
                    //    y
                    // Bj ^
                    //  \ | theta0
                    // r0\|-.
                    // ---o-----o--> x
                    //  Bi      Si
                    //

                    sys.position(0) = coord_type(4.0, 0.0, 0.0); // Si
                    sys.position(1) = coord_type(0.0, 0.0, 0.0); // Bi
                    sys.position(2) = coord_type(r * std::cos(theta),
                                                 r * std::sin(theta), 0.0); // Bj

                    test::clear_force(sys);
                    test::apply_random_rotation(sys, mt);
                    test::apply_random_perturbation(sys, mt, 0.01);

                    constexpr real_type tol = 1e-4;
                    constexpr real_type dr  = 1e-5;

                    test::check_force(sys, interaction, tol, dr);
                    test::check_virial(sys, interaction, tol);
                    test::check_force_and_virial(sys, interaction, tol);
                    test::check_force_and_energy(sys, interaction, tol);
                    test::check_force_energy_virial(sys, interaction, tol);
                } // perturbation
            } // theta
        } // r
    }
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_EXPONENTIAL_FUNCTIONS_SCALAR_REALPOW_HPP_INCLUDED
#define NT2_EXPONENTIAL_FUNCTIONS_SCALAR_REALPOW_HPP_INCLUDED
#include <nt2/exponential/functions/realpow.hpp>
#include <boost/assert.hpp>
#include <nt2/include/functions/scalar/is_flint.hpp>
#include <nt2/include/functions/scalar/is_nltz.hpp>
#include <nt2/include/functions/scalar/pow.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( realpow_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                              (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      BOOST_ASSERT_MSG( is_nltz(a0) || is_flint(a1),
                        "realpow cannot produce complex result." );
      return  pow(a0, a1);
    }
  };
} }

#endif


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/range/segmented_input_range.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-std

    Computes the sum over elements in the given Contiguous Range @range{first,last} and
    the initial value @c init.

    \notebox{The summation order can be different from the order of a sequential summation
            , thus leading to different results.
            }

    @par Example:
    @snippet reduce.simple.cpp reduce-simple
    Possible output:
    @code
    SIMD reduce     : 45
    STD  accumulate : 45
    @endcode

    @param first  Beginning of the range of elements to sum
    @param last   End of the range of elements to sum
    @param init   Initial value of the sum

    @return The sum of the given value and elements in the given range.
  **/
  template<typename T> T reduce(T const* first, T const* last, T init)
  {
    pack<T> acc(0);
    auto pr = segmented_input_range(first,last);

    for( auto const& e : std::get<0>(pr) ) init += e;
    for( auto const& e : std::get<1>(pr) ) acc  += e;
    for( auto const& e : std::get<2>(pr) ) init += e;

    return init + sum(acc);
  }

  /*!
    @ingroup group-std

    Computes the generalized sum of the elements in the given Contiguous Range @range{first,last}
    over the binary functions @c binop and @c reduce, using @c init as the initial value.

    While @c binop is applied over the result of dereferencing the input pointers, @c reduce is to
    be used in the final reduction of the SIMD part of the generalized sum.

    \notebox{The summation order can be different from the order of a sequential summation
            , thus leading to different results.
            }

    @par Example:
    @snippet reduce.phases.cpp reduce-phases
    Possible output:
    @code
    SIMD reduce     : 285
    @endcode

    @param first    Beginning of the range of elements to sum
    @param last     End of the range of elements to sum
    @param init     Initial value of the reduction
    @param binop    Binary function object that will be applied in unspecified order to the
                    result of dereferencing the input pointers, the results of other @c binop
                    and @ init.
    @param neutral  Value containing the neutral element of @c binop
    @param reduce   Binary function object that will be applied to complete the reduction

    @return The generalized sum of the given value and elements in the given range over @ binop.
  **/
  template<typename T, typename U, typename F, typename N, typename G>
  U reduce( T const* first, T const* last, U init, F binop, N neutral, G reduce )
  {
    pack<U> acc(neutral);
    auto pr = segmented_input_range(first,last);

    for( auto const& e : std::get<0>(pr) ) init = binop(init,e);
    for( auto const& e : std::get<1>(pr) ) acc  = binop(acc,e);
    for( auto const& e : std::get<2>(pr) ) init = binop(init,e);
    for( U           e : acc)              init = reduce(init,e);

    return init;
  }

  /*!
    @ingroup group-std

    Computes the generalized sum of the elements in the given Contiguous Range @range{first,last}
    over the binary function @c binop, using @c init as the initial value.

    @c binop is applied over the result of dereferencing the input pointers and in the final
    reduction of the SIMD part of the generalized sum.

    \notebox{The summation order can be different from the order of a sequential summation
            , thus leading to different results.
            }

    @par Example:
    @snippet reduce.phase.cpp reduce-phase
    Possible output:
    @code
    SIMD reduce     : 362880
    @endcode

    @param first    Beginning of the range of elements to sum
    @param last     End of the range of elements to sum
    @param init     Initial value of the reduction
    @param binop    Binary function object that will be applied in unspecified order to the
                    result of dereferencing the input pointers, the results of other @c binop
                    and @c init.
    @param neutral  Value containing the neutral element of @c binop

    @return The sum of the given value and elements in the given range.
  **/
  template<typename T, typename U, typename F, typename N>
  U reduce(T const* first, T const* last, U init, F binop, N neutral)
  {
    return reduce(first,last,init,binop,neutral,binop);
  }
} }

#endif


#define BOOST_TEST_MODULE "test_contact_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/local/ContactInteraction.hpp>
#include <mjolnir/forcefield/local/GaussianPotential.hpp>
#include <mjolnir/util/make_unique.hpp>

BOOST_AUTO_TEST_CASE(Contact_numerical_difference)
{
    namespace test = mjolnir::test;

    mjolnir::LoggerManager::set_default_logger("test_contact_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coord_type       = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::GaussianPotential<real_type>;
    using interaction_type = mjolnir::ContactInteraction<traits_type, potential_type>;

    const real_type k(1.0);
    const real_type native(std::sqrt(3.0));
    const real_type sigma(0.15);

    potential_type   potential(k, sigma, native);
    interaction_type interaction("none", {{ {{0,1}}, potential}});

    std::mt19937 mt(123456789);

    for(std::size_t i = 0; i < 1000; ++i)
    {
        system_type sys(2, boundary_type{});
        test::clear_everything(sys);

        sys.position(0) = coord_type( 0.0, 0.0, 0.0);
        sys.position(1) = coord_type( 1.0, 1.0, 1.0);

        test::apply_random_rotation(sys, mt);
        test::apply_random_perturbation(sys, mt, 0.01);

        constexpr real_type tol = 1e-6;
        constexpr real_type dr  = 1e-6;

        test::check_force(sys, interaction, tol, dr);
        test::check_virial(sys, interaction, tol);
        test::check_force_and_virial(sys, interaction, tol);
        test::check_force_and_energy(sys, interaction, tol);
        test::check_force_energy_virial(sys, interaction, tol);
    }
}


/**
 * \ file GainSwellFilter.cpp
 */

#include <ATK/Dynamic/GainSwellFilter.h>

#include <ATK/Core/InPointerFilter.h>
#include <ATK/Core/OutPointerFilter.h>
#include <ATK/Core/Utilities.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#include <boost/math/constants/constants.hpp>

constexpr gsl::index PROCESSSIZE = 64;

BOOST_AUTO_TEST_CASE( GainSwellFilter_softness_test )
{
  ATK::GainFilter<ATK::GainSwellFilter<double>> filter;
  filter.set_softness(0.5);
  BOOST_CHECK_EQUAL(filter.get_softness(), 0.5);
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_softness_range_test )
{
  ATK::GainFilter<ATK::GainSwellFilter<double>> filter;
  BOOST_CHECK_THROW(filter.set_softness(-0.000001), ATK::RuntimeError);
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(10);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_0_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 0;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(1, outdata[i], 0.1); // if input is zero, we still need a gain of 1 to have a progression of 1 for values < threshold
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_threshold_05_ratio_2_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(2);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.490775019, outdata[i], 0.1);
  }
}

BOOST_AUTO_TEST_CASE( GainSwellFilter_const_1_threshold_05_ratio_4_test )
{
  std::array<double, PROCESSSIZE> data;
  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    data[i] = 1;
  }

  ATK::InPointerFilter<double> generator(data.data(), 1, PROCESSSIZE, false);
  generator.set_output_sampling_rate(48000);

  std::array<double, PROCESSSIZE> outdata;

  ATK::GainFilter<ATK::GainSwellFilter<double>> filter(1);
  filter.set_input_sampling_rate(48000);
  filter.set_input_port(0, &generator, 0);
  filter.set_threshold(0.5);
  filter.set_ratio(4);
  filter.set_softness(1);

  ATK::OutPointerFilter<double> output(outdata.data(), 1, PROCESSSIZE, false);
  output.set_input_sampling_rate(48000);
  output.set_input_port(0, &filter, 0);

  output.process(PROCESSSIZE);

  for(gsl::index i = 0; i < PROCESSSIZE; ++i)
  {
    BOOST_REQUIRE_CLOSE(0.240860119, outdata[i], 0.1);
  }
}