/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018. Toyota Research Institute
 *  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 CNRS-LAAS and AIST 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 OWNER 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.
 */

/** @author Hongkai Dai */
/**
 * Test the signed distance query between two convex objects, when calling with
 * solver = GST_LIBCCD.
 */
#include "fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h"

#include <gtest/gtest.h>
#include <Eigen/Dense>

#include "fcl/narrowphase/detail/gjk_solver_libccd.h"

namespace fcl {
namespace detail {
// Given two spheres, sphere 1 has radius1, and centered at point A, whose
// position is p_F1 measured and expressed in frame F; sphere 2 has radius2,
// and centered at point B, whose position is p_F2 measured and expressed in
// frame F. Computes the signed distance between the two spheres.
// We use the monogram notation on spatial vectors. The monogram notation is
// explained in
// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
template <typename S>
S ComputeSphereSphereDistance(S radius1, S radius2, const Vector3<S>& p_F1,
                              const Vector3<S>& p_F2) {
  S min_distance = (p_F1 - p_F2).norm() - radius1 - radius2;
  return min_distance;
}

template <typename S>
void TestSphereToSphereGJKSignedDistance(S radius1, S radius2,
                                         const Vector3<S>& p_F1,
                                         const Vector3<S>& p_F2,
                                         S solver_tolerance, S test_tol,
                                         S min_distance_expected) {
  // Test if GJKSignedDistance computes the right distance. Here we used sphere
  // to sphere as the geometries. The distance between sphere and sphere should
  // be computed using distance between primitives, instead of the GJK
  // algorithm. But here we choose spheres for simplicity.
  //
  // There are two tolerances: the solver_tolerance, and the test_tol.
  // solver_tolerance is used to determine when the algorithm terminates. If
  // the objects are separated, the GJK algorithm terminates when the change of
  // distance is below solver_tolerance, which does NOT mean that the separation
  // distance computed by GJK is within solver_tolerance to the true distance,
  // so we use test_tol as a separate tolerance to check the accuracy of the
  // computed distance.
  // If the objects penetrate, the EPA algorithm terminates when the difference
  // between the upper bound of penetration depth and the lower bound
  // is below solver_tolerance, which means that the EPA computed penetration
  // depth is within solver_tolerance to the true depth.
  // The tolerance is explained in GJKSignedDistance() in gjk_libccd-inl.h
  fcl::Sphere<S> s1(radius1);
  fcl::Sphere<S> s2(radius2);
  fcl::Transform3<S> tf1, tf2;
  tf1.setIdentity();
  tf2.setIdentity();
  tf1.translation() = p_F1;
  tf2.translation() = p_F2;
  void* o1 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s1, tf1);
  void* o2 = GJKInitializer<S, fcl::Sphere<S>>::createGJKObject(s2, tf2);

  S dist;
  // N1 and N2 are the witness points on sphere 1 and 2 respectively.
  Vector3<S> p_FN1, p_FN2;
  GJKSolver_libccd<S> gjkSolver;
  gjkSolver.distance_tolerance = solver_tolerance;
  bool res = GJKSignedDistance(
      o1, detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(), o2,
      detail::GJKInitializer<S, Sphere<S>>::getSupportFunction(),
      gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
      &p_FN1, &p_FN2);

  EXPECT_EQ(res, min_distance_expected >= 0);

  EXPECT_NEAR(dist, min_distance_expected, test_tol);
  // Now check if the distance between N1 and N2 matches with dist, they should
  // match independent of what solver_tolerance we choose.
  EXPECT_NEAR((p_FN1 - p_FN2).norm(), std::abs(dist),
              fcl::constants<S>::eps_78());
  // Check if p1 is on the boundary of sphere 1, and p2 is on the boundary of
  // sphere 2.
  EXPECT_NEAR((p_FN1 - p_F1).norm(), radius1, test_tol);
  EXPECT_NEAR((p_FN2 - p_F2).norm(), radius2, test_tol);
  // The witness points N1 and N2 are defined as by shifting geometry B with
  // the vector N1 - N2, the shifted geometry B' and A are touching. Hence
  // if we shift sphere 1 by p_FN2 - p_FN1, then the two spheres should be
  // touching. The shifted sphere is centered at p_F1 + p_FN2 - p_FN1.
  EXPECT_NEAR((p_F1 + p_FN2 - p_FN1 - p_F2).norm(), radius1 + radius2,
              test_tol);
  // Note that we do not check the computed witness points to the true witness
  // points. There are two reasons
  // 1. Generally, the witness points are NOT guaranteed to be unique (consider
  // plane-to-plane contact).
  // 2. Even if there are unique witness points, it is hard to infer the bounds
  // on the computed witness points, based on the tolerance of the solver. This
  // bounds depend on the curvature of the geometries near the contact region,
  // and we do not have a general approach to compute the bound for generic
  // geometries.
  // On the other hand, for sphere-sphere contact, it is possible to compute
  // the bounds, since the witness points are unique, and the curvature is
  // constant. For the moment, we are satisfied with the test above. If the
  // future maintainer wants to improve this test, he/she might compute these
  // bounds for the sphere-sphere case.

  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
  GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
}

template <typename S>
struct SphereSpecification {
  SphereSpecification<S>(S radius_, const Vector3<S>& center_)
      : radius{radius_}, center{center_} {}
  S radius;
  Vector3<S> center;
};

template <typename S>
void TestNonCollidingSphereGJKSignedDistance() {
  std::vector<SphereSpecification<S>> spheres;
  spheres.emplace_back(0.5, Vector3<S>(0, 0, -1.2));
  spheres.emplace_back(0.5, Vector3<S>(1.25, 0, 0));
  spheres.emplace_back(0.3, Vector3<S>(-0.2, 0, 0));
  spheres.emplace_back(0.4, Vector3<S>(-0.2, 0, 1.1));
  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
      if ((spheres[i].center - spheres[j].center).norm() >
          spheres[i].radius + spheres[j].radius) {
        // Not in collision.
        for (const S solver_tolerance : {S(1e-4), S(1e-5), S(1e-6)}) {
          const S min_distance_expected =
              ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius,
                                          spheres[i].center, spheres[j].center);
          // When the change of distance is below solver_tolerances[k], it does
          // not mean that the error in separating distance is below
          // solver_tolerances[k]. Empirically we find the error is less than 10
          // * solver_tolerances[k], but there is no proof.
          TestSphereToSphereGJKSignedDistance<S>(
              spheres[i].radius, spheres[j].radius, spheres[i].center,
              spheres[j].center, solver_tolerance, 10 * solver_tolerance,
              min_distance_expected);
        }
      } else {
        GTEST_FAIL() << "The two spheres collide."
                     << "\nSpheres[" << i << "] with radius "
                     << spheres[i].radius << ", centered at "
                     << spheres[i].center.transpose() << "\nSpheres[" << j
                     << "] with radius " << spheres[j].radius
                     << ", centered at " << spheres[j].center.transpose()
                     << "\n";
      }
    }
  }
}

template <typename S>
void TestCollidingSphereGJKSignedDistance() {
  std::vector<SphereSpecification<S>> spheres;
  spheres.emplace_back(0.5, Vector3<S>(0, 0, 0));
  spheres.emplace_back(0.5, Vector3<S>(0.75, 0, 0));
  spheres.emplace_back(0.3, Vector3<S>(0.2, 0, 0));
  spheres.emplace_back(0.4, Vector3<S>(0.2, 0, 0.4));
  for (int i = 0; i < static_cast<int>(spheres.size()); ++i) {
    for (int j = i + 1; j < static_cast<int>(spheres.size()); ++j) {
      if ((spheres[i].center - spheres[j].center).norm() <
          spheres[i].radius + spheres[j].radius) {
        // colliding
        const S min_distance_expected =
            ComputeSphereSphereDistance(spheres[i].radius, spheres[j].radius,
                                        spheres[i].center, spheres[j].center);
        for (const S solver_tolerance : {S(1E-4), S(1E-5), S(1E-6)}) {
          // For colliding spheres, the solver_tolerance is the bound on the
          // error relative to the true answer.
          TestSphereToSphereGJKSignedDistance<S>(
              spheres[i].radius, spheres[j].radius, spheres[i].center,
              spheres[j].center, solver_tolerance, solver_tolerance,
              min_distance_expected);
        }
      } else {
        GTEST_FAIL() << "The two spheres failed to collide."
                     << "\nSpheres[" << i << "] with radius "
                     << spheres[i].radius << ", centered at "
                     << spheres[i].center.transpose() << "\nSpheres[" << j
                     << "] with radius " << spheres[j].radius
                     << ", centered at " << spheres[j].center.transpose()
                     << "\n";
      }
    }
  }
}

GTEST_TEST(FCL_GJKSignedDistance, sphere_sphere) {
  TestNonCollidingSphereGJKSignedDistance<double>();
  TestNonCollidingSphereGJKSignedDistance<float>();
  TestCollidingSphereGJKSignedDistance<double>();
  TestCollidingSphereGJKSignedDistance<float>();
}

//----------------------------------------------------------------------------
//                 Box test
// Given two boxes, we can perturb the pose of one box so the boxes are
// penetrating, touching or separated.
template <typename S>
struct BoxSpecification {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  BoxSpecification(const fcl::Vector3<S>& m_size) : size(m_size) {
    X_FB.setIdentity();
  }
  fcl::Vector3<S> size;
  fcl::Transform3<S> X_FB;
};

template <typename S>
void TestBoxesInFrameF(const Transform3<S>& X_WF) {
  const fcl::Vector3<S> box1_size(1, 1, 1);
  const fcl::Vector3<S> box2_size(0.6, 0.8, 1);
  // Put the two boxes on the xy plane of frame F.
  // B1 is the frame rigidly attached to box 1, B2 is the frame rigidly
  // attached to box 2. W is the world frame. F is a frame fixed to the world.
  // X_FB1 is the pose of box 1 expressed and measured in frame F, X_FB2 is the
  // pose of box 2 expressed and measured in frame F.
  fcl::Transform3<S> X_FB1, X_FB2;
  // Box 1 is fixed.
  X_FB1.setIdentity();
  X_FB1.translation() << 0, 0, 0.5;

  // First fix the orientation of box 2, such that one of its diagonal (the
  // one connecting the vertex (0.3, -0.4, 1) and (-0.3, 0.4, 1) is parallel to
  // the x axis in frame F. If we move the position of box 2, we get different
  // signed distance.
  X_FB2.setIdentity();
  X_FB2.linear() << 0.6, -0.8, 0, 0.8, 0.6, 0, 0, 0, 1;

  // p_xy_FN1 is the xy position of point N1 (the deepest penetration point on
  // box 1) measured and expressed in the frame F.
  // p_xy_FN2 is the xy position of point N2 (the deepest penetration point on
  // box 2) measured and expressed in the frame F.
  auto CheckDistance = [&box1_size, &box2_size, &X_FB1, &X_WF](
      const Transform3<S>& X_FB2, S distance_expected,
      const Vector2<S>& p_xy_FN1_expected, const Vector2<S>& p_xy_FN2_expected,
      S solver_distance_tolerance, S test_distance_tolerance,
      S test_witness_tolerance) {
    const fcl::Transform3<S> X_WB1 = X_WF * X_FB1;
    const fcl::Transform3<S> X_WB2 = X_WF * X_FB2;
    fcl::Box<S> box1(box1_size);
    fcl::Box<S> box2(box2_size);
    void* o1 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box1, X_WB1);
    void* o2 = GJKInitializer<S, fcl::Box<S>>::createGJKObject(box2, X_WB2);
    S dist;
    Vector3<S> p_WN1, p_WN2;
    GJKSolver_libccd<S> gjkSolver;
    gjkSolver.distance_tolerance = solver_distance_tolerance;
    bool res = GJKSignedDistance(
        o1, detail::GJKInitializer<S, Box<S>>::getSupportFunction(), o2,
        detail::GJKInitializer<S, Box<S>>::getSupportFunction(),
        gjkSolver.max_distance_iterations, gjkSolver.distance_tolerance, &dist,
        &p_WN1, &p_WN2);

    // It is unclear how FCL handles touching contact. It could return either
    // true or false for touching contact. So, we ignore the condition where
    // expected distance is zero.
    if (distance_expected < 0) {
      EXPECT_FALSE(res);
    } else if (distance_expected > 0) {
      EXPECT_TRUE(res);
    }

    EXPECT_NEAR(dist, distance_expected, test_distance_tolerance);
    const Vector3<S> p_FN1 =
        X_WF.linear().transpose() * (p_WN1 - X_WF.translation());
    const Vector3<S> p_FN2 =
        X_WF.linear().transpose() * (p_WN2 - X_WF.translation());

    EXPECT_TRUE(p_FN1.template head<2>().isApprox(p_xy_FN1_expected,
                                                  test_witness_tolerance));
    EXPECT_TRUE(p_FN2.template head<2>().isApprox(p_xy_FN2_expected,
                                                  test_witness_tolerance));
    // The z height of the closest points should be the same.
    EXPECT_NEAR(p_FN1(2), p_FN2(2), test_witness_tolerance);
    // The closest point is within object A/B, so the z height should be
    // within [0, 1]
    EXPECT_GE(p_FN1(2), 0);
    EXPECT_GE(p_FN2(2), 0);
    EXPECT_LE(p_FN1(2), 1);
    EXPECT_LE(p_FN2(2), 1);

    GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o1);
    GJKInitializer<S, fcl::Sphere<S>>::deleteGJKObject(o2);
  };

  auto CheckBoxEdgeBoxFaceDistance = [&CheckDistance](
      const Transform3<S>& X_FB2, S solver_distance_tolerance,
      S test_distance_tolerance, S test_witness_tolerance) {
    const double expected_distance = -X_FB2.translation()(0) - 1;
    CheckDistance(
        X_FB2, expected_distance, Vector2<S>(-0.5, X_FB2.translation()(1)),
        Vector2<S>(X_FB2.translation()(0) + 0.5, X_FB2.translation()(1)),
        solver_distance_tolerance, test_distance_tolerance,
        test_witness_tolerance);
  };

  //---------------------------------------------------------------
  //                      Touching contact
  // Test with different solver distance tolerances.
  std::vector<S> solver_tolerances = {S(1E-4), fcl::constants<S>::eps_12(),
                                      fcl::constants<S>::eps_34(),
                                      fcl::constants<S>::eps_78()};
  for (int i = 0; i < static_cast<int>(solver_tolerances.size()); ++i) {
    const S solver_distance_tolerance = solver_tolerances[i];
    // For touching contact, FCL might call either GJK or EPA algorithm. When it
    // calls GJK algorithm, there is no theoretical guarantee, on how the
    // distance error is related to solver's distance_tolerance.
    // Empirically we find 10x is reasonable.
    const S test_distance_tolerance = 10 * solver_distance_tolerance;
    const S test_witness_tolerance = test_distance_tolerance;
    // An edge of box 2 is touching a face of box 1
    X_FB2.translation() << -1, 0, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);

    // The touching face on box 1 is parallel to the y axis, so shifting box 2
    // on y axis still gives touching contact. Shift box 2 on y axis by 0.1m.
    X_FB2.translation() << -1, 0.1, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);

    // Shift box 2 on y axis by -0.1m.
    X_FB2.translation() << -1, -0.1, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);
    // TODO(hongkai.dai@tri.global): Add other touching contact cases, including
    // face-face, face-vertex, edge-edge, edge-vertex and vertex-vertex.
  }

  //--------------------------------------------------------------
  //                      Penetrating contact
  // An edge of box 2 penetrates into a face of box 1
  for (int i = 0; i < static_cast<int>(solver_tolerances.size()); ++i) {
    const S solver_distance_tolerance = solver_tolerances[i];
    // For penetrating contact, FCL calls EPA algorithm. When the solver
    // terminates, the computed distance should be within
    // solver.distance_tolerance to the actual distance.
    const S test_distance_tolerance = solver_distance_tolerance;
    const S test_witness_tolerance = test_distance_tolerance;

    X_FB2.translation() << -0.9, 0, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);

    // Shift box 2 on y axis by 0.1m.
    X_FB2.translation() << -0.9, 0.1, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);

    // Shift box 2 on y axis by -0.05m.
    X_FB2.translation() << -0.9, -0.05, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);

    // Shift box 2 on y axis by -0.1m.
    X_FB2.translation() << -0.9, -0.1, 0.5;
    CheckBoxEdgeBoxFaceDistance(X_FB2, solver_distance_tolerance,
                                test_distance_tolerance,
                                test_witness_tolerance);
  }
}

template <typename S>
void TestBoxes() {
  // Frame F coincides with the world frame W.
  Transform3<S> X_WF;
  X_WF.setIdentity();
  TestBoxesInFrameF(X_WF);

  // Frame F is shifted from the world frame W.
  X_WF.translation() << 0, 0, 1;
  TestBoxesInFrameF(X_WF);

  X_WF.translation() << 0, 1, 0;
  TestBoxesInFrameF(X_WF);

  X_WF.translation() << 1, 0, 0;
  TestBoxesInFrameF(X_WF);

  // Frame F is rotated from the world frame W.
  X_WF.setIdentity();
  const S kPi = fcl::constants<S>::pi();
  X_WF.linear() =
      Eigen::AngleAxis<S>(0.1 * kPi, Vector3<S>::UnitZ()).toRotationMatrix();
  TestBoxesInFrameF(X_WF);

  // TODO(hongkai.dai): This test exposes an error in simplexToPolytope4, that
  // the initial simplex can be degenerate. Should add the special case on
  // degenerate simplex in simplexToPolytope4.
  /*X_WF.translation() << 0, 0, 0;
  X_WF.linear() =
      Eigen::AngleAxis<S>(0.1 * kPi, Vector3<S>(1.0 / 3, 2.0 / 3, -2.0 / 3))
          .toRotationMatrix();
  TestBoxesInFrameF(X_WF);*/

  // Frame F is rotated and shifted from the world frame W.
  X_WF.translation() << 0.1, 0.2, 0.3;
  TestBoxesInFrameF(X_WF);
}

GTEST_TEST(FCL_GJKSignedDistance, box_box) {
  TestBoxes<double>();
  TestBoxes<float>();
}
}  // namespace detail
}  // namespace fcl

//==============================================================================
int main(int argc, char* argv[]) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


#define BOOST_TEST_MAIN// Copyright John Maddock 2008
//  (C) Copyright Paul A. Bristow 2011 (added tests for changesign)
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/special_functions/sign.hpp>

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp> // Boost.Test
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>

#include <iostream>
   using std::cout;
   using std::endl;
   using std::setprecision;

template <class RealType>
void test_spots(RealType /*T*/, const char* /*type_name*/)
{
   // Basic sanity checks.
   RealType a = 0;
   RealType b = 1;
   RealType c = -1;
   BOOST_CHECK_EQUAL((boost::math::signbit)(a), 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), 0);
   BOOST_CHECK_EQUAL((boost::math::changesign)(b), RealType(-1));
   BOOST_CHECK_EQUAL((boost::math::changesign)(c), RealType(+1));
   BOOST_CHECK_EQUAL((boost::math::changesign)(a), RealType(0));

   // Compare to formula for changsign(x) = copysign(x, signbit(x) ? 1.0 : -1.0)
   BOOST_CHECK_EQUAL((boost::math::changesign)(b),
      (boost::math::copysign)(b, (boost::math::signbit)(b) ? RealType(1.) : RealType(-1.) ));

   BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(1));
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
   a = 1;
   BOOST_CHECK_EQUAL((boost::math::signbit)(a), 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(1));
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
   a = -1;
   BOOST_CHECK((boost::math::signbit)(a) != 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), -1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(-1));
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(-1));
   a = boost::math::tools::max_value<RealType>();
   BOOST_CHECK_EQUAL((boost::math::signbit)(a), 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(1));
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
   a = -boost::math::tools::max_value<RealType>();
   BOOST_CHECK((boost::math::signbit)(a) != 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), -1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(-1));
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(-1));

   if(std::numeric_limits<RealType>::has_infinity)
   {
      a = std::numeric_limits<RealType>::infinity();
      BOOST_CHECK_EQUAL((boost::math::signbit)(a), 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
      BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(1));
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
      BOOST_CHECK_EQUAL((boost::math::changesign)(a), -a);

      a = -std::numeric_limits<RealType>::infinity();
      BOOST_CHECK((boost::math::signbit)(a) != 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(a), -1);
      BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(-1));
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(-1));
      BOOST_CHECK_EQUAL((boost::math::changesign)(a), -a);
   }
#if !defined(__SUNPRO_CC) && !defined(BOOST_INTEL)
   if(std::numeric_limits<RealType>::has_quiet_NaN)
   {
      a = std::numeric_limits<RealType>::quiet_NaN();
      BOOST_CHECK_EQUAL((boost::math::signbit)(a), 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
      BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(1));
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
      // BOOST_CHECK_EQUAL((boost::math::changesign)(a), -a); // NaN comparison fails always!
      BOOST_CHECK((boost::math::signbit)((boost::math::changesign)(a)) != 0);

      a = -std::numeric_limits<RealType>::quiet_NaN();
      BOOST_CHECK((boost::math::signbit)(a) != 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(a), -1);
      BOOST_CHECK_EQUAL((boost::math::copysign)(b, a), RealType(-1));
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(-1));
      //BOOST_CHECK_EQUAL((boost::math::changesign)(a), -a); // NaN comparison fails always!
      BOOST_CHECK_EQUAL((boost::math::signbit)((boost::math::changesign)(a)), 0);

   }
#endif
   //
   // Try some extreme values:
   //
   a = boost::math::tools::min_value<RealType>();
   b = -a;
   c = -1;
   BOOST_CHECK((boost::math::signbit)(a) == 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
   BOOST_CHECK((boost::math::signbit)(b) != 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(b), -1);
   c = 1;
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, b), RealType(-1));
   //
   // try denormalised values:
   //
   a /= 4;
   if(a != 0)
   {
      b = -a;
      c = -1;
      BOOST_CHECK((boost::math::signbit)(a) == 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
      BOOST_CHECK((boost::math::signbit)(b) != 0);
      BOOST_CHECK_EQUAL((boost::math::sign)(b), -1);
      c = 1;
      BOOST_CHECK_EQUAL((boost::math::copysign)(c, b), RealType(-1));
   }
   a = boost::math::tools::max_value<RealType>() / 2;
   b = -a;
   c = -1;
   BOOST_CHECK((boost::math::signbit)(a) == 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(a), 1);
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, a), RealType(1));
   BOOST_CHECK((boost::math::signbit)(b) != 0);
   BOOST_CHECK_EQUAL((boost::math::sign)(b), -1);
   c = 1;
   BOOST_CHECK_EQUAL((boost::math::copysign)(c, b), RealType(-1));
}

BOOST_AUTO_TEST_CASE( test_main )
{
   // Basic sanity-check spot values.
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
   test_spots(0.0F, "float"); // Test float. OK at decdigits = 0 tolerance = 0.0001 %
   test_spots(0.0, "double"); // Test double. OK at decdigits 7, tolerance = 1e07 %
   // long double support for the sign functions is considered "core" so we always test it
   // even when long double support is turned off via BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L, "long double"); // Test long double.
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_spots(boost::math::concepts::real_concept(0), "real_concept"); // Test real_concept.
#endif

} // BOOST_AUTO_TEST_CASE( test_main )


// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file image_calc.cc
///

// Apply specified arithmetic operations to given input images and save
// the output with desired pixel type.

#include <vw/Core/FundamentalTypes.h>
#include <vw/Core/Log.h>
#include <vw/Image/Algorithms.h>
#include <vw/Image/ImageIO.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/Image/Manipulation.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/PixelMath.h>
#include <vw/Image/Statistics.h>
#include <vw/Image/Filter.h>
#include <vw/FileIO/DiskImageView.h>

#include <asp/Core/Common.h>
#include <asp/Core/Macros.h>

#include <vector>

#include <boost/program_options.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
namespace po = boost::program_options;

using namespace vw;

/**
  Program implementing simple calculator functionality for large images.

*/

namespace b_s = boost::spirit;

//================================================================================
// - The operations tree structure

/*
  Boost::Spirit requires the use of some specific Boost types in order to
   store the parsed information.
*/

enum OperationType {
  OP_pass,
  // UNARY operations
  OP_number,   // This is a leaf node containing a number.
  OP_variable,
  OP_negate,
  OP_abs,
  // BINARY operations
  OP_add,
  OP_subtract,
  OP_divide,
  OP_multiply,
  OP_power,
  // MULTI operations
  OP_min,
  OP_max
};

std::string getTagName(const OperationType o) {

  switch(o) {

    case OP_pass:     return "PASS";
    case OP_number:   return "NUMBER";
    case OP_variable: return "VARIABLE";
    case OP_negate:   return "NEGATE";
    case OP_abs:      return "ABS";
    case OP_add:      return "ADD";
    case OP_subtract: return "SUBTRACT";
    case OP_divide:   return "DIVIDE";
    case OP_multiply: return "MULTIPLY";
    case OP_power:    return "POWER";
    case OP_min:      return "MIN";
    case OP_max:      return "MAX";
    default:          return "ERROR";
  }
}

const int TAB_SIZE = 4;
void tab(int indent) {

  for (int i = 0; i < indent; ++i)
    std::cout << ' ';
}

// TODO: Are there pixel functions for these?
template <typename T>
T manual_min(const std::vector<T> &vec) {

  T minVal = vec[0];
  for (size_t i=1; i<vec.size(); ++i)
    if (vec[i] < minVal)
      minVal = vec[i];
  return minVal;
}
template <typename T>
T manual_max(const std::vector<T> &vec) {

  T maxVal = vec[0];
  for (size_t i=1; i<vec.size(); ++i)
    if (vec[i] > maxVal)
      maxVal = vec[i];
  return maxVal;
}

// This type represents an operation performed on one or more inputs.
struct calc_operation {

    OperationType               opType; // The operation to be performed on the children
    double value; // If this is a leaf node, the number is stored here.  Ignored unless OP_number.
    int varName;
    std::vector<calc_operation> inputs; // The inputs to the operation

    calc_operation() : opType(OP_pass) {}

    /// Recursive function to print out the contents of this object
    void print(const int indent=0) {

      if (opType == OP_number) {
        tab(indent);
        std::cout << "Node: " << getTagName(opType) << " = " << value << std::endl;
      }
      else if (opType == OP_variable) {
        tab(indent);
        std::cout << "Node: " << getTagName(opType) << " = " << varName << std::endl;
      }
      else {
        std::cout << std::endl;
        tab(indent);
        std::cout << "tag: " << getTagName(opType) << std::endl;
        tab(indent);
        std::cout << "value: " << value << std::endl;
        tab(indent);
        std::cout << "varName: " << varName << std::endl;
        tab(indent);
        std::cout << '{' << std::endl;

        for (size_t i=0; i<inputs.size(); ++i)
          inputs[i].print(indent+TAB_SIZE);

        tab(indent);
        std::cout << '}' << std::endl;
      }

    }

    /// Recursive function to eliminate extraneous nodes created by our parsing technique
    void clearEmptyNodes() {
      // Recursively call this function on all inputs
      for (size_t i=0; i<inputs.size(); ++i)
        inputs[i].clearEmptyNodes();

      if (opType != OP_pass)
        return;

      // Check for errors
      if (inputs.size() != 1) {
        std::cout << "ERROR: pass node with " << inputs.size() << " Nodes!\n";
        return;
      }

      // Replace this node with its input node
      value   = inputs[0].value;
      opType  = inputs[0].opType;
      varName = inputs[0].varName;
      std::vector<calc_operation> temp = inputs[0].inputs;
      inputs = temp;
    }

    /// Apply the operation tree to the input parameters and return a result
    template <typename T>
    T applyOperation(const std::vector<T> &params) const {
      // Get the results from each input node.
      // - This is a recursive call.
      const size_t numInputs = inputs.size();
      std::vector<T> inputResults(numInputs);
      for (size_t i=0; i<numInputs; ++i)
        inputResults[i] = inputs[i].applyOperation(params);

      // Now perform the operation for this node
      switch(opType) {
        // Unary
        case OP_number:   return T(value);
        case OP_variable: if (varName >= static_cast<int>(params.size()))
          vw_throw(ArgumentErr()
                   << "Unrecognized variable input. Note that the first variable is var_0.\n");
          return params[varName];
        if (numInputs < 1)
          vw_throw(LogicErr() << "Insufficient inputs for this operation.\n");
        case OP_negate:   return T(-1 * inputResults[0]);
        case OP_abs:      return T(std::abs(inputResults[0])); // regular abs casts to integer.
        // Binary
        if (numInputs < 2)
          vw_throw(LogicErr() << "Insufficient inputs for this operation.\n");
        case OP_add:      return (inputResults[0] + inputResults[1]);
        case OP_subtract: return (inputResults[0] - inputResults[1]);
        case OP_divide:   return (inputResults[0] / inputResults[1]);
        case OP_multiply: return (inputResults[0] * inputResults[1]);
        case OP_power:    return (pow(inputResults[0], inputResults[1]));
        // Multi
        case OP_min:      return manual_min(inputResults); // TODO: Do these functions exist?
        case OP_max:      return manual_max(inputResults);

        default:
          vw_throw(LogicErr() << "Unexpected operation type.\n");
      }
    }
};

// We need to tell fusion about our calc_operation struct
// to make it a first-class fusion citizen
BOOST_FUSION_ADAPT_STRUCT(
    calc_operation,
    (OperationType, opType)
    (double, value)
    (int, varName)
    (std::vector<calc_operation>, inputs)
)

//================================================================================
// - Boost::Spirit equation parsing

// Helper constants to aid in accessing a calc_operation struct
const int OP  = 0;
const int NUM = 1;
const int VAR = 2;
const int IN  = 3;

template <typename ITER>
struct calc_grammar : b_s::qi::grammar<ITER, calc_operation(), b_s::ascii::space_type> {
  // Constructor function
  calc_grammar() : calc_grammar::base_type(expression) {
     // Definitions
     using boost::phoenix::at;
     using boost::phoenix::at_c;
     using boost::phoenix::push_back;
     using b_s::qi::double_;
     using b_s::qi::int_;
     using b_s::qi::eps;
     using b_s::qi::_val;
     using b_s::qi::_1; // This is required to avoid namespace mixups with other Boost modules.
     using b_s::qi::_2;
     using b_s::qi::_3;

     // This approach works but it processes expressions right to left.
     // - To get what you want, use parenthesis.

     // An outer expression
     expression =
        (term  [push_back(at_c<IN>(_val), _1)] )
         >> *(  ('+' >> expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_add     ]) |  // Addition
                ('-' >> expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_subtract])    // Subtraction
             );

     // Middle priority
     term =
         (factor [push_back(at_c<IN>(_val), _1)])
         >> *( ('*' >> term [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_multiply] ) |  // Multiplication
               ('/' >> term [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_divide  ] )    // Subtraction
             );

    // The highest priority
    // - TODO: An additional layer to prevent double signs?
    factor =
        (double_          [at_c<NUM>(_val)=_1,            at_c<OP>(_val)=OP_number]    ) | // Just a number
        // The min and max operations take a comma seperated list of expressions
        ("min(" > expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_min] % ',' > ')') |
        ("max(" > expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_max] % ',' > ')') |
        ( ("pow(" > expression > ',' > expression > ')')
                [push_back(at_c<IN>(_val), _1), push_back(at_c<IN>(_val), _2), at_c<OP>(_val)=OP_power] ) |
        ("abs(" > expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_abs] > ')') | // Absolute value
        ('(' > expression [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_pass] > ')') | // Something in parenthesis
        ('-' >> factor    [push_back(at_c<IN>(_val), _1), at_c<OP>(_val)=OP_negate]    ) | // Negative sign
        //('+' >> factor    [handler]      );  // Positive sign
        ("var_" > int_ [at_c<VAR>(_val)=_1, at_c<OP>(_val)=OP_variable] ) ;
        //(b_s::ascii::string  [at_c<VAR>(_val)=_1,  at_c<OP>(_val)=OP_variable]    ) ; // A variable name

    /*

  // This code is for proper left priority parsing, but this approach does not work at all using
  //  Boost::Spirit.  An entirely new approach is needed but it is not worth the time figuring out.

    // This split-based approach will make the parser visit the parameters in the correct order,
    // but the current method of each term call returning an OP node won't work.  There will have
    // to be more of a central authority that keeps adding operations as it encounters them.

    term = (factor [push_back(at_c<IN>(_val), _1)])
            >> (termP [push_back(at_c<IN>(_val), _1)] )
    termP =

           ( ('*' >> factor >> termP ) [push_back(at_c<IN>(_val),  b_s::qi::_0), at_c<OP>(_val)=OP_multiply]
           )
          | eps;

    // The highest priority
    factor =
        (double_          [at_c<NUM>(_val)=_1, at_c<OP>(_val)=OP_number]); // Just a number

    */
  } // End constructor

  // Grammer rules
  b_s::qi::rule<ITER, calc_operation(), b_s::ascii::space_type> expression, term, factor;

}; // End struct calc_grammer

//=================================================================================

/// List of possible output data types
enum DataType {
  DT_UINT8   = 0,
  DT_UINT16  = 1,
  DT_UINT32  = 2,
  //DT_INT8    = 3, // Not supported by GDAL.
  DT_INT16   = 3,
  DT_INT32   = 4,
  DT_FLOAT32 = 5,
  DT_FLOAT64 = 6
};

/// Returns the default nodata value for each data type (just the minimum value)
double get_default_nodata(const DataType d) {
  switch(d) {
  case    DT_UINT8  : return std::numeric_limits<vw::uint8>::min();
  case    DT_INT16  : return std::numeric_limits<vw::int16>::min();
  case    DT_UINT16 : return std::numeric_limits<vw::uint16>::min();
  case    DT_INT32  : return std::numeric_limits<vw::int32>::min();
  case    DT_UINT32 : return std::numeric_limits<vw::uint32>::min();
  case    DT_FLOAT32: return -std::numeric_limits<vw::float32>::max();
  default :           return -std::numeric_limits<vw::float64>::max();
  };
}

/// Converts a double to another numeric type with min/max value clamping.
/// - TODO: Replace this with the correct VW function.
template <typename T>
T clamp_and_cast(const double val) {
  const T minVal = std::numeric_limits<T>::min();
  const T maxVal = std::numeric_limits<T>::max();
  if (val < static_cast<double>(minVal)) return (minVal);
  if (val > static_cast<double>(maxVal)) return (maxVal);
  return static_cast<T>(val);
}

// Specializations for floating point values.
template <typename T>
T clamp_and_cast_float(const double val) {
  const T minVal = -std::numeric_limits<T>::max();
  const T maxVal = std::numeric_limits<T>::max();
  if (val < static_cast<double>(minVal)) return (minVal);
  if (val > static_cast<double>(maxVal)) return (maxVal);
  return static_cast<T>(val);
}

template <>
vw::float32 clamp_and_cast<vw::float32>(const double val) {
  return clamp_and_cast_float<vw::float32>(val);
}

template <>
vw::float64 clamp_and_cast<vw::float64>(const double val) {
  return clamp_and_cast_float<vw::float64>(val);
}

/// Image view class which applies the calc_operation tree to each pixel location.
template <class ImageT, typename OutputPixelT>
class ImageCalcView : public ImageViewBase<ImageCalcView<ImageT, OutputPixelT> > {

public: // Definitions
  typedef typename ImageT::pixel_type  input_pixel_type;
  typedef OutputPixelT pixel_type;  // This is what controls the type of image that is written to disk.
  typedef OutputPixelT result_type;

private: // Variables
  std::vector<ImageT    > m_image_vec;
  std::vector<bool      > m_has_nodata_vec;
  std::vector<input_pixel_type> m_nodata_vec;
  result_type    m_output_nodata;
  calc_operation m_operation_tree;
  int m_num_rows;
  int m_num_cols;
  int m_num_channels;

public: // Functions

  // Constructor
  ImageCalcView( std::vector<ImageT>                & imageVec,
                 std::vector<bool  >           const& has_nodata_vec,
                 std::vector<input_pixel_type> const& nodata_vec,
                 result_type outputNodata,
                 calc_operation const& operation_tree)
                  : m_image_vec(imageVec),   m_has_nodata_vec(has_nodata_vec),
                    m_nodata_vec(nodata_vec), m_output_nodata(outputNodata),
                    m_operation_tree(operation_tree) {
    const size_t numImages = imageVec.size();
    VW_ASSERT( (numImages > 0), ArgumentErr() << "ImageCalcView: One or more images required.." );
    VW_ASSERT( (has_nodata_vec.size() == numImages), LogicErr() << "ImageCalcView: Incorrect hasNodata count passed in." );
    VW_ASSERT( (nodata_vec.size()    == numImages), LogicErr() << "ImageCalcView: Incorrect nodata count passed in." );

    // Make sure all images are the same size
    m_num_rows     = imageVec[0].rows();
    m_num_cols     = imageVec[0].cols();
    m_num_channels = imageVec[0].planes();
    for (size_t i=1; i<numImages; ++i) {
      if ( (imageVec[i].rows()   != m_num_rows    ) ||
           (imageVec[i].cols()   != m_num_cols    ) ||
           (imageVec[i].planes() != m_num_channels)   )
        vw_throw(ArgumentErr() << "Error: Input images must all have the same size and number of channels.");
    }
  }

  inline int32 cols  () const { return m_num_cols; }
  inline int32 rows  () const { return m_num_rows; }
  inline int32 planes() const { return m_num_channels; }

  inline result_type operator()( int32 i, int32 j, int32 p=0 ) const
  {
    return 0; // NOT IMPLEMENTED.
  }

  typedef ProceduralPixelAccessor<ImageCalcView<ImageT, OutputPixelT> > pixel_accessor;
  inline pixel_accessor origin() const { return pixel_accessor( *this, 0, 0 ); }

  typedef CropView<ImageView<result_type> > prerasterize_type;
  inline prerasterize_type prerasterize( BBox2i const& bbox ) const {
    //typedef typename PixelChannelType<typename input_pixel_type>::type output_channel_type; // TODO: Why does this not compile?
    typedef typename ImageChannelType<ImageView<result_type> >::type output_channel_type;

    // Set up the output image tile
    ImageView<result_type> tile(bbox.width(), bbox.height());

    // TODO: Left to right processing.

    // Set up for pixel calculations
    const size_t num_images = m_image_vec.size();
    std::vector<input_pixel_type> input_pixels(num_images);
    std::vector<double>           input_doubles(num_images);

    // Rasterize all the input images at this particular tile
    std::vector<ImageView<input_pixel_type> > input_tiles(num_images);
    for (size_t i=0; i<num_images; ++i)
      input_tiles[i] = crop(m_image_vec[i], bbox);

    // Loop through each output pixel and compute each output value
    for (int c = 0; c < bbox.width(); c++) {
      for (int r = 0; r < bbox.height(); r++) {
        // Fetch all the input pixels for this location
        bool isNodata = false;
        for (size_t i=0; i<num_images; ++i) {
          input_pixels[i] = (input_tiles[i])(c,r);

          // If any of the input pixels are nodata, the output is nodata.
          if (m_has_nodata_vec[i] && (m_nodata_vec[i] == input_pixels[i])) {
            isNodata = true;
            break;
          }
        } // End image loop

        if (isNodata) { // Output is nodata, move on to the next pixel
          tile(c, r) = m_output_nodata;
          continue;
        }

        for (int chan=0; chan<m_num_channels; ++chan) {
          for (size_t i=0; i<num_images; ++i) {
            input_doubles[i] = input_pixels[i][chan];
          } // End image loop

          // Apply the operation tree to this pixel and store in the output pixel
          double newVal = m_operation_tree.applyOperation<double>(input_doubles);
          //if (newVal > 255)
          //  std::cout << "Value: " << newVal << " --> " << static_cast<double>(clamp_and_cast<output_channel_type>(newVal)) << "\n";
          tile(c, r, chan) = clamp_and_cast<output_channel_type>(newVal);

        } // End channel loop

      } // End row loop
    } // End column loop

  // Return the tile we created with fake borders to make it look the size of the entire output image
  return prerasterize_type(tile,
                           -bbox.min().x(), -bbox.min().y(),
                           cols(), rows() );

  } // End prerasterize function

 template <class DestT>
 inline void rasterize( DestT const& dest, BBox2i const& bbox ) const {
   vw::rasterize( prerasterize(bbox), dest, bbox );
 }

}; // End class ImageCalcView

//======================================================================================================

struct Options : vw::cartography::GdalWriteOptions {
  Options() : out_nodata_value(-1) {}
  // Input
  std::vector<std::string> input_files;
  bool        has_in_nodata;
  double      in_nodata_value;

  // Settings
  std::string output_data_string;
  DataType    output_data_type;
  bool        has_out_nodata;
  double      out_nodata_value;
  std::string calc_string;
  std::string output_file, metadata;
};

// Handling input
void handle_arguments( int argc, char *argv[], Options& opt ) {

  const std::string calc_string_help =
    "The operation to be performed on the input images. "
    "Input images must all be the same size and type. "
    "Currently only single channel images are supported. "
    "Recognized operators: +, -, /, *, (), pow(), abs(), min(), max(), var_0, var_1, ..."
    "Use var_n to refer to the pixel of the n-th input image. "
    "Order of operations is parsed with RIGHT priority, use parenthesis "
    "to assure the order you want. "
    "Surround the entire string with double quotes.";

  const std::string data_type_string =
    "The data type of the output file:\n"
    "  uint8   \n"
    "  uint16  \n"
    "  uint32  \n"
    "  int16   \n"
    "  int32   \n"
    "  float32 \n"
    "  float64 (default)\n";

  po::options_description general_options("");
  general_options.add_options()
    ("output-file,o", po::value(&opt.output_file), "Output file name.")
    ("calc,c",            po::value(&opt.calc_string), calc_string_help.c_str())
    ("output-data-type,d",  po::value(&opt.output_data_string)->default_value("float64"), data_type_string.c_str())
    ("input-nodata-value",  po::value(&opt.in_nodata_value), "Value that is no-data in the input images.")
    ("output-nodata-value", po::value(&opt.out_nodata_value), "Value to use for no-data in the output image.")
    ("mo",  po::value(&opt.metadata)->default_value(""), "Write metadata to the output file. Provide as a string in quotes if more than one item, separated by a space, such as 'VAR1=VALUE1 VAR2=VALUE2'. Neither the variable names nor the values should contain spaces.")
    ("help,h",            "Display this help message");

  general_options.add( vw::cartography::GdalWriteOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("input-files", po::value<std::vector<std::string> >(&opt.input_files));

  po::positional_options_description positional_desc;
  positional_desc.add("input-files", -1);

  std::string usage("[options] <image-files>");
  bool allow_unregistered = false;
  std::vector<std::string> unregistered;
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
			     positional, positional_desc, usage,
			     allow_unregistered, unregistered );

  if ( opt.input_files.empty() )
    vw_throw( ArgumentErr() << "Missing input files.\n" << usage << general_options );
  if ( opt.calc_string.size() == 0)
    vw_throw( ArgumentErr() << "Missing operation string.\n" << usage << general_options );

  if      (opt.output_data_string == "uint8"  ) opt.output_data_type = DT_UINT8;
  else if (opt.output_data_string == "uint16" ) opt.output_data_type = DT_UINT16;
  else if (opt.output_data_string == "uint32" ) opt.output_data_type = DT_UINT32;
  else if (opt.output_data_string == "int16"  ) opt.output_data_type = DT_INT16;
  else if (opt.output_data_string == "int32"  ) opt.output_data_type = DT_INT32;
  else if (opt.output_data_string == "float32") opt.output_data_type = DT_FLOAT32;
  else if (opt.output_data_string == "float64") opt.output_data_type = DT_FLOAT64;
  else
    vw_throw(ArgumentErr() << "Unsupported output data type: '" << opt.output_data_string << "'.\n" );

 // Fill out opt.has_in_nodata and opt.has_out_nodata depending if the user specified these options
 if (!vm.count("input-nodata-value")){
    opt.has_in_nodata   = false;
  }else
    opt.has_in_nodata = true;

 if (!vm.count("output-nodata-value")){
    opt.has_out_nodata   = false;
    opt.out_nodata_value = get_default_nodata(opt.output_data_type);
  }else
    opt.has_out_nodata = true;

 vw::create_out_dir(opt.output_file);
}

/// This function call is just to clean up the case statement in load_inputs_and_process
template <typename PixelT, typename OutputT>
void generate_output(const std::string                         & output_file,
                     const Options                             & opt,
                     const calc_operation                      & calc_tree,
                     const bool                                  have_georef,
                     const vw::cartography::GeoReference       & georef,
                           std::vector< ImageViewRef<PixelT> > & input_images,
                     const std::vector<bool  >                 & has_nodata_vec,
                     const std::vector<PixelT>                 & nodata_vec ) {

  // Parse keywords from --mo.
  std::map<std::string, std::string> keywords;
  asp::parse_append_metadata(opt.metadata, keywords);

  vw_out() << "Writing: " << output_file << std::endl;
  vw::cartography::block_write_gdal_image
    (output_file,
     ImageCalcView< ImageViewRef<PixelT>, OutputT >(input_images,
                                                    has_nodata_vec,
                                                    nodata_vec,
                                                    opt.out_nodata_value,
                                                    calc_tree),
     have_georef, georef,
     opt.has_out_nodata, opt.out_nodata_value,
     opt,
     TerminalProgressCallback("image_calc","Writing:"),
     keywords);
}

/// This function loads the input images and calls the main processing function
template <typename PixelT>
void load_inputs_and_process(Options &opt, const std::string &output_file, const calc_operation &calc_tree) {

  // Read the georef from the first file, they should all have the same value.
  const size_t numInputFiles = opt.input_files.size();
  std::vector< ImageViewRef<PixelT> > input_images(numInputFiles);
  std::vector<bool  >                 has_nodata_vec(numInputFiles);
  std::vector<PixelT>                 nodata_vec(numInputFiles);

  bool have_georef = false;
  vw::cartography::GeoReference georef;

  // Loop through each input file
  for (size_t i=0; i<numInputFiles; ++i) {
    const std::string input = opt.input_files[i];

    if (!have_georef) {
      have_georef = vw::cartography::read_georeference(georef, input);
      if (have_georef)
        vw_out() << "\t--> Copying georef from input image " << input << std::endl;
    }

    // Determining the format of the input
    boost::shared_ptr<vw::DiskImageResource> rsrc(vw::DiskImageResourcePtr(input));
    //ChannelTypeEnum channel_type = rsrc->channel_type();
    //PixelFormatEnum pixel_format = rsrc->pixel_format();

    // Check for nodata value in the file
    if ( rsrc->has_nodata_read() ) {
      nodata_vec[i] = rsrc->nodata_read();
      std::cout << "\t--> Extracted nodata value from file: " << nodata_vec[i] << ".\n";
      has_nodata_vec[i] = true;
      opt.has_out_nodata = true; // If any inputs have nodata, the output must have nodata.
    }
    else { // File does not specify nodata
      if (opt.has_in_nodata) { // User has specified a default nodata value
        has_nodata_vec[i]  = true;
        nodata_vec   [i]  = PixelT(opt.in_nodata_value);
      }
      else // Don't use nodata for this input
        has_nodata_vec[i] = false;
    }

    DiskImageView<PixelT> this_disk_image(input);
    input_images[i] = this_disk_image;

  } // loop through input images

  if (opt.has_out_nodata)
    vw_out() << "\t--> Writing output nodata value " << opt.out_nodata_value << std::endl;

  // Write out the selected data type
  //vw_out() << "Writing: " << output_file << " with data type " << opt.output_data_type << std::endl;
  switch(opt.output_data_type) {
    case    DT_UINT8  : generate_output<PixelT, PixelGray<vw::uint8  > >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    case    DT_INT16  : generate_output<PixelT, PixelGray<vw::int16  > >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    case    DT_UINT16 : generate_output<PixelT, PixelGray<vw::uint16 > >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    case    DT_INT32  : generate_output<PixelT, PixelGray<vw::int32  > >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    case    DT_UINT32 : generate_output<PixelT, PixelGray<vw::uint32 > >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    case    DT_FLOAT32: generate_output<PixelT, PixelGray<vw::float32> >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
    default :           generate_output<PixelT, PixelGray<vw::float64> >(output_file, opt, calc_tree, have_georef, georef, input_images, has_nodata_vec, nodata_vec); break;
  };

}

/// The main function calls the cmd line parsers and figures out the input image type
int main( int argc, char *argv[] ) {

  Options opt;
  try {
    handle_arguments( argc, argv, opt );

    std::string exp(opt.calc_string);

    calc_grammar<std::string::const_iterator> grammerParser;
    calc_operation calc_tree;

    std::string::const_iterator iter = exp.begin();
    std::string::const_iterator end = exp.end();
    bool r = phrase_parse(iter, end, grammerParser, boost::spirit::ascii::space, calc_tree);

    if (r && iter == end) {// Successfully parsed the calculation expression
      //std::cout << "-------------------------\n";
      //std::cout << "Parsing succeeded\n";
      //std::cout << "-------------------------\n";
      ////calc_tree.print();
      ////std::cout << "----------- pruned --------------\n";
      calc_tree.clearEmptyNodes();
      //calc_tree.print();
    }
    else { // Failed to parse the calculation expression
      std::string::const_iterator some = iter+30;
      std::string context(iter, (some>end)?end:some);
      std::cout << "-------------------------\n";
      std::cout << "Parsing calculation expression failed\n";
      std::cout << "stopped at: \": " << context << "...\"\n";
      std::cout << "-------------------------\n";
      return -1;
    }

    // Use a default output file if none provided
    const std::string firstFile = opt.input_files[0];
    //vw_out() << "Loading: " << firstFile << "\n";
    size_t pt_idx = firstFile.rfind(".");
    std::string output_file;
    if (opt.output_file.size() != 0)
      output_file = opt.output_file;
    else {
      output_file = firstFile.substr(0,pt_idx)+"_calc";
      output_file += firstFile.substr(pt_idx,firstFile.size()-pt_idx);
    }

    // Determining the format of the input images (all are assumed to be the same type.)
    boost::shared_ptr<vw::DiskImageResource> rsrc(vw::DiskImageResourcePtr(firstFile));
    ChannelTypeEnum input_data_type = rsrc->channel_type();
    //PixelFormatEnum pixel_format = rsrc->pixel_format();

    // Redirect to another function with the correct template type
    switch(input_data_type) {
    case VW_CHANNEL_INT8   : load_inputs_and_process<PixelGray<vw::int8   > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_UINT8  : load_inputs_and_process<PixelGray<vw::uint8  > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_INT16  : load_inputs_and_process<PixelGray<vw::int16  > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_UINT16 : load_inputs_and_process<PixelGray<vw::uint16 > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_INT32  : load_inputs_and_process<PixelGray<vw::int32  > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_UINT32 : load_inputs_and_process<PixelGray<vw::uint32 > >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_FLOAT32: load_inputs_and_process<PixelGray<vw::float32> >(opt, output_file, calc_tree);  break;
    case VW_CHANNEL_FLOAT64: load_inputs_and_process<PixelGray<vw::float64> >(opt, output_file, calc_tree);  break;
    default : vw_throw(ArgumentErr() << "Input image format " << input_data_type << " is not supported.\n");
    };
  } ASP_STANDARD_CATCHES;

}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-operator
    Function object implementing unary_minus capabilities

    return the elementwise unary minus of the parameter

    Infix notation can be used with operator '-'

    @par Semantic:

    For every parameter of type T

    @code
    auto r = unary_minus(x);
    @endcode

    or

    @code
    auto r = -x;
    @endcode

    @par Note:

    - Be aware that for signed integers the unary_minus of @ref Valmin is
    @ref Valmin. This is a side effect of the 2-complement
    representation of integers. To avoid this, you may use the
    saturated_ functor (calling saturated_(unary_minus)(x))
    or convert the input parameter to a larger type
    before taking the unary_minus value.

    @see  minus, unary_plus

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

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

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/set.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTANT_ASSERT(elem(list(2, int_<2>, int_<3>, 'x'), int_<3>));
    BOOST_HANA_CONSTANT_ASSERT(elem(set(1, '2', type<int>, "foobar"), type<int>));
    //! [main]
}


#include <cmath>
#include <boost/shared_ptr.hpp>
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <cmd_vel_smoother/CmdVelSmootherConfig.h>
#include <geometry_msgs/Twist.h>

template<class T> inline T sgn(T val) { return val > 0 ? 1.0 : -1.0; }

namespace dyn = dynamic_reconfigure;

class VelocitySmootherNode
{
  ros::NodeHandle nh_, pnh_;
  ros::Publisher  pub_;
  ros::Subscriber sub_;
  ros::Timer timer_;

  boost::mutex cfg_mutex_;
  typedef cmd_vel_smoother::CmdVelSmootherConfig Config;
  boost::shared_ptr<dyn::Server<Config> > dyn_srv_;

  int interpolate_max_frame_, count_;
  double desired_rate_;
  double x_acc_lim_, y_acc_lim_, yaw_acc_lim_;
  ros::Time latest_time_;
  geometry_msgs::Twist::ConstPtr latest_vel_;
  geometry_msgs::Twist::Ptr pub_vel_;

public:
  ros::Duration timerDuration() {
    return ros::Duration(1./(desired_rate_ * 2.2));
  }

  void dynConfigCallback(Config &cfg, uint32_t level) {
    boost::mutex::scoped_lock lock(cfg_mutex_);
    x_acc_lim_ = cfg.x_acc_lim;
    y_acc_lim_ = cfg.y_acc_lim;
    yaw_acc_lim_ = cfg.yaw_acc_lim;
    interpolate_max_frame_ = cfg.interpolate_max_frame;

    if(desired_rate_ != cfg.desired_rate) {
      desired_rate_ = cfg.desired_rate;
      if (timer_.isValid()) {
        ros::Duration d = timerDuration();
        timer_.setPeriod(d);
        ROS_INFO_STREAM("timer loop rate is changed to " << 1.0 / d.toSec() << "[Hz]");
      }
    }
  }

  void velCallback(const geometry_msgs::Twist::ConstPtr &msg){
    latest_time_ = ros::Time::now();
    latest_vel_ = msg;
    count_ = 0;
    pub_.publish(msg);
  }

  void timerCallback(const ros::TimerEvent& event){
    if (count_ > interpolate_max_frame_) return;
    if (latest_vel_ == NULL) return;

    double past_sec = (event.current_real - latest_time_).toSec();
    if (past_sec < 1.0 / desired_rate_) return;

    double g_x   = std::abs(latest_vel_->linear.x  / past_sec);
    double g_y   = std::abs(latest_vel_->linear.y  / past_sec);
    double g_yaw = std::abs(latest_vel_->angular.z / past_sec);

    ROS_DEBUG_STREAM("G: (" << g_x << ", " << g_y << ", " << g_yaw << ") detected");

    bool need_publish = false;
    if (g_x > x_acc_lim_) {
      pub_vel_->linear.x = (g_x - x_acc_lim_) * past_sec * sgn(latest_vel_->linear.x);
      need_publish = true;
    }

    if (g_y > y_acc_lim_) {
      pub_vel_->linear.y = (g_y - y_acc_lim_) * past_sec * sgn(latest_vel_->linear.y);
      need_publish = true;
    }

    if (g_yaw > yaw_acc_lim_) {
      pub_vel_->angular.z = (g_yaw - yaw_acc_lim_) * past_sec * sgn(latest_vel_->angular.z);
      need_publish = true;
    }

    if (need_publish)
      pub_.publish(*pub_vel_);
    count_++;
  }

  VelocitySmootherNode(): nh_(), pnh_("~") {
    latest_time_ = ros::Time::now();

    dyn_srv_ = boost::make_shared<dyn::Server<Config> >(pnh_);
    dyn::Server<Config>::CallbackType f = boost::bind(&VelocitySmootherNode::dynConfigCallback, this, _1, _2);
    dyn_srv_->setCallback(f);

    pub_vel_ = boost::shared_ptr<geometry_msgs::Twist>(new geometry_msgs::Twist());
    pub_ = nh_.advertise<geometry_msgs::Twist>("output", 100);
    sub_ = nh_.subscribe("input", 1, &VelocitySmootherNode::velCallback, this);
    timer_ = nh_.createTimer(timerDuration(), &VelocitySmootherNode::timerCallback, this);
  };
  ~VelocitySmootherNode() {};

};

int main(int argc, char** argv) {
  ros::init(argc, argv, "cmd_vel_smoother");
  VelocitySmootherNode n;
  ros::spin();

  return 0;
}


#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <vector>

#include "src/clustering/lossfunction/LossFunction.hpp"
#include "src/clustering/lossfunction/BinderLoss.hpp"
#include "src/clustering/lossfunction/VariationInformation.hpp"

TEST(loss, binder_loss) {
  LossFunction binder(0);
  binder = new BinderLoss(1, 1);

  Eigen::VectorXi cluster1 << 1, 1, 1;
  Eigen::VectorXi cluster2 << 1, 1, 2;

  binder.setCluster(cluster1, cluster2);
  ASSERT_EQ(1, binder.loss());

  cluster2 << 1, 2, 3;
  ASSERT_EQ(3, binder.loss());
}

TEST(loss, vi_loss) {
  LossFunction vi(0);
  vi = new VariationInformation(1, 1);

  Eigen::VectorXi cluster1 << 1, 1, 1;
  Eigen::VectorXi cluster2 << 1, 1, 2;

  binder.setCluster(cluster1, cluster2);
  ASSERT_EQ(1, binder.loss());

  cluster2 << 1, 2, 3;
  ASSERT_EQ(3, binder.loss());
}


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

#ifndef BOOST_MATH_INTERPOLATORS_CARDINAL_QUINTIC_B_SPLINE_HPP
#define BOOST_MATH_INTERPOLATORS_CARDINAL_QUINTIC_B_SPLINE_HPP
#include <memory>
#include <limits>
#include <boost/math/interpolators/detail/cardinal_quintic_b_spline_detail.hpp>

namespace boost{ namespace math{ namespace interpolators {

template <class Real>
class cardinal_quintic_b_spline
{
public:
    // If you don't know the value of the derivative at the endpoints, leave them as nans and the routine will estimate them.
    // y[0] = y(a), y[n - 1] = y(b), step_size = (b - a)/(n -1).
    cardinal_quintic_b_spline(const Real* const y,
                                size_t n,
                                Real t0 /* initial time, left endpoint */,
                                Real h  /*spacing, stepsize*/,
                                std::pair<Real, Real> left_endpoint_derivatives = {std::numeric_limits<Real>::quiet_NaN(), std::numeric_limits<Real>::quiet_NaN()},
                                std::pair<Real, Real> right_endpoint_derivatives = {std::numeric_limits<Real>::quiet_NaN(), std::numeric_limits<Real>::quiet_NaN()})
     : impl_(std::make_shared<detail::cardinal_quintic_b_spline_detail<Real>>(y, n, t0, h, left_endpoint_derivatives, right_endpoint_derivatives))
    {}

    // Oh the bizarre error messages if we template this on a RandomAccessContainer:
    cardinal_quintic_b_spline(std::vector<Real> const & y,
                                Real t0 /* initial time, left endpoint */,
                                Real h  /*spacing, stepsize*/,
                                std::pair<Real, Real> left_endpoint_derivatives = {std::numeric_limits<Real>::quiet_NaN(), std::numeric_limits<Real>::quiet_NaN()},
                                std::pair<Real, Real> right_endpoint_derivatives = {std::numeric_limits<Real>::quiet_NaN(), std::numeric_limits<Real>::quiet_NaN()})
     : impl_(std::make_shared<detail::cardinal_quintic_b_spline_detail<Real>>(y.data(), y.size(), t0, h, left_endpoint_derivatives, right_endpoint_derivatives))
    {}

    Real operator()(Real t) const {
        return impl_->operator()(t);
    }

    Real prime(Real t) const {
       return impl_->prime(t);
    }

    Real double_prime(Real t) const {
        return impl_->double_prime(t);
    }

    Real t_max() const {
        return impl_->t_max();
    }

private:
    std::shared_ptr<detail::cardinal_quintic_b_spline_detail<Real>> impl_;
};

}}}
#endif


// Copyright 2015-2019 Carnegie Mellon University.  See LICENSE file for terms.

#include "cdg.hpp"
#include "masm.hpp"
#include "util.hpp"
#include "funcs.hpp"

#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/reverse_graph.hpp>

namespace pharos {

#define NULL_VERTEX boost::graph_traits<ControlFlowGraph>::null_vertex()
using DomIdx = typename boost::property_map<ControlFlowGraph, boost::vertex_index_t>::type;

CDG::CDG(FunctionDescriptor &f) {
  Rose::BinaryAnalysis::ControlFlow cfg_analysis;

  // Save a pointer to the FunctionDescriptor.
  fd = &f;

  // Build the CFG and get the blocks in forward flow order
  cfg = fd->get_pharos_cfg();
  forward_list = fd->get_vertices_in_flow_order();

  // Some sanity checking to prove that the true entry vertex is always zero.  This test still
  // fails in some rare cases where function's entry_block() is NULL.  This is apparently due
  // to some bug in the partitioner, because the case I looked at was garbage code made from
  // bytes in the import segment.  More analysis is sadly still required.
  CFGVertex entry = 0;

  // The entry block from the perspective of the control flow graph.
  SgAsmBlock *eblock = get(boost::vertex_name, cfg, entry);
  // The entry block from the perspective of the function descriptor.
  SgAsmBlock *feblock = fd->get_entry_block();

  // Now report various failures without asserting.
  if (eblock == NULL) {
    GERROR << "CFG entry block is NULL for function: " << fd->address_string() << LEND;
  }
  if (feblock == NULL) {
    GERROR << "Entry block is NULL for function: " << fd->address_string() << LEND;
  }
  if (eblock != NULL and feblock != NULL and eblock != feblock) {
    GERROR << "Unexpected entry block mismatch " << addr_str(eblock->get_address())
           << " != " << addr_str(feblock->get_address()) << "." << LEND;
  }

  // If building the dominance and post-dominance maps turns out to be expensive, we could
  // improve performance by deferring it's computation until someone asks a dominance question.

  // Build the immediate-dominator array
  auto idx = DomIdx(get(boost::vertex_index, cfg));
  imm_dom = DomVec(num_vertices(cfg), NULL_VERTEX);
  auto dom_map = make_iterator_property_map(imm_dom.begin(), idx);
  lengauer_tarjan_dominator_tree(cfg, entry, dom_map);

  // Dominance analysis failed
  // We won't have a correct dominance analysis
  if (imm_dom.size() == 0) {
    GERROR << "Dominance analysis failed for function: " << fd->address_string() << LEND;
  }

  // Build the immediate-post-dominator array.
  auto retblocks = cfg_analysis.return_blocks(cfg, entry);
  switch (retblocks.size()) {
   case 0:
    imm_post_dom = DomVec(num_vertices(cfg), NULL_VERTEX);
    break;
   case 1:
    {
      // This function has one exit block
      auto rev_cfg = make_reverse_graph(cfg);
      auto post_idx = DomIdx(get(boost::vertex_index, rev_cfg));
      imm_post_dom = DomVec(num_vertices(rev_cfg), NULL_VERTEX);
      auto post_map = make_iterator_property_map(imm_post_dom.begin(), post_idx);
      lengauer_tarjan_dominator_tree(rev_cfg, retblocks[0], post_map);
    }
    break;
   default:
    {
      // This function has multiple exit blocks.
      ControlFlowGraph copy = cfg;
      auto unique_exit = add_vertex(copy);
      for (auto v : retblocks) {
        add_edge(v, unique_exit, copy);
      }
      auto rev_cfg = make_reverse_graph(cfg);
      auto post_idx = DomIdx(get(boost::vertex_index, rev_cfg));
      imm_post_dom = DomVec(num_vertices(rev_cfg), NULL_VERTEX);
      auto post_map = make_iterator_property_map(imm_post_dom.begin(), post_idx);
      lengauer_tarjan_dominator_tree(rev_cfg, retblocks[0], post_map);
      for (auto & v : imm_post_dom) {
        if (v == unique_exit) {
          v = NULL_VERTEX;
        }
      }
    }
  }

  // Post-dominance analysis failed
  // We won't have a correct dominance analysis
  if (imm_post_dom.size() == 0) {
    GERROR << "Post-dominance analysis failed for function: " << fd->address_string() << LEND;
  }

  initialize_block_dependencies();
  // dumpControlDependencies();
}

// A helper routine to evaluate the immediate (or post) dominance map recursively, turning an
// immediate result into an overall result.  The return value is whether A dominates B.
bool CDG::dominance_helper(
  const DomVec & map,
  const CFGVertex &a, const CFGVertex &b)
{
  // This at() call will throw an exception if b is not in the map.
  const CFGVertex& bs_dominator = map.at(b);
  // If B's not immediately dominated by anybody, it's not immediately dominated A.
  if (bs_dominator == NULL_VERTEX) return false;
  // If B is immediately dominated by A, then requested condition is true.
  if (bs_dominator == a) return true;
  // Otherwise, we need to determine whether A dominates B's immediate dominator.
  else return dominance_helper(map, a, bs_dominator);
}

// Does i1 dominate i2?  This convenience version takes two instructions instead of two basic
// blocks or two control flow graph vertices.
bool CDG::dominates(const SgAsmX86Instruction *i1, const SgAsmX86Instruction *i2) const {
  if (!i1 || !i2) return false;
  SgAsmBlock *bb1 = insn_get_block(i1);
  assert(bb1);
  SgAsmBlock *bb2 = insn_get_block(i2);
  assert(bb2);

  //GTRACE << "Insn " << addr_str(i1->get_address()) << " is in block " << addr_str(bb1->get_address()) << LEND;
  //GTRACE << "Insn " << addr_str(i2->get_address()) << " is in block " << addr_str(bb2->get_address()) << LEND;
  if (bb1 == bb2) return i1->get_address() <= i2->get_address();

  // If the basic blocks aren't in the map return false.
  if (block_to_vertex.find(bb1) == block_to_vertex.end() ||
      block_to_vertex.find(bb2) == block_to_vertex.end()) return false;

  // Return the dominance result based on the vertexes.  Calling at() cannot throw since we
  // just checked that both blocks are in the map.
  //GTRACE << "Asking helper(" << block_to_vertex.at(bb1) << ", " << block_to_vertex.at(bb2) << ")" << LEND;
  return dominates(block_to_vertex.at(bb1), block_to_vertex.at(bb2));
}

// Does i1 post-dominate i2?  This convenience version takes two instructions instead of two
// basic blocks or two control flow graph vertices.
bool CDG::post_dominates(const SgAsmX86Instruction *i1, const SgAsmX86Instruction *i2) const {
  if (!i1 || !i2) return false;
  SgAsmBlock *bb1 = insn_get_block(i1);
  assert(bb1);
  SgAsmBlock *bb2 = insn_get_block(i2);
  assert(bb2);

  if (bb1 == bb2) return i1->get_address() >= i2->get_address();

  // If the basic blocks aren't in the map return false.
  if (block_to_vertex.find(bb1) == block_to_vertex.end() ||
      block_to_vertex.find(bb2) == block_to_vertex.end()) return false;

  // Return the post dominance result based on the vertexes.  Calling at() cannot throw since
  // we just checked that both blocks are in the map.
  return post_dominates(block_to_vertex.at(bb1), block_to_vertex.at(bb2));
}

void CDG::get_dependents_between(const CFGVertex& a, const CFGVertex& b, CFGVertexSet &dependents) const {
  // Either of these at() calls might throw, but that's probably better than blindly creating nodes.
  const CFGVertex& apd = imm_post_dom.at(a);
  const CFGVertex& bpd = imm_post_dom.at(b);

  dependents.insert(b);
  if (apd != NULL_VERTEX && bpd == apd) {
    return;
  } else if (bpd == a) {
    dependents.insert(a);
    return;
  } else if (bpd != NULL_VERTEX) {
    get_dependents_between(a, bpd, dependents);
  }
}

// This method has not been carefully reviewed since Wes wrote it...  It appears to be part of
// the constructor really.  It looks like it builds block_to_vertex, depends_on, and
// dependents_of.
void CDG::initialize_block_dependencies() {
  // Find all edges (A,B) in the CFG such that B is not
  // an ancestor of A in the post-dominator tree
  for (size_t x = 0; x < forward_list.size(); x++) {
    CFGVertex vertex = forward_list[x];

    // Get the basic block
    SgAsmBlock *bb = get(boost::vertex_name, cfg, vertex);
    assert(bb != NULL);
    block_to_vertex[bb] = vertex;

    // dumpBlock(vertex);

    // Get the successors
    boost::graph_traits<ControlFlowGraph>::out_edge_iterator ei, ei_end;
    for (boost::tie(ei, ei_end)=out_edges(vertex, cfg); ei!=ei_end; ++ei) {
      CFGVertex successor = target(*ei, cfg);
      if (!post_dominates(successor, vertex)) {
        CFGVertexSet dependants;

        // If we can find a way to get to the END from vertex without going through successor,
        // All nodes that are post-dominated by successor upto the least-common post-dominator
        // of vertex, are control dependant on vertex (FOW'87)
        get_dependents_between(vertex,successor,dependants);
        if (dependents_of.find(vertex) == dependents_of.end())
          dependents_of[vertex] = dependants;
        else
          dependents_of[vertex].insert(dependants.begin(),dependants.end());

        for (CFGVertexSet::iterator it = dependants.begin(); it != dependants.end(); it++) {
          if (depends_on.find(*it) == depends_on.end()) {
            CFGVertexSet d;
            d.insert(vertex);
            depends_on[*it] = d;
          }
          else {
            depends_on[*it].insert(vertex);
          }
        }

        STRACE << "Adding (" << vertex << "," << successor << ")" << LEND;
      }
    }
  }
}

SgAsmX86Instruction* CDG::getControllingInstruction(const CFGVertex& vertex) const {
  SgAsmBlock *bb = get(boost::vertex_name, cfg, vertex);
  assert(bb != NULL);

  SgAsmStatementPtrList & l = bb->get_statementList();
  assert(l.size() > 0);
  SgAsmX86Instruction *lastInsn = isSgAsmX86Instruction(l[l.size()-1]);
  assert(lastInsn);

  if (lastInsn->get_kind() >= x86_ja && lastInsn->get_kind() <= x86_js) {
    return lastInsn;
  }
  else {
    //SERROR << "Control-flow depends on a block that doesn't have a conditional branch..." << LEND;
    return NULL;
  }
}

X86InsnSet CDG::getControlDependencies(const SgAsmX86Instruction *insn) const {
  X86InsnSet out;

  if (insn == NULL) return out;

  // Get the parent block
  SgAsmBlock *bb = insn_get_block(insn);
  assert(bb);
  if (block_to_vertex.find(bb) == block_to_vertex.end()) {
    STRACE << "Parent of " << debug_instruction(insn) << " resides in another function" << LEND;
    return out;
  }

  // Get the controling blocks
  // STRACE << debug_instruction(insn) << " depends on blocks: ";
  const CFGVertex& v = block_to_vertex.at(bb);
  // Previously, this very suspiciously created entries in depends_on due to the undesired
  // creation behavior of the [x] operator.  It's unclear what the impact of that was, but it
  // seems very unlikely to have been intentional.
  if (depends_on.find(v) != depends_on.end()) {
    for (const CFGVertex &o : depends_on.at(v)) {
      SgAsmX86Instruction *c = getControllingInstruction(o);
      if (c)
        out.insert(c);
    }
  }

  return out;
}

Insn2InsnSetMap CDG::getControlDependencies() const {
  Insn2InsnSetMap out;
  for (const SgAsmStatement* bs : fd->get_func()->get_statementList()) {
    const SgAsmBlock *bb = isSgAsmBlock(bs);
    assert(bb);
    for (SgAsmStatement* is : bb->get_statementList()) {
      SgAsmX86Instruction *insn = isSgAsmX86Instruction(is);
      InsnSet temp;
      X86InsnSet deps = getControlDependencies(insn);
      if (deps.size() > 0) {
        for (X86InsnSet::iterator it = deps.begin(); it != deps.end(); it++) {
          temp.insert(*it);
        }
        out[insn] = temp;
      }
    }
  }
  return out;
}

// Dump the strict immediate dominance relationship maps generated by ROSE.
void CDG::dump_dominance_maps() const
{
  // Writing this routine helped clear up a lot of the previous confusion about dominance.
  // Since each node (except the entry node) is immediately dominated _BY_ one and only one
  // node, it makes sense for the map to be keyed by the node being dominated, and the value to
  // be the node doing the dominating, which is of course backwards of the API that most folks
  // would find "natural", which is to list the node doing the dominating first.  Thus we have
  // Wes' strangely named functions, and confusion about which parameter is dominating which.

  OINFO << "Dominance Map contains:" << LEND;
  for (size_t v = 0; v < imm_dom.size(); ++v) {
    const CFGVertex& t = imm_dom[v];
    rose_addr_t addr1 = 0;
    rose_addr_t addr2 = 0;
    SgAsmBlock *bb1 = isSgAsmBlock(get(boost::vertex_name, cfg, v));
    if (bb1 != NULL) addr1 = bb1->get_address();
    OINFO << "  vertex=" << v << " bb=" << addr_str(addr1) << " is immediately dominated by ";

    if (t == NULL_VERTEX) {
      OINFO << "nothing." << LEND;
    }
    else {
      SgAsmBlock *bb2 = isSgAsmBlock(get(boost::vertex_name, cfg, t));
      if (bb2 != NULL) addr2 = bb2->get_address();
      OINFO << "vertex=" << t << " bb=" << addr_str(addr2) << LEND;
    }
  }

  OINFO << "Post-Dominance Map contains:" << LEND;
  for (size_t v = 0; v < imm_post_dom.size() - 1; ++v) {
    const CFGVertex& t = imm_post_dom[v];
    rose_addr_t addr1 = 0;
    rose_addr_t addr2 = 0;
    SgAsmBlock *bb1 = isSgAsmBlock(get(boost::vertex_name, cfg, v));
    if (bb1 != NULL) addr1 = bb1->get_address();
    OINFO << "  vertex=" << v << " bb=" << addr_str(addr1) << " is immediately post-dominated by ";
    if (t == NULL_VERTEX) {
      OINFO << "nothing." << LEND;
    }
    else {
      SgAsmBlock *bb2 = isSgAsmBlock(get(boost::vertex_name, cfg, t));
      if (bb2 != NULL) addr2 = bb2->get_address();
      OINFO << "vertex=" << t << " bb=" << addr_str(addr2) << LEND;
    }
  }
}

void CDG::dumpBlock(const CFGVertex& vertex) const {
  // Get the basic block
  SgAsmBlock *bb = get(boost::vertex_name, cfg, vertex);
  assert(bb != NULL);

  // Dump statements
  SINFO << "CFG Vertex " << vertex << " " << addr_str(bb->get_address()) <<LEND;
  for (const SgAsmStatement* s : bb->get_statementList()) {
    const SgAsmX86Instruction *insn = isSgAsmX86Instruction(s);
    SINFO << debug_instruction(insn) << LEND;
  }
}

void CDG::dumpControlDependencies() const {
  SINFO << "Control dependencies:" << LEND;

  for (size_t y = 0; y < forward_list.size(); y++) {
    CFGVertex v = forward_list.at(y);
    SINFO << "Vertex: " << v << LEND;
    if (depends_on.find(v) != depends_on.end()) {
      SINFO << "  Depends on:";
      for (const CFGVertex& dv : depends_on.at(v)) {
        SINFO << " " << dv;
      }
      SINFO << LEND;
    }
    if (dependents_of.find(v) != dependents_of.end()) {
      SINFO << "  Dependants:";
      for (const CFGVertex& dv : dependents_of.at(v)) {
        SINFO << " " << dv;
      }
      SINFO << LEND;
    }
  }
}

void CDG::dumpInsnDependencies() const {
  for (const SgAsmStatement* bs : fd->get_func()->get_statementList()) {
    const SgAsmBlock *bb = isSgAsmBlock(bs);
    assert(bb);
    for (const SgAsmStatement* is : bb->get_statementList()) {
      const SgAsmX86Instruction *insn = isSgAsmX86Instruction(is);
      SINFO << debug_instruction(insn) << " =f=> ";
      X86InsnSet deps = getControlDependencies(insn);
      if (deps.size() == 0) {
        SINFO << "ENTRY" << LEND;
      } else {
        for (const SgAsmX86Instruction *dep : deps) {
          SINFO << addr_str(dep->get_address()) << " ";
        }
        SINFO << LEND;
      }
    }
  }
}

} // namespace pharos

/* Local Variables:   */
/* mode: c++          */
/* fill-column:    95 */
/* comment-column: 0  */
/* End:               */


#include "modules/neuralNetwork/layer/activation/activation.hpp"
#include "modules/neuralNetwork/neuralNetwork.hpp"

#ifdef _KORALI_USE_CUDNN
  #include "auxiliar/cudaUtils.hpp"
#endif

#ifdef _KORALI_USE_ONEDNN
  #include "auxiliar/dnnUtils.hpp"
using namespace dnnl;
#endif

#include <Eigen/Dense>
using namespace Eigen;

namespace korali
{
namespace neuralNetwork
{
namespace layer
{
;

void Activation::initialize()
{
  // The node count for this layer should be the same as the previous layer
  _outputChannels = _prevLayer->_outputChannels;

  // Checking Layer size
  if (_outputChannels == 0) KORALI_LOG_ERROR("Node count for layer (%lu) should be larger than zero.\n", _index);

  // Checking position
  if (_index == 0) KORALI_LOG_ERROR("Activation layers cannot be the starting layer of the NN\n");
  if (_index == _nn->_layers.size() - 1) KORALI_LOG_ERROR("Activation layers cannot be the last layer of the NN\n");
}

void Activation::createForwardPipeline()
{
  // Calling base layer function
  Layer::createForwardPipeline();

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // If it is an element-wise operation, create an element-wise primitive
    if (_function.rfind("Elementwise", 0) == 0)
    {
      if (_function == "Elementwise/Clip") _activationAlgorithm = algorithm::eltwise_clip;
      if (_function == "Elementwise/Linear") _activationAlgorithm = algorithm::eltwise_linear;
      if (_function == "Elementwise/Log") _activationAlgorithm = algorithm::eltwise_log;
      if (_function == "Elementwise/Logistic") _activationAlgorithm = algorithm::eltwise_logistic;
      if (_function == "Elementwise/ReLU") _activationAlgorithm = algorithm::eltwise_relu;
      if (_function == "Elementwise/SoftReLU") _activationAlgorithm = algorithm::eltwise_soft_relu;
      if (_function == "Elementwise/SoftSign") KORALI_LOG_ERROR("ONEDNN does not support activation functions of type 'Elementwise/SoftSign'.");
      if (_function == "Elementwise/Tanh") _activationAlgorithm = algorithm::eltwise_tanh;

      // Creating descriptor
      auto activationDesc = eltwise_forward::desc(
        _propKind,
        _activationAlgorithm,
        _prevLayer->_outputMem[0].get_desc(),
        _alpha,
        _beta);

      // Create primitive descriptor.
      _forwardEltwiseActivationPrimitiveDesc = eltwise_forward::primitive_desc(activationDesc, _nn->_dnnlEngine);

      // Create the primitive.
      _forwardActivationPrimitive = eltwise_forward(_forwardEltwiseActivationPrimitiveDesc);
    }

    // Check other possible types of activation functions
    if (_function == "Softmax")
    {
      // Creating descriptor
      const int axis = 1;
      auto activationDesc = softmax_forward::desc(_propKind, _prevLayer->_outputMem[0].get_desc(), axis);

      // Create primitive descriptor.
      _forwardSoftmaxActivationPrimitiveDesc = softmax_forward::primitive_desc(activationDesc, _nn->_dnnlEngine);

      // Create the primitive.
      _forwardActivationPrimitive = softmax_forward(_forwardSoftmaxActivationPrimitiveDesc);
    }
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    cudnnErrCheck(cudnnCreateActivationDescriptor(&_activationDesc));
    cudnnActivationMode_t activationMode;

    if (_function == "Elementwise/Clip") activationMode = CUDNN_ACTIVATION_CLIPPED_RELU;
    if (_function == "Elementwise/Linear") activationMode = CUDNN_ACTIVATION_IDENTITY;
    if (_function == "Elementwise/Log") KORALI_LOG_ERROR("Activation function not supported: %s.\n", _function.c_str());
    if (_function == "Elementwise/Logistic") activationMode = CUDNN_ACTIVATION_SIGMOID;
    if (_function == "Elementwise/ReLU") activationMode = CUDNN_ACTIVATION_RELU;
    if (_function == "Elementwise/SoftReLU") KORALI_LOG_ERROR("CUDNN does not support activation functions of type 'Elementwise/SoftSign'.");
    if (_function == "Elementwise/SoftSign") KORALI_LOG_ERROR("CUDNN does not support activation functions of type 'Elementwise/SoftSign'.");
    if (_function == "Elementwise/Tanh") activationMode = CUDNN_ACTIVATION_TANH;
    if (_function == "Softmax") activationMode = CUDNN_ACTIVATION_IDENTITY;

    if (cudnnSetActivationDescriptor(_activationDesc, activationMode, CUDNN_PROPAGATE_NAN, _alpha) != CUDNN_STATUS_SUCCESS) KORALI_LOG_ERROR("Error creating activation algorithm\n");
  }
#endif
}

void Activation::createBackwardPipeline()
{
  // Calling base layer function
  Layer::createBackwardPipeline();

// Creating backward propagation primitives for activation functions
#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // If it is an element-wise operation, create an element-wise backward primitive
    if (_function.rfind("Elementwise", 0) == 0)
    {
      // Creating descriptor
      auto activationDesc = eltwise_backward::desc(_activationAlgorithm, _prevLayer->_outputMem[0].get_desc(), _outputMem[0].get_desc(), _alpha, _beta);

      // Create primitive descriptor.
      auto backwardActivationPrimitiveDesc = eltwise_backward::primitive_desc(activationDesc, _nn->_dnnlEngine, _forwardEltwiseActivationPrimitiveDesc);

      // Create the primitive.
      _backwardActivationPrimitive = eltwise_backward(backwardActivationPrimitiveDesc);
    }

    // Check other possible types of activation functions
    if (_function == "Softmax")
    {
      // Creating descriptor
      const int axis = 1;
      auto activationDesc = softmax_backward::desc(_prevLayer->_outputMem[0].get_desc(), _outputMem[0].get_desc(), axis);

      // Create primitive descriptor.
      auto backwardActivationPrimitiveDesc = softmax_backward::primitive_desc(activationDesc, _nn->_dnnlEngine, _forwardSoftmaxActivationPrimitiveDesc);

      // Create the primitive.
      _backwardActivationPrimitive = softmax_backward(backwardActivationPrimitiveDesc);
    }
  }
#endif
}

void Activation::forwardData(const size_t t)
{
  size_t N = _batchSize;
  size_t OC = _outputChannels;

  if (_nn->_engine == "Korali")
  {
    if (_function == "Elementwise/Clip")
    {
      for (size_t i = 0; i < N * OC; i++)
      {
        if (_prevLayer->_outputValues[i] < _alpha)
          _outputValues[i] = _alpha;
        else if (_prevLayer->_outputValues[i] > _beta)
          _outputValues[i] = _beta;
        else
          _outputValues[i] = _prevLayer->_outputValues[i];
      }
    }
    if (_function == "Elementwise/Linear")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = _prevLayer->_outputValues[i] * _alpha + _beta;
    }
    if (_function == "Elementwise/Log")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = std::log(_prevLayer->_outputValues[i]);
    }
    if (_function == "Elementwise/ReLU")
    {
      for (size_t i = 0; i < N * OC; i++)
        if (_prevLayer->_outputValues[i] > 0.0f)
          _outputValues[i] = _prevLayer->_outputValues[i];
        else
          _outputValues[i] = _prevLayer->_outputValues[i] * _alpha;
    }
    if (_function == "Elementwise/SoftReLU")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = std::log(1.0f + std::exp(_prevLayer->_outputValues[i]));
    }

    if (_function == "Elementwise/Tanh")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = std::tanh(_prevLayer->_outputValues[i]);
    }
    if (_function == "Elementwise/Logistic")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = 1.0f / (1.0f + std::exp(-_prevLayer->_outputValues[i]));
    }
    if (_function == "Elementwise/SoftSign")
    {
      for (size_t i = 0; i < N * OC; i++)
        _outputValues[i] = _prevLayer->_outputValues[i] / (1.0f + std::abs(_prevLayer->_outputValues[i]));
    }
    if (_function == "Softmax")
    {
      for (size_t i = 0; i < N; i++)
      {
        float LSE = logSumExp(&_prevLayer->_outputValues[i * OC], OC);
        for (size_t j = 0; j < OC; j++)
          _outputValues[i * OC + j] = std::exp(_prevLayer->_outputValues[i * OC + j] - LSE);
      }
    }
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Primitive arguments.
    _forwardActivationArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];
    _forwardActivationArgs[DNNL_ARG_DST] = _outputMem[t];

    _forwardActivationPrimitive.execute(_nn->_dnnlStream, _forwardActivationArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    if (_function == "Elementwise/Linear")
    {
      cudaErrCheck(cudaMemcpy(
        _outputTensor[t],
        _prevLayer->_outputTensor[t],
        N * OC * sizeof(float),
        cudaMemcpyDeviceToDevice));
    }
    else if (_function == "Softmax")
    {
      cudnnErrCheck(cudnnSoftmaxForward(
        _nn->_cuDNNHandle,
        CUDNN_SOFTMAX_LOG,
        CUDNN_SOFTMAX_MODE_CHANNEL,
        &_alpha,
        _prevLayer->_outputTensorDesc,
        _prevLayer->_outputTensor[t],
        &_beta,
        _outputTensorDesc,
        _outputTensor[t]));
    }
    else
    {
      cudnnErrCheck(cudnnActivationForward(
        _nn->_cuDNNHandle,
        _activationDesc,
        &_alpha,
        _prevLayer->_outputTensorDesc,
        _prevLayer->_outputTensor[t],
        &_beta,
        _outputTensorDesc,
        _outputTensor[t]));
    }
  }
#endif
}

void Activation::backwardData(const size_t t)
{
  size_t N = _batchSize;
  size_t OC = _outputChannels;

  if (_nn->_mode == "Inference")
    KORALI_LOG_ERROR("Requesting Layer backward data propagation but NN was configured for inference only.\n");

  if (_nn->_engine == "Korali")
  {
    if (_function == "Elementwise/Linear")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] * _alpha;

    if (_function == "Elementwise/Log")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] / _prevLayer->_outputValues[i];

    if (_function == "Elementwise/ReLU")
    {
      for (size_t i = 0; i < N * OC; i++)
        if (_prevLayer->_outputValues[i] > 0.0f)
        {
          _prevLayer->_outputGradient[i] = _outputGradient[i];
        }
        else
        {
          _prevLayer->_outputGradient[i] = _outputGradient[i] * _alpha;
        }
    }
    if (_function == "Elementwise/SoftReLU")
      for (size_t i = 0; i < N * OC; i++)
      {
        const float expOutVal = std::exp(_outputValues[i]);
        _prevLayer->_outputGradient[i] = _outputGradient[i] * (expOutVal - 1.0f) / expOutVal;
      }

    if (_function == "Elementwise/Tanh")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] * (1.0f - _outputValues[i] * _outputValues[i]);

    if (_function == "Elementwise/Logistic")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] * _outputValues[i] * (1.0f - _outputValues[i]);

    if (_function == "Elementwise/SoftSign")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] / ((1.0f + std::abs(_outputValues[i])) * (1.0f + std::abs(_outputValues[i])));

    if (_function == "Softmax")
      for (size_t i = 0; i < N * OC; i++)
        _prevLayer->_outputGradient[i] = _outputGradient[i] * _outputValues[i] * (1.0f - _outputValues[i]);
  }

#ifdef _KORALI_USE_ONEDNN
  if (_nn->_engine == "OneDNN")
  {
    // Primitive arguments.
    _backwardActivationArgs[DNNL_ARG_DIFF_DST] = _outputGradientMem[t];             // Input
    _backwardActivationArgs[DNNL_ARG_SRC] = _prevLayer->_outputMem[t];              // Input
    _backwardActivationArgs[DNNL_ARG_DIFF_SRC] = _prevLayer->_outputGradientMem[t]; // Output
    if (_function == "Softmax") _backwardActivationArgs[DNNL_ARG_DST] = _outputMem[t];

    _backwardActivationPrimitive.execute(_nn->_dnnlStream, _backwardActivationArgs);
  }
#endif

#ifdef _KORALI_USE_CUDNN
  if (_nn->_engine == "CuDNN")
  {
    if (_function == "Elementwise/Linear")
    {
      cudaErrCheck(cudaMemcpy(
        _prevLayer->_outputGradientTensor[t],
        _outputGradientTensor[t],
        N * OC * sizeof(float),
        cudaMemcpyDeviceToDevice));
    }
    else if (_function == "Softmax")
    {
      cudnnErrCheck(cudnnSoftmaxBackward(
        _nn->_cuDNNHandle,
        CUDNN_SOFTMAX_LOG,
        CUDNN_SOFTMAX_MODE_CHANNEL,
        &_alpha,
        _outputTensorDesc,
        _outputTensor[t],
        _outputTensorDesc,
        _outputGradientTensor[t],
        &_beta,
        _prevLayer->_outputTensorDesc,
        _prevLayer->_outputGradientTensor[t]));
    }
    else
    {
      cudnnErrCheck(cudnnActivationBackward(
        _nn->_cuDNNHandle,
        _activationDesc,
        &_alpha,
        _outputTensorDesc,
        _outputTensor[t],
        _outputTensorDesc,
        _outputGradientTensor[t],
        _prevLayer->_outputTensorDesc,
        _prevLayer->_outputTensor[t],
        &_beta,
        _prevLayer->_outputTensorDesc,
        _prevLayer->_outputGradientTensor[t]));
    }
  }
#endif
}

void Activation::setConfiguration(knlohmann::json& js)
{
 if (isDefined(js, "Results"))  eraseValue(js, "Results");

 if (isDefined(js, "Function"))
 {
 try { _function = js["Function"].get<std::string>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ activation ] \n + Key:    ['Function']\n%s", e.what()); }
{
 bool validOption = false;
 if (_function == "Elementwise/Clip") validOption = true;
 if (_function == "Elementwise/Linear") validOption = true;
 if (_function == "Elementwise/Log") validOption = true;
 if (_function == "Elementwise/Logistic") validOption = true;
 if (_function == "Elementwise/ReLU") validOption = true;
 if (_function == "Elementwise/SoftReLU") validOption = true;
 if (_function == "Elementwise/SoftSign") validOption = true;
 if (_function == "Elementwise/Tanh") validOption = true;
 if (_function == "Softmax") validOption = true;
 if (validOption == false) KORALI_LOG_ERROR(" + Unrecognized value (%s) provided for mandatory setting: ['Function'] required by activation.\n", _function.c_str());
}
   eraseValue(js, "Function");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Function'] required by activation.\n");

 if (isDefined(js, "Alpha"))
 {
 try { _alpha = js["Alpha"].get<float>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ activation ] \n + Key:    ['Alpha']\n%s", e.what()); }
   eraseValue(js, "Alpha");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Alpha'] required by activation.\n");

 if (isDefined(js, "Beta"))
 {
 try { _beta = js["Beta"].get<float>();
} catch (const std::exception& e)
 { KORALI_LOG_ERROR(" + Object: [ activation ] \n + Key:    ['Beta']\n%s", e.what()); }
   eraseValue(js, "Beta");
 }
  else   KORALI_LOG_ERROR(" + No value provided for mandatory setting: ['Beta'] required by activation.\n");

 Layer::setConfiguration(js);
 _type = "layer/activation";
 if(isDefined(js, "Type")) eraseValue(js, "Type");
 if(isEmpty(js) == false) KORALI_LOG_ERROR(" + Unrecognized settings for Korali module: activation: \n%s\n", js.dump(2).c_str());
}

void Activation::getConfiguration(knlohmann::json& js)
{

 js["Type"] = _type;
   js["Function"] = _function;
   js["Alpha"] = _alpha;
   js["Beta"] = _beta;
 Layer::getConfiguration(js);
}

void Activation::applyModuleDefaults(knlohmann::json& js)
{

 std::string defaultString = "{\"Alpha\": 1.0, \"Beta\": 0.0}";
 knlohmann::json defaultJs = knlohmann::json::parse(defaultString);
 mergeJson(js, defaultJs);
 Layer::applyModuleDefaults(js);
}

void Activation::applyVariableDefaults()
{

 Layer::applyVariableDefaults();
}

;

} //layer
} //neuralNetwork
} //korali
;