#include <NTL/GF2E.h>

NTL_START_IMPL

NTL_TLS_GLOBAL_DECL(SmartPtr<GF2EInfoT>, GF2EInfo_stg)

NTL_CHEAP_THREAD_LOCAL
GF2EInfoT *GF2EInfo = 0;

GF2EInfoT::GF2EInfoT(const GF2X& NewP)
{
   build(p, NewP);
   _card_exp = p.n;

   long sz = p.size;

// The following crossovers were set using the programs
// GF2EXKarCross.cpp, GF2EXModCross.cpp, GF2EXModCross.cpp,
// and GF2EXGCDCross.cpp.
// To use these programs, one has to remove the #if 0 guards
// in GF2EX.cpp on mul_disable_plain, BuildPlain, and DivRemPlain.

// There are three different configurations that are treated separately:
//   * with gf2x lib and with pclmul instruction available
//   * without gf2x lib but with pclmul
//   * without gf2X lib and without pclmul
// It is possible that one could be using gf2x lib on a platform without
// pclmul, in which case the crossovers used here are not optimal.  It is also
// possible that one could be using gf2x lib with pclmul, but compile NTL with
// NATIVE=off, so that NTL assumes there is no pclmul.  Again, this will lead
// to crossovers that are not optimal.

// The crossovers were calculated based on a Skylake Xeon processor:
// Intel(R) Xeon(R) Gold 6132 CPU @ 2.60GHz.

#if (defined(NTL_GF2X_LIB) && defined(NTL_HAVE_PCLMUL))

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 3;
      else
         KarCross = 4;
   }
   else if (sz <= 6) KarCross = 8;
   else if (sz <= 9) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 15;
      else
         ModCross = 20;
   }
   else if (sz <=  9) ModCross =  60;
   else if (sz <= 18) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross =  50;
      else
         DivCross =  75;
   }
   else if (sz <=  2) DivCross = 100;
   else if (sz <=  3) DivCross = 150;
   else if (sz <=  4) DivCross = 200;
   else if (sz <=  6) DivCross = 250;
   else if (sz <=  9) DivCross = 225;
   else if (sz <= 15) DivCross = 125;
   else if (sz < 125) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 225;
   }
   else if (sz <=  2) GCDCross =  450;
   else if (sz <=  4) GCDCross =  600;
   else if (sz <  12) GCDCross = 1150;
   else               GCDCross =  600;

#elif (defined(NTL_HAVE_PCLMUL))

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 5;
      else
         KarCross = 8;
   }
   else if (sz <= 5) KarCross = 8;
   else if (sz <= 9) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 30;
      else
         ModCross = 45;
   }
   else if (sz <=  2) ModCross = 110;
   else if (sz <=  3) ModCross = 105;
   else if (sz <=  4) ModCross =  65;
   else if (sz <=  5) ModCross =  60;
   else if (sz <=  6) ModCross =  55;
   else if (sz <=  8) ModCross =  50;
   else if (sz <= 12) ModCross =  30;
   else if (sz <= 18) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross =  75;
      else
         DivCross = 125;
   }
   else if (sz <=  2) DivCross = 450;
   else if (sz <=  3) DivCross = 425;
   else if (sz <=  4) DivCross = 375;
   else if (sz <=  6) DivCross = 250;
   else if (sz <=  8) DivCross = 225;
   else if (sz <= 16) DivCross = 125;
   else if (sz <= 45) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 225;
   }
   else if (sz < 12) GCDCross = 1150;
   else              GCDCross =  850;

#else

   //========== KarCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         KarCross = 4;
      else
         KarCross = 12;
   }
   else if (sz <= 3) KarCross = 4;
   else              KarCross = 2;

   //========== ModCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         ModCross = 45;
      else
         ModCross = 65;
   }
   else if (sz <=  2) ModCross =  25;
   else               ModCross =  15;

   //========== DivCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         DivCross = 175;
      else
         DivCross = 250;
   }
   else if (sz <=  4) DivCross = 100;
   else               DivCross =  75;

   //========== GCDCross ==========

   if (sz <= 1) {
      if (deg(p) <= NTL_BITS_PER_LONG/2)
         GCDCross = 225;
      else
         GCDCross = 850;
   }
   else if (sz <  8) GCDCross =  850;
   else if (sz < 12) GCDCross =  600;
   else              GCDCross =  450;

#endif

}

const ZZ& GF2E::cardinality()
{
   if (!GF2EInfo) LogicError("GF2E::cardinality: undefined modulus");

   do { // NOTE: thread safe lazy init
      Lazy<ZZ>::Builder builder(GF2EInfo->_card);
      if (!builder()) break;
      UniquePtr<ZZ> p;
      p.make();
      power(*p, 2, GF2EInfo->_card_exp);
      builder.move(p);
   } while (0);

   return *GF2EInfo->_card;
}

void GF2E::init(const GF2X& p)
{
   GF2EContext c(p);
   c.restore();
}

void GF2EContext::save()
{
   NTL_TLS_GLOBAL_ACCESS(GF2EInfo_stg);
   ptr = GF2EInfo_stg;
}

void GF2EContext::restore() const
{
   NTL_TLS_GLOBAL_ACCESS(GF2EInfo_stg);
   GF2EInfo_stg = ptr;
   GF2EInfo = GF2EInfo_stg.get();
}

GF2EBak::~GF2EBak()
{
   if (MustRestore) c.restore();
}

void GF2EBak::save()
{
   c.save();
   MustRestore = true;
}

void GF2EBak::restore()
{
   c.restore();
   MustRestore = false;
}

const GF2E& GF2E::zero()
{
   static const GF2E z(INIT_NO_ALLOC); // GLOBAL (assumes C++11 thread-safe init)
   return z;
}

istream& operator>>(istream& s, GF2E& x)
{
   GF2X y;

   NTL_INPUT_CHECK_RET(s, s >> y);
   conv(x, y);

   return s;
}

void div(GF2E& x, const GF2E& a, const GF2E& b)
{
   GF2E t;

   inv(t, b);
   mul(x, a, t);
}

void div(GF2E& x, GF2 a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void div(GF2E& x, long a, const GF2E& b)
{
   inv(x, b);
   mul(x, x, a);
}

void inv(GF2E& x, const GF2E& a)
{
   InvMod(x._GF2E__rep, a._GF2E__rep, GF2E::modulus());
}

NTL_END_IMPL


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SDK_SIMD_EXTENSIONS_X86_FMA4_HPP_INCLUDED
#define BOOST_SIMD_SDK_SIMD_EXTENSIONS_X86_FMA4_HPP_INCLUDED

#if defined(__FMA4__)
#  ifndef BOOST_SIMD_HAS_FMA4_SUPPORT
#    define BOOST_SIMD_HAS_FMA4_SUPPORT
#  endif
#elif defined(BOOST_SIMD_HAS_FMA4_SUPPORT) && !defined(_MSC_VER)
#  undef BOOST_SIMD_HAS_FMA4_SUPPORT
#endif

#ifdef BOOST_SIMD_HAS_FMA4_SUPPORT
#  ifndef BOOST_SIMD_HAS_AVX_SUPPORT
#    define BOOST_SIMD_HAS_AVX_SUPPORT
#  endif
#  ifndef BOOST_SIMD_HAS_SSE4A_SUPPORT
#    define BOOST_SIMD_HAS_SSE4A_SUPPORT
#  endif
#endif

#if !defined(BOOST_SIMD_DETECTED) && defined(BOOST_SIMD_HAS_FMA4_SUPPORT)

////////////////////////////////////////////////////////////////////////////////
// FMA4 extensions flags
////////////////////////////////////////////////////////////////////////////////
#define BOOST_SIMD_DETECTED
#define BOOST_SIMD_FMA4
#define BOOST_SIMD_SSE_FAMILY
#define BOOST_SIMD_STRING             "FMA4"
#define BOOST_SIMD_STRING_LIST        "SSE2 SSE3 SSE4A SSSE3 SSE4_1 SSE4_2 AVX FMA4"
#define BOOST_SIMD_BYTES              32
#define BOOST_SIMD_BITS               256
#define BOOST_SIMD_CARDINALS          (2)(4)(8)(16)(32)
#define BOOST_SIMD_TAG_SEQ            (::boost::simd::tag::avx_)(::boost::simd::tag::sse_)

#ifndef BOOST_SIMD_DEFAULT_EXTENSION
#define BOOST_SIMD_DEFAULT_EXTENSION  ::boost::simd::tag::avx_
#endif

#define BOOST_SIMD_DEFAULT_SITE       ::boost::simd::tag::fma4_
#define BOOST_SIMD_SIMD_HAS_ALL_TYPES

// FMA4 header not standardized
#ifdef _MSC_VER
#include <intrin.h>
#else
#include <x86intrin.h>
#include <fma4intrin.h>
#endif

#include <boost/simd/sdk/simd/extensions/meta/sse.hpp>
#include <boost/simd/sdk/simd/extensions/meta/avx.hpp>

#endif
#endif


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

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

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

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

  BOOST_DISPATCH_OVERLOAD ( compare_greater_
                          , (typename X, typename A0)
                          , bd::cpu_
                          , bs::pack_<bd::fundamental_<A0>, X>
                          , bs::pack_<bd::fundamental_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE bool operator()( const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      return compare_less(a1, a0);
    }
  };
} } }

#endif


/*
 * Copyright 2013-2014, Unbounded Robotics Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Unbounded Robotics, Inc. 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: Michael Ferguson

#include <math.h>
#include <Eigen/Eigen>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/convex_hull.h>
#include <simple_grasping/shape_extraction.h>

namespace simple_grasping
{

bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                  const pcl::ModelCoefficients::Ptr model,
                  pcl::PointCloud<pcl::PointXYZRGB>& output,
                  shape_msgs::SolidPrimitive& shape,
                  geometry_msgs::Pose& pose)
{
  // Used to decide between various shapes
  double min_volume = 1000.0;  // the minimum volume shape found thus far.
  Eigen::Matrix3f transformation;  // the transformation for the best-fit shape

  // Compute z height as maximum distance from planes
  double height = 0.0;
  for (size_t i = 0; i < input.size(); ++i)
  {
    Eigen::Vector4f pp(input[i].x, input[i].y, input[i].z, 1);
    Eigen::Vector4f m(model->values[0], model->values[1], model->values[2], model->values[3]);
    double distance_to_plane = fabs(pp.dot(m));
    if (distance_to_plane > height)
      height = distance_to_plane;
  }

  // Project object into 2d, using plane model coefficients
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat(new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::ProjectInliers<pcl::PointXYZRGB> projection;
  projection.setModelType(pcl::SACMODEL_PLANE);
  projection.setInputCloud(input.makeShared());  // stupid API
  projection.setModelCoefficients(model);
  projection.filter(*flat);

  // Rotate plane so that Z=0
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr flat_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
  Eigen::Vector3f normal(model->values[0], model->values[1], model->values[2]);
  Eigen::Quaternionf qz; qz.setFromTwoVectors(normal, Eigen::Vector3f::UnitZ());
  Eigen::Matrix3f plane_rotation = qz.toRotationMatrix();
  Eigen::Matrix3f inv_plane_rotation = plane_rotation.inverse();

  for (size_t i = 0; i < flat->size(); ++i)
  {
    pcl::PointXYZRGB p;
    p.getVector3fMap() = plane_rotation * (*flat)[i].getVector3fMap();
    flat_projected->push_back(p);
  }

  // Find the convex hull
  pcl::PointCloud<pcl::PointXYZRGB> hull;
  pcl::ConvexHull<pcl::PointXYZRGB> convex_hull;
  convex_hull.setInputCloud(flat_projected);
  convex_hull.setDimension(2);
  convex_hull.reconstruct(hull);

  // Try fitting a rectangle
  shape_msgs::SolidPrimitive rect;  // the best-fit rectangle
  rect.type = rect.BOX;
  rect.dimensions.resize(3);
  for (size_t i = 0; i < hull.size() - 1; ++i)
  {
    // For each pair of hull points, determine the angle
    double rise = hull[i+1].y - hull[i].y;
    double run = hull[i+1].x - hull[i].x;
    // and normalize..
    {
      double l = sqrt((rise * rise) + (run * run));
      rise = rise/l;
      run = run/l;
    }

    // Build rotation matrix from change of basis
    Eigen::Matrix3f rotation;
    rotation(0, 0) = run;
    rotation(0, 1) = rise;
    rotation(0, 2) = 0.0;
    rotation(1, 0) = -rise;
    rotation(1, 1) = run;
    rotation(1, 2) = 0.0;
    rotation(2, 0) = 0.0;
    rotation(2, 1) = 0.0;
    rotation(2, 2) = 1.0;
    Eigen::Matrix3f inv_rotation = rotation.inverse();

    // Project hull to new coordinate system
    pcl::PointCloud<pcl::PointXYZRGB> projected_cloud;
    for (size_t j = 0; j < hull.size(); ++j)
    {
      pcl::PointXYZRGB p;
      p.getVector3fMap() = rotation * hull[j].getVector3fMap();
      projected_cloud.push_back(p);
    }

    // Compute min/max
    double x_min = 1000.0;
    double x_max = -1000.0;
    double y_min = 1000.0;
    double y_max = -1000.0;
    for (size_t j = 0; j < projected_cloud.size(); ++j)
    {
      if (projected_cloud[j].x < x_min)
        x_min = projected_cloud[j].x;
      if (projected_cloud[j].x > x_max)
        x_max = projected_cloud[j].x;

      if (projected_cloud[j].y < y_min)
        y_min = projected_cloud[j].y;
      if (projected_cloud[j].y > y_max)
        y_max = projected_cloud[j].y;
    }

    // Is this the best estimate?
    double area = (x_max - x_min) * (y_max - y_min);
    if (area*height < min_volume)
    {
      transformation = inv_plane_rotation * inv_rotation;

      rect.dimensions[0] = (x_max - x_min);
      rect.dimensions[1] = (y_max - y_min);
      rect.dimensions[2] = height;

      Eigen::Vector3f pose3f((x_max + x_min)/2.0, (y_max + y_min)/2.0,
                             projected_cloud[0].z + height/2.0);
      pose3f = transformation * pose3f;
      pose.position.x = pose3f(0);
      pose.position.y = pose3f(1);
      pose.position.z = pose3f(2);

      Eigen::Quaternionf q(transformation);
      pose.orientation.x = q.x();
      pose.orientation.y = q.y();
      pose.orientation.z = q.z();
      pose.orientation.w = q.w();

      min_volume = area * height;
      shape = rect;
    }
  }

  // Try fitting a cylinder
  shape_msgs::SolidPrimitive cylinder;  // the best-fit cylinder
  cylinder.type = cylinder.CYLINDER;
  cylinder.dimensions.resize(2);
  for (size_t i = 0; i < hull.size(); ++i)
  {
    for (size_t j = i + 1; j < hull.size(); ++j)
    {
      // For each pair of hull points determine the center point
      //  between them as a possible cylinder
      pcl::PointXYZRGB p;
      p.x = (hull[i].x + hull[j].x) / 2.0;
      p.y = (hull[i].y + hull[j].y) / 2.0;
      double radius = 0.0;
      // Find radius from this point
      for (size_t k = 0; k < hull.size(); ++k)
      {
        double dx = hull[k].x - p.x;
        double dy = hull[k].y - p.y;
        double r = sqrt((dx * dx) + (dy * dy));
        if (r > radius)
          radius = r;
      }
      // Is this cylinder the best match?
      double volume = M_PI * radius * radius * height;
      if (volume < min_volume)
      {
        transformation = inv_plane_rotation;

        cylinder.dimensions[0] = height;
        cylinder.dimensions[1] = radius;

        Eigen::Vector3f pose3f(p.x, p.y, hull[0].z + height/2.0);
        pose3f = transformation * pose3f;
        pose.position.x = pose3f(0);
        pose.position.y = pose3f(1);
        pose.position.z = pose3f(2);

        min_volume = volume;
        shape = cylinder;
      }
    }
  }

  // TODO: Try fitting a sphere?

  // Project input to new frame
  Eigen::Vector3f origin(pose.position.x, pose.position.y, pose.position.z);
  for (size_t j = 0; j < input.size(); ++j)
  {
    pcl::PointXYZRGB p;
    p.getVector3fMap() = transformation * (input[j].getVector3fMap() - origin);
    output.push_back(p);
  }
  return true;
}

bool extractShape(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                  pcl::PointCloud<pcl::PointXYZRGB>& output,
                  shape_msgs::SolidPrimitive& shape,
                  geometry_msgs::Pose& pose)
{
  // Find lowest point, use as z height
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  coefficients->values.resize(4);
  coefficients->values[0] = 0.0;
  coefficients->values[1] = 0.0;
  coefficients->values[2] = 1.0;
  coefficients->values[3] = 1000.0;  // z-height
  for (size_t i = 0; i < input.size(); ++i)
  {
    if (input[i].z < coefficients->values[3])
      coefficients->values[3] = input[i].z;
  }
  coefficients->values[3] = -coefficients->values[3];
  return extractShape(input, coefficients, output, shape, pose);
}

bool extractUnorientedBoundingBox(const pcl::PointCloud<pcl::PointXYZRGB>& input,
                                  shape_msgs::SolidPrimitive& shape,
                                  geometry_msgs::Pose& pose)
{
  double x_min = 1000.0;
  double x_max = -1000.0;
  double y_min = 1000.0;
  double y_max = -1000.0;
  double z_min = 1000.0;
  double z_max = -1000.0;

  for (size_t i = 0; i < input.size(); ++i)
  {
    if (input[i].x < x_min)
      x_min = input[i].x;
    if (input[i].x > x_max)
      x_max = input[i].x;

    if (input[i].y < y_min)
      y_min = input[i].y;
    if (input[i].y > y_max)
      y_max = input[i].y;

    if (input[i].z < z_min)
      z_min = input[i].z;
    if (input[i].z > z_max)
      z_max = input[i].z;
  }

  pose.position.x = (x_min + x_max)/2.0;
  pose.position.y = (y_min + y_max)/2.0;
  pose.position.z = (z_min + z_max)/2.0;

  shape.type = shape.BOX;
  shape.dimensions.push_back(x_max-x_min);
  shape.dimensions.push_back(y_max-y_min);
  shape.dimensions.push_back(z_max-z_min);

  return true;
}

}  // namespace simple_grasping


#ifndef __wrapper_MSSMNoFV_onshell_soft_parameters_decl_gm2calc_1_3_0_hpp__
#define __wrapper_MSSMNoFV_onshell_soft_parameters_decl_gm2calc_1_3_0_hpp__

#include <cstddef>
#include <Eigen/Core>
#include <ostream>
#include "forward_decls_wrapper_classes.hpp"
#include "gambit/Backends/wrapperbase.hpp"
#include "abstract_MSSMNoFV_onshell_soft_parameters.hpp"
#include "wrapper_MSSMNoFV_onshell_susy_parameters_decl.hpp"

#include "identification.hpp"

namespace CAT_3(BACKENDNAME,_,SAFE_VERSION)
{

   namespace gm2calc
   {

      class MSSMNoFV_onshell_soft_parameters : public MSSMNoFV_onshell_susy_parameters
      {
            // Member variables:
         public:
            // -- Static factory pointers:
            static Abstract_MSSMNoFV_onshell_soft_parameters* (*__factory0)();
            static Abstract_MSSMNoFV_onshell_soft_parameters* (*__factory1)(const gm2calc::MSSMNoFV_onshell_susy_parameters&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, double, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, double, double, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>&, double, double, double);

            // -- Other member variables:

            // Member functions:
         public:
            void print(::std::basic_ostream<char, std::char_traits<char> >& arg_1) const;

            void clear();

            void set_TYd(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYd_);

            void set_TYd(int i, int k, double value);

            void set_TYe(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYe_);

            void set_TYe(int i, int k, double value);

            void set_TYu(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYu_);

            void set_TYu(int i, int k, double value);

            void set_BMu(double BMu_);

            void set_mq2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mq2_);

            void set_mq2(int i, int k, double value);

            void set_ml2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& ml2_);

            void set_ml2(int i, int k, double value);

            void set_mHd2(double mHd2_);

            void set_mHu2(double mHu2_);

            void set_md2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& md2_);

            void set_md2(int i, int k, double value);

            void set_mu2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mu2_);

            void set_mu2(int i, int k, double value);

            void set_me2(const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& me2_);

            void set_me2(int i, int k, double value);

            void set_MassB(double MassB_);

            void set_MassWB(double MassWB_);

            void set_MassG(double MassG_);

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYd() const;

            double get_TYd(int i, int k) const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYe() const;

            double get_TYe(int i, int k) const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_TYu() const;

            double get_TYu(int i, int k) const;

            double get_BMu() const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_mq2() const;

            double get_mq2(int i, int k) const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_ml2() const;

            double get_ml2(int i, int k) const;

            double get_mHd2() const;

            double get_mHu2() const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_md2() const;

            double get_md2(int i, int k) const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_mu2() const;

            double get_mu2(int i, int k) const;

            const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& get_me2() const;

            double get_me2(int i, int k) const;

            double get_MassB() const;

            double get_MassWB() const;

            double get_MassG() const;

            // Wrappers for original constructors:
         public:
            MSSMNoFV_onshell_soft_parameters();
            MSSMNoFV_onshell_soft_parameters(const gm2calc::MSSMNoFV_onshell_susy_parameters& arg_1, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYd_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYe_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& TYu_, double BMu_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mq2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& ml2_, double mHd2_, double mHu2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& md2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& mu2_, const ::Eigen::Matrix<double, 3, 3, 0, 3, 3>& me2_, double MassB_, double MassWB_, double MassG_);

            // Special pointer-based constructor:
            MSSMNoFV_onshell_soft_parameters(Abstract_MSSMNoFV_onshell_soft_parameters* in);

            // Copy constructor:
            MSSMNoFV_onshell_soft_parameters(const MSSMNoFV_onshell_soft_parameters& in);

            // Assignment operator:
            MSSMNoFV_onshell_soft_parameters& operator=(const MSSMNoFV_onshell_soft_parameters& in);

            // Destructor:
            ~MSSMNoFV_onshell_soft_parameters();

            // Returns correctly casted pointer to Abstract class:
            Abstract_MSSMNoFV_onshell_soft_parameters* get_BEptr() const;

      };
   }

}

#include "gambit/Backends/backend_undefs.hpp"

#endif /* __wrapper_MSSMNoFV_onshell_soft_parameters_decl_gm2calc_1_3_0_hpp__ */


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

#ifndef CRYPTO3_MAC_HMAC_HPP
#define CRYPTO3_MAC_HMAC_HPP

#include <nil/crypto3/detail/pack.hpp>

#include <nil/crypto3/mac/detail/hmac/accumulator.hpp>
#include <nil/crypto3/mac/detail/hmac/hmac_policy.hpp>

#include <nil/crypto3/hash/hash_state.hpp>
#include <nil/crypto3/hash/algorithm/hash.hpp>

#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>

namespace nil {
    namespace crypto3 {
        namespace mac {
            /*!
             * @brief
             * @tparam Hash
             * @ingroup mac
             */
            template<typename Hash>
            class hmac {
                typedef detail::hmac_policy<Hash> policy_type;

                typedef typename policy_type::byte_type byte_type;
                typedef typename policy_type::word_type word_type;

                typedef typename policy_type::construction_type construction_type;
                typedef typename construction_type::endian_type endian_type;

                constexpr static const std::size_t block_bytes = policy_type::block_bits / CHAR_BIT;

            public:
                typedef Hash hash_type;

                constexpr static const std::size_t block_bits = policy_type::block_bits;
                constexpr static const std::size_t block_words = policy_type::block_words;
                typedef typename policy_type::block_type block_type;

                constexpr static const std::size_t key_bits = policy_type::key_bits;
                constexpr static const std::size_t key_words = policy_type::key_words;
                typedef typename policy_type::key_type key_type;

                constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                typedef typename policy_type::digest_type digest_type;

                hmac(const key_type &key) {
                    schedule_key(key);
                }

            protected:
                void schedule_key(const key_type &key) {
                    m_hash->clear();

                    const uint8_t ipad = 0x36;
                    const uint8_t opad = 0x5C;

                    std::fill(ikey.begin(), ikey.end(), ipad);
                    std::fill(okey.begin(), okey.end(), opad);

                    if (length > m_hash->hash_block_size()) {
                        secure_vector<uint8_t> hmac_key = m_hash->process(key, length);
                        xor_buf(ikey, hmac_key, hmac_key.size());
                        xor_buf(okey, hmac_key, hmac_key.size());
                    } else {
                        xor_buf(ikey, key, length);
                        xor_buf(okey, key, length);
                    }

                    m_hash->update(m_ikey);
                }

            private:
                std::array<byte_type, block_bytes> ikey, okey;
            };
        }    // namespace mac
    }        // namespace crypto3
}    // namespace nil

#endif


#define BOOST_TEST_MODULE pichi method test

#include <boost/test/unit_test.hpp>
#include <pichi/crypto/method.hpp>

using namespace pichi::crypto;

BOOST_AUTO_TEST_SUITE(METHOD)

BOOST_AUTO_TEST_CASE(KEY_SIZE_Test)
{
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::RC4_MD5>);
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::BF_CFB>);
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::AES_128_CTR>);
  BOOST_CHECK_EQUAL(24, KEY_SIZE<CryptoMethod::AES_192_CTR>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::AES_256_CTR>);
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::AES_128_CFB>);
  BOOST_CHECK_EQUAL(24, KEY_SIZE<CryptoMethod::AES_192_CFB>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::AES_256_CFB>);
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::CAMELLIA_128_CFB>);
  BOOST_CHECK_EQUAL(24, KEY_SIZE<CryptoMethod::CAMELLIA_192_CFB>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::CAMELLIA_256_CFB>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::CHACHA20>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::SALSA20>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::CHACHA20_IETF>);
  BOOST_CHECK_EQUAL(16, KEY_SIZE<CryptoMethod::AES_128_GCM>);
  BOOST_CHECK_EQUAL(24, KEY_SIZE<CryptoMethod::AES_192_GCM>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::AES_256_GCM>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::CHACHA20_IETF_POLY1305>);
  BOOST_CHECK_EQUAL(32, KEY_SIZE<CryptoMethod::XCHACHA20_IETF_POLY1305>);
}

BOOST_AUTO_TEST_CASE(IV_SIZE_Test)
{
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::RC4_MD5>);
  BOOST_CHECK_EQUAL(8, IV_SIZE<CryptoMethod::BF_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_128_CTR>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_192_CTR>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_256_CTR>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_128_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_192_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_256_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::CAMELLIA_128_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::CAMELLIA_192_CFB>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::CAMELLIA_256_CFB>);
  BOOST_CHECK_EQUAL(8, IV_SIZE<CryptoMethod::CHACHA20>);
  BOOST_CHECK_EQUAL(8, IV_SIZE<CryptoMethod::SALSA20>);
  BOOST_CHECK_EQUAL(12, IV_SIZE<CryptoMethod::CHACHA20_IETF>);
  BOOST_CHECK_EQUAL(16, IV_SIZE<CryptoMethod::AES_128_GCM>);
  BOOST_CHECK_EQUAL(24, IV_SIZE<CryptoMethod::AES_192_GCM>);
  BOOST_CHECK_EQUAL(32, IV_SIZE<CryptoMethod::AES_256_GCM>);
  BOOST_CHECK_EQUAL(32, IV_SIZE<CryptoMethod::CHACHA20_IETF_POLY1305>);
  BOOST_CHECK_EQUAL(32, IV_SIZE<CryptoMethod::XCHACHA20_IETF_POLY1305>);
}

BOOST_AUTO_TEST_CASE(NONCE_SIZE_Test)
{
  BOOST_CHECK_EQUAL(12, NONCE_SIZE<CryptoMethod::AES_128_GCM>);
  BOOST_CHECK_EQUAL(12, NONCE_SIZE<CryptoMethod::AES_192_GCM>);
  BOOST_CHECK_EQUAL(12, NONCE_SIZE<CryptoMethod::AES_256_GCM>);
  BOOST_CHECK_EQUAL(12, NONCE_SIZE<CryptoMethod::CHACHA20_IETF_POLY1305>);
  BOOST_CHECK_EQUAL(24, NONCE_SIZE<CryptoMethod::XCHACHA20_IETF_POLY1305>);
}

BOOST_AUTO_TEST_CASE(TAG_SIZE_Test)
{
  BOOST_CHECK_EQUAL(16, TAG_SIZE<CryptoMethod::AES_128_GCM>);
  BOOST_CHECK_EQUAL(16, TAG_SIZE<CryptoMethod::AES_192_GCM>);
  BOOST_CHECK_EQUAL(16, TAG_SIZE<CryptoMethod::AES_256_GCM>);
  BOOST_CHECK_EQUAL(16, TAG_SIZE<CryptoMethod::CHACHA20_IETF_POLY1305>);
  BOOST_CHECK_EQUAL(16, TAG_SIZE<CryptoMethod::XCHACHA20_IETF_POLY1305>);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * vector_filterMain.c
 *
 *  Created on: Nov 12, 2013
 *      Author: hxin
 */

//#ifndef BOOST_PP_IS_ITERATING
//#include "print.h"
#include <stdint.h>
#include "vector_filter.h"
#include <stdio.h>
#include <string.h>
#include <nmmintrin.h>
#include <emmintrin.h>

#include "mask.h"
/*
 #include <boost/preprocessor/repetition.hpp>
 #include <boost/preprocessor/iteration.hpp>
 #include <boost/preprocessor/arithmetic.hpp>
 #include <boost/preprocessor/punctuation/comma_if.hpp>

 #define SSE_BIT_LENGTH		128
 #define BASE_SIZE			2
 #define SSE_BASE_NUM		BOOST_PP_DIV(SSE_BIT_LENGTH, BASE_SIZE)
 #define BYTE_BASE_NUM		BOOST_PP_DIV(8, BASE_SIZE)
 #define SSE_BYTE_NUM		BOOST_PP_DIV(SSE_BIT_LENGTH, 8)

 uint8_t MASK_SSE_END[SSE_BIT_LENGTH * SSE_BYTE_NUM / BASE_SIZE] = {

 #define BOOST_PP_ITERATION_LIMITS	(0, SSE_BIT_LENGTH / 2  - 1)
 #define BOOST_PP_FILENAME_1			"vector_filterMain.c" // this file
 #include BOOST_PP_ITERATE()

 #else // BOOST_PP_IS_ITERATING

 #define I		BOOST_PP_ITERATION()
 #define PRINT_DATA(z, n, data) data

 #define FF_NUM	BOOST_PP_DIV(I, BYTE_BASE_NUM)
 BOOST_PP_ENUM(FF_NUM, PRINT_DATA, 0xff)

 #if		FF_NUM != 0
 BOOST_PP_COMMA()
 #endif	//FF_NUM != 0

 #if		BOOST_PP_MOD(I, BYTE_BASE_NUM) == 1
 0x03
 #elif	BOOST_PP_MOD(I, BYTE_BASE_NUM) == 2
 0x0f
 #elif	BOOST_PP_MOD(I, BYTE_BASE_NUM) == 3
 0x3f
 #else
 0x00
 #endif	//End of switch

 #define ZZ_NUM	BOOST_PP_SUB( BOOST_PP_SUB(SSE_BYTE_NUM, 1), FF_NUM)

 #if		ZZ_NUM != 0
 BOOST_PP_COMMA()
 #endif	//ZZ_NUM != 0

 BOOST_PP_ENUM(ZZ_NUM, PRINT_DATA, 0x00)

 #if I != BOOST_PP_ITERATION_FINISH()
 BOOST_PP_COMMA()
 #endif // I != BOOST_PP_ITERATION_FINISH()

 #undef	FF_NUM
 #undef	ZZ_NUM
 #undef	I
 #undef	PRINT_DATA

 #endif // BOOST_PP_IS_ITERATING
 #ifndef BOOST_PP_IS_ITERATING
 };

 */

#define _MAX_LENGTH_ 320

char read_t[_MAX_LENGTH_] __aligned__;
char ref_t[_MAX_LENGTH_] __aligned__;

//uint8_t read_bit_t[_MAX_LENGTH_ / 4] __aligned__;
//uint8_t ref_bit_t[_MAX_LENGTH_ / 4] __aligned__;

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

	int length = 128;
	int error = 0;
	int repeat_count = 10000;
	int average_loc = 10;

	strcpy(read_t,
			"AAAAAAAAAAAAAGACTAACCACCTTGTCCTGTTGTCTGTCTGGTCAGCCAATCATTGGGACCACACACCCCAGCATCGTGGACTGCGTGCTGAAGGTGC");
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTTTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATG");
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTCATCAGCTATACGGCTTCGGCAACCTTGAGGGGCCGCGCCCCGTTACCCTTTATACGTTTCCCGGGCAAGCCTTCGTGTGGGAGCATATGTATATGG");
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGTGGCCGCGGCCCGTTACACTTTATACGTTATCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATG");
	strcpy(ref_t,
			"AAAAAAAAAAAAAGACTAACCACCTTGTCCTGTTGTCTGTCTGGTCAGCCAATCATTGGGACCACACACCCCAGCATGGTGGACTGCGTGCTGAAGGGGC");
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCCGTCGGCAACCTTGAGGGGTCGCGCCCCGTTACACTTTATACGTTTACCATTGCAAGCCTTCGTGTCGGAGCATATGTATA");
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCT ACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTTTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATG");
//			"TCGCTAGTTAGCCGGACCTAAAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTCTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATGG");

	if (argc >= 2)
		length = atoi(argv[1]);
	if (argc >= 3)
		error = atoi(argv[2]);
	if (argc >= 4)
		repeat_count = atoi(argv[3]);
	if (argc >= 5)
		average_loc = atoi(argv[4]);

//	for (int i = 0; i < SSE_BIT_LENGTH * SSE_BYTE_NUM / BASE_SIZE1; i++) {
//		if (i % SSE_BYTE_NUM == 0)
//			printf("\n");
//		printf("%x ", MASK_SSE_END1[i]);
//	}

//	if (read_t[repeat_count] = 'A')
//		read_t[repeat_count] = 'C';
//	else
//		read_t[repeat_count] = 'A';

//		printf("\n");

//	while (repeat_count--)
//		bit_vec_filter_sse_simulate11(read_t, ref_t, length, error, average_loc);
//	while (repeat_count--)
//		bit_vec_filter_sse_simulate1(read_t, ref_t, length, error, average_loc);
//	if (bit_vec_filter_sse11(read_t, ref_t, length, error))
//		printf("Pass Filter\n");
//	else
//		printf("Fail Filter\n");
//

	if (bit_vec_filter_sse1(read_t, ref_t, length, error))
		printf("Pass Filter\n");
	else
		printf("Fail Filter\n");

//	strcpy(read_t,
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTTTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATG");
////			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTTTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATGG");
//	strcpy(ref_t,
//			"TCGCTAGTAGCCGGAACTAACAGGTAGGCCTACATCAGCTATACGGCCGTCGGCAACCTTGAGGGGTCGCGCCCCGTTACACTTTATACGTTTACCATTGCAAGCCTTCGTGTCGGAGCATATGTATA");
////			"TCGCTAGTTAGCCGGACCTAAAGGTAGGCCTACATCAGCTATACGGCATCGGCAACCTTGAGGGGCCGCGCCCCGTTACACTTTATACGTTTCCCTTGCAAGCCTTCGTGTCGGAGCATATGTATATGG");
//
//	if (bit_vec_filter_no_flipping_sse1(read_t, ref_t, length, error))
//		printf("Pass Filter\n");
//	else
//		printf("Fail Filter\n");

	return 0;

}

//#endif


//---------------------------------------------------------------------------//
//  MIT License
//
//  Copyright (c) 2020-2021 Mikhail Komarov <nemo@nil.foundation>
//  Copyright (c) 2020-2021 Nikita Kaskov <nemo@nil.foundation>

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

#ifndef FILECOIN_STORAGE_PROOFS_POREP_STACKED_VANILLA_PARAMS_HPP
#define FILECOIN_STORAGE_PROOFS_POREP_STACKED_VANILLA_PARAMS_HPP

#include <array>
#include <string>

#include <boost/filesystem/path.hpp>
#include <boost/log/trivial.hpp>

#include <nil/crypto3/hash/sha2.hpp>
#include <nil/crypto3/hash/algorithm/hash.hpp>

#include <nil/filecoin/storage/proofs/porep/stacked/vanilla/challenges.hpp>
#include <nil/filecoin/storage/proofs/porep/stacked/vanilla/column_proof.hpp>
#include <nil/filecoin/storage/proofs/porep/stacked/vanilla/labelling_proof.hpp>
#include <nil/filecoin/storage/proofs/porep/stacked/vanilla/encoding_proof.hpp>

#include <nil/filecoin/storage/proofs/core/merkle/proof.hpp>
#include <nil/filecoin/storage/proofs/core/merkle/builders.hpp>

namespace nil {
    namespace filecoin {
        namespace stacked {
            namespace vanilla {

                /*************************  Pre-defined values  ***********************************/

                constexpr static const std::size_t BINARY_ARITY = 2;
                constexpr static const std::size_t QUAD_ARITY = 4;
                constexpr static const std::size_t OCT_ARITY = 8;

                /*************************  SetupParams  ***********************************/

                struct SetupParams {
                    // Number of nodes
                    std::size_t nodes;

                    // Base degree of DRG
                    std::size_t degree;

                    std::size_t expansion_degree;

                    std::array<std::uint8_t, 32> porep_id;
                    LayerChallenges layer_challenges;
                };

                /*************************  PublicParams  ***********************************/

                template<typename MerkleTreeType>
                struct PublicParams {
                    typedef MerkleTreeType tree_type;
                    typedef typename tree_type::hash_type hash_type;

                    std::string identifier() {
                        return std::string("layered_drgporep::PublicParams{{ graph:") +
                               std::to_string(graph.identifier()) +
                               ", challenges: " + std::to_string(layer_challenges) +
                               ", tree: " + std::to_string(MerkleTreeType::display()) + "}}";
                    }

                    StackedBucketGraph<hash_type> graph;
                    LayerChallenges layer_challenges;
                };

                /*************************  Tau  ***********************************/

                /// Tau for a single parition.
                template<typename DDomain, typename EDomain>
                struct Tau {
                    EDomain comm_d;
                    DDomain comm_r;
                };

                /*************************  PersistentAux  ***********************************/

                /// Stored along side the sector on disk.
                template<typename D>
                struct PersistentAux {
                    D comm_c;
                    D comm_r_last;
                };

                /*************************  ???  ***********************************/

                typedef std::function<void(const StoreConfig &, std::size_t, std::size_t)> VerifyCallback;

                /*************************  Labels  ***********************************/

                template<typename MerkleTreeType>
                struct Labels {
                    Labels(const std::vector<StoreConfig> &labels) : labels(labels) {
                    }

                    void verify_stores(VerifyCallback callback, const boost::filesystem::path &cache_dir) {
                        std::vector<StoreConfig> updated_path_labels = labels;
                        std::size_t required_configs = get_base_tree_count<MerkleTreeType>();
                        for (const auto &label : updated_path_labels) {
                            label.path = cache_dir;
                            callback(label, BINARY_ARITY, required_configs);
                        }
                    }

                    DiskStore<typename MerkleTreeType::hash_type::digest_type> labels_for_layer(std::size_t layer) {
                        BOOST_ASSERT_MSG(layer != 0, "Layer cannot be 0");
                        BOOST_ASSERT_MSG(
                            layer <= layers(),
                            std::format("Layer {%d} is not available (only {%d} layers available)", layer, layers()));

                        std::size_t row_index = layer - 1;
                        StoreConfig config = labels[row_index];
                        assert(config.size.is_some());

                        DiskStore::new_from_disk(config.size, MerkleTreeType::base_arity, config);
                    }

                    /// Returns label for the last layer.
                    DiskStore<typename MerkleTreeType::hash_type::digest_type> labels_for_last_layer() {
                        return labels_for_layer(labels.size() - 1);
                    }

                    /// How many layers are available.
                    std::size_t layers() const {
                        return labels.size();
                    }

                    /// Build the column for the given node.
                    Column<typename MerkleTreeType::hash_type> column(std::uint32_t node) {
                        std::vector<typename MerkleTreeType::hash_type::digest_type> rows;

                        for (const auto &label : labels) {
                            assert(label.size());
                            DiskStore store = DiskStore::new_from_disk(label.size, MerkleTreeType::base_arity, label);
                            rows.push_back(store.read_at(node));
                        }

                        return Column<typename MerkleTreeType::hash_type>(node, rows);
                    }

                    /// Update all configs to the new passed in root cache path.
                    void update_root(const boost::filesystem::path &root) {
                        for (const auto &config : labels) {
                            config.path = root;
                        }
                    }

                    std::vector<StoreConfig> labels;
                };

                /*************************  TemporaryAux  ***********************************/

                template<typename MerkleTreeType, typename Hash>
                struct TemporaryAux {
                    void set_cache_path(const boost::filesystem::path &cache_path) {
                        for (labels::iterator label = labels.begin(); label != labels.end(); ++label) {
                            (*label).path = cache_path;
                        }
                        tree_d_config.path = cache_path;
                        tree_r_last_config.path = cache_path;
                        tree_c_config.path = cache_path;
                    }

                    DiskStore<typename MerkleTreeType::hash_type::digest_type> labels_for_layer(std::size_t layer) {
                        return labels.labels_for_layer(layer);
                    }

                    typename MerkleTreeType::hash_type::digest_type domain_node_at_layer(std::size_t layer,
                                                                                std::uint32_t node_index) {
                        return labels_for_layer(layer).read_at(node_index);
                    }

                    Column<typename MerkleTreeType::hash_type> column(std::uint32_t column_index) {
                        return labels.column(column_index);
                    }

                    // 'clear_temp' will discard all persisted merkle and layer data
                    // that is no longer required.
                    void clear_temp(TemporaryAux<MerkleTreeType, Hash> t_aux) {

                        const auto delete_tree_c_store = [&](const StoreConfig &config, std::size_t tree_c_size) {
                            DiskStore<typename MerkleTreeType::hash_type::digest_type> tree_c_store =
                                DiskStore<typename MerkleTreeType::hash_type::digest_type>::new_from_disk(
                                    tree_c_size, MerkleTreeType::base_arity, config);
                            // Note: from_data_store requires the base tree leaf count
                            DiskTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                                     MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>
                                tree_c = DiskTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                                                  MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>::
                                    from_data_store(tree_c_store,
                                                    get_merkle_tree_leafs(tree_c_size, MerkleTreeType::base_arity));
                            tree_c.erase(config.clone());
                        };

                        if (is_cached(&t_aux.tree_d_config)) {
                            std::size_t tree_d_size = t_aux.tree_d_config.size.context("tree_d config has no size");
                            DiskStore<typename Hash::digest_type> tree_d_store =
                                DiskStore::new_from_disk(tree_d_size, BINARY_ARITY, &t_aux.tree_d_config)
                                    .context("tree_d");
                            // Note: from_data_store requires the base tree leaf count
                            BinaryMerkleTree<Hash> tree_d = BinaryMerkleTree<Hash>::from_data_store(
                                tree_d_store, get_merkle_tree_leafs(tree_d_size, BINARY_ARITY));

                            tree_d.erase(t_aux.tree_d_config);
                            BOOST_LOG_TRIVIAL(trace) << "tree d deleted";
                        }

                        std::size_t tree_count = get_base_tree_count<MerkleTreeType>();
                        std::size_t tree_c_size = t_aux.tree_c_config.size;
                        const auto configs = split_config(t_aux.tree_c_config.clone(), tree_count);

                        if (is_cached(&t_aux.tree_c_config)) {
                            delete_tree_c_store(&t_aux.tree_c_config, tree_c_size);
                        } else if (is_cached(configs[0])) {
                            for (configs::const_iterator config = configs.begin(); config != configs.end(); ++config) {
                                // Trees with sub-trees cannot be instantiated and deleted via the existing tree
                                // interface since knowledge of how the base trees are split exists outside of merkle
                                // light.  For now, we manually remove each on disk tree file since we know where they
                                // are here.
                                boost::filesystem::path tree_c_path = StoreConfig::data_path(config->path, config->id);
                                boost::filesystem::remove(tree_c_path);
                            }
                        }
                        BOOST_LOG_TRIVIAL(trace) << "tree c deleted";

                        for (int i = 0; i < t_aux.labels.labels.size(); i++) {
                            StoreConfig cur_config = t_aux.labels.labels[i].clone();
                            if (is_cached(cur_config)) {
                                DiskStore<typename MerkleTreeType::hash_type::digest_type>::delete (cur_config)
                                    .with_context(|| std::format("labels %d", i));
                                BOOST_LOG_TRIVIAL(trace) << std::format("layer %d deleted", i);
                            }
                        }
                    }

                    Labels<MerkleTreeType> labels;
                    StoreConfig tree_d_config;
                    StoreConfig tree_r_last_config;
                    StoreConfig tree_c_config;

                private:
                    bool is_cached(const StoreConfig &config) {
                        return boost::filesystem::exists(
                            boost::filesystem::path(StoreConfig::data_path(config.path, config.id)));
                    };
                };

                /*************************  PublicInputs  ***********************************/

                template<typename T, typename S>
                struct PublicInputs {
                    typedef Tau<T, S> tau_type;
                    typedef std::size_t challenge_type;

                    std::vector<std::size_t> challenges(const LayerChallenges &layer_challenges, std::size_t leaves,
                                                        std::size_t partition_k = 0) {
                        k = partition_k;
                        return layer_challenges.derive<T>(leaves, replica_id, seed, k);
                    }

                    T replica_id;

                    std::array<std::uint8_t, 32> seed;

                    Tau<T, S> tau;

                    /// Partition index
                    std::size_t k;
                };

                /*************************  LabelsCache  ***********************************/

                template<typename MerkleTreeType>
                struct LabelsCache {
                    typedef MerkleTreeType tree_type;
                    typedef typename tree_type::hash_type tree_hash_type;

                    LabelsCache(const Labels<MerkleTreeType> &labels) {
                        std::vector<DiskStore<typename tree_hash_type::digest_type>> disk_store_labels(labels.size());
                        for (int i = 0; i < labels.size(); i++) {
                            disk_store_labels.emplace_back(labels.labels_for_layer(i + 1));
                        }

                        return {disk_store_labels};
                    }

                    std::size_t size() {
                        return labels.size();
                    }

                    bool empty() {
                        return labels.empty();
                    }

                    const DiskStore<typename tree_hash_type::digest_type> &labels_for_layer(std::size_t layer) {
                        BOOST_ASSERT_MSG(layer != 0, "Layer cannot be 0");
                        assert(layer <= layers());

                        std::size_t row_index = layer - 1;
                        return labels[row_index];
                    }

                    /// Returns the labels on the last layer.
                    const DiskStore<typename tree_hash_type::digest_type> &labels_for_last_layer() {
                        return labels[labels.size() - 1];
                    }

                    /// How many layers are available.
                    std::size_t layers() {
                        return labels.size();
                    }

                    /// Build the column for the given node.
                    Column<typename MerkleTreeType::hash_type> column(std::uint32_t node) {
                        std::vector<typename MerkleTreeType::hash_type::digest_type> rows;

                        for (const DiskStore<typename MerkleTreeType::hash_type::digest_type> &l : labels) {
                            rows.push_back(l.read_at(node));
                        }

                        return {node, rows};
                    }

                    std::vector<DiskStore<typename MerkleTreeType::hash_type::digest_type>> labels;
                };

                /*************************  TemporaryAuxCache  ***********************************/

                template<typename MerkleTreeType, typename Hash>
                struct TemporaryAuxCache {
                    typedef MerkleTreeType tree_type;
                    typedef Hash hash_type;

                    typedef typename tree_type::hash_type tree_hash_type;

                    /// The encoded nodes for 1..layers.
                    LabelsCache<tree_type> labels;
                    BinaryMerkleTree<hash_type> tree_d;

                    // Notably this is a LevelCacheTree instead of a full merkle.
                    LCTree<typename tree_type::hash_type, typename tree_type::Arity, typename tree_type::SubTreeArity,
                           typename tree_type::TopTreeArity>
                        tree_r_last;

                    // Store the 'rows_to_discard' value from the tree_r_last
                    // StoreConfig for later use (i.e. proof generation).
                    std::size_t tree_r_last_config_rows_to_discard;

                    DiskTree<tree_hash_type, typename tree_type::Arity, typename tree_type::SubTreeArity,
                             typename tree_type::TopTreeArity>
                        tree_c;
                    TemporaryAux<tree_type, hash_type> t_aux;
                    boost::filesystem::path replica_path;

                    TemporaryAuxCache(const TemporaryAux<tree_type, hash_type> &t_aux,
                                      const boost::filesystem::path &replica_path) {
                        // tree_d_size stored in the config is the base tree size
                        std::size_t tree_d_size = t_aux.tree_d_config.size();
                        std::size_t tree_d_leafs = get_merkle_tree_leafs(tree_d_size, BINARY_ARITY);
                        BOOST_LOG_TRIVIAL(trace)
                            << std::format("Instantiating tree d with size {} and leafs {}", tree_d_size, tree_d_leafs);
                        DiskStore<typename Hash::digest_type> tree_d_store =
                            DiskStore::new_from_disk(tree_d_size, BINARY_ARITY, &t_aux.tree_d_config)
                                .context("tree_d_store");
                        BinaryMerkleTree<hash_type> tree_d =
                            BinaryMerkleTree<Hash>::from_data_store(tree_d_store, tree_d_leafs).context("tree_d");

                        std::size_t tree_count = get_base_tree_count<MerkleTreeType>();
                        std::vector<StoreConfig> configs = split_config(t_aux.tree_c_config.clone(), tree_count);

                        // tree_c_size stored in the config is the base tree size
                        std::size_t tree_c_size = t_aux.tree_c_config.size;
                        BOOST_LOG_TRIVIAL(trace)
                            << std::format("Instantiating tree c [count {}] with size {} and arity {}", tree_count,
                                           tree_c_size, MerkleTreeType::base_arity);
                        DiskTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                                 MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>
                            tree_c = create_disk_tree<
                                DiskTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                                         MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>>(tree_c_size,
                                                                                configs);

                        // tree_r_last_size stored in the config is the base tree size
                        std::size_t tree_r_last_size = t_aux.tree_r_last_config.size;
                        std::size_t tree_r_last_config_rows_to_discard = t_aux.tree_r_last_config.rows_to_discard;
                        const auto(configs, replica_config) = split_config_and_replica(
                            t_aux.tree_r_last_config,
                            replica_path,
                            get_merkle_tree_leafs(tree_r_last_size, MerkleTreeType::base_arity),
                            tree_count);

                        BOOST_LOG_TRIVIAL(trace)
                            << std::format("Instantiating tree r last [count {}] with size {} and arity {}, {}, {}",
                                           tree_count, tree_r_last_size, MerkleTreeType::base_arity,
                                           MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity);
                        LCTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                               MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>
                            tree_r_last =
                                create_lc_tree<LCTree<typename MerkleTreeType::hash_type, MerkleTreeType::base_arity,
                                                      MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>>(
                                    tree_r_last_size, configs, replica_config);

                        return {{t_aux.labels.context("labels_cache")},
                                tree_d,
                                tree_r_last,
                                tree_r_last_config_rows_to_discard,
                                tree_c,
                                replica_path,
                                t_aux};
                    }

                    DiskStore<typename MerkleTreeType::hash_type::digest_type> &labels_for_layer(std::size_t layer) {
                        return labels.labels_for_layer(layer);
                    }

                    typename MerkleTreeType::hash_type::digest_type domain_node_at_layer(std::size_t layer,
                                                                                std::uint32_t node_index) {
                        return labels_for_layer(layer).read_at(node_index);
                    }

                    Column<typename MerkleTreeType::hash_type> column(std::uint32_t column_index) {
                        return labels.column(column_index);
                    }
                };

                /*************************  PrivateInputs  ***********************************/

                template<typename MerkleTreeType, typename Hash>
                struct PrivateInputs {
                    PersistentAux<typename MerkleTreeType::hash_type::digest_type> p_aux;
                    TemporaryAuxCache<MerkleTreeType, Hash> t_aux;
                };

                /*************************  Proof  ***********************************/

                template<typename MerkleTreeType, typename Hash>
                struct Proof {
                    typedef Hash hash_type;
                    typedef MerkleTreeType tree_type;
                    typedef typename tree_type::hash_type tree_hash_type;

                    MerkleProof<hash_type, MerkleTreeType::base_arity, MerkleTreeType::sub_tree_arity,
                                MerkleTreeType::top_tree_arity>
                        comm_d_proofs;
                    MerkleProof<tree_hash_type, MerkleTreeType::base_arity, MerkleTreeType::sub_tree_arity,
                                MerkleTreeType::top_tree_arity>
                        comm_r_last_proof;
                    ReplicaColumnProof<MerkleProof<tree_hash_type, MerkleTreeType::base_arity,
                                                   MerkleTreeType::sub_tree_arity, MerkleTreeType::top_tree_arity>>
                        replica_column_proofs;

                    /// Indexed by layer in 1..layers.
                    std::vector<LabelingProof<typename MerkleTreeType::hash_type>> labeling_proofs;
                    EncodingProof<typename MerkleTreeType::hash_type> encoding_proof;
                };

                /*************************  ReplicaColumnProof  ***********************************/

                template<typename MerkleProofType>
                struct ReplicaColumnProof {
                    typedef MerkleProofType proof_type;

                    ColumnProof<proof_type> c_x;
                    std::vector<ColumnProof<proof_type>> drg_parents;
                    std::vector<ColumnProof<proof_type>> exp_parents;
                };

                /*************************  TransformedLayers  ***********************************/

                template<typename MerkleTreeType, typename Hash>
                using TransformedLayers =
                    std::tuple<Tau<typename MerkleTreeType::hash_type::digest_type, typename Hash::digest_type>,
                               PersistentAux<typename MerkleTreeType::hash_type::digest_type>,
                               TemporaryAux<MerkleTreeType, Hash>>;

                /*************************  ???  ***********************************/

                template<typename Hash, typename InputDataRange>
                typename Hash::digest_type get_node(const InputDataRange &data, std::size_t index) {
                    return Hash::digest_type::try_from_bytes(data_at_node(data, index));
                }

                /// Generate the replica id as expected for Stacked DRG.
                template<typename InputDataRange, typename Hash = crypto3::hashes::sha2<256>>
                typename Hash::digest_type
                    generate_replica_id(const std::array<std::uint8_t, 32> &prover_id, std::uint64_t sector_id,
                                        const std::array<std::uint8_t, 32> &ticket, const InputDataRange &comm_d,
                                        const std::array<std::uint8_t, 32> &porep_seed) {
                    using namespace nil::crypto3;

                    accumulator_set<Hash> acc;

                    hash<Hash>(prover_id, acc);
                    hash<Hash>(sector_id, acc);
                    hash<Hash>(ticket, acc);
                    hash<Hash>(comm_d, acc);
                    hash<Hash>(porep_seed, acc);

                    return accumulators::extract::hash<Hash>(acc);
                }
            }    // namespace vanilla
        }        // namespace stacked
    }            // namespace filecoin
}    // namespace nil

#endif


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceofflineRamp.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"

using namespace  std;
using namespace  Eigen;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

//ros::Publisher pid1 ;

void  SendGazebo(QList<LinkM> links){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;
    data.data=links[1].JointAngle;
    pub1.publish(data);
    data.data=links[2].JointAngle;
    pub2.publish(data);
    data.data=links[3].JointAngle;
    pub3.publish(data);
    data.data=links[4].JointAngle;
    pub4.publish(data);
    data.data=links[5].JointAngle;
    pub5.publish(data);
    data.data=links[6].JointAngle;
    pub6.publish(data);
    data.data=links[7].JointAngle;
    pub7.publish(data);
    data.data=links[8].JointAngle;
    pub8.publish(data);
    data.data=links[9].JointAngle;
    pub9.publish(data);
    data.data=links[10].JointAngle;
    pub10.publish(data);
    data.data=links[11].JointAngle;
    pub11.publish(data);
    data.data=links[12].JointAngle;
    pub12.publish(data);
    data.data=links[13].JointAngle;
    pub13.publish(data);
    data.data=links[14].JointAngle;
    pub14.publish(data);
    data.data=links[15].JointAngle;
    pub15.publish(data);
    data.data=links[16].JointAngle;
    pub16.publish(data);
    data.data=links[17].JointAngle;
    pub17.publish(data);
    data.data=links[18].JointAngle;
    pub18.publish(data);
    data.data=links[19].JointAngle;
    pub19.publish(data);
    data.data=links[20].JointAngle;
    pub20.publish(data);
    data.data=links[21].JointAngle;
    pub21.publish(data);
    data.data=links[22].JointAngle;
    pub22.publish(data);
    data.data=links[23].JointAngle;
    pub23.publish(data);
    data.data=links[24].JointAngle;
    pub24.publish(data);
    data.data=links[25].JointAngle;
    pub25.publish(data);
    data.data=links[26].JointAngle;
    pub26.publish(data);
    data.data=links[27].JointAngle;
    pub27.publish(data);
    data.data=links[28].JointAngle;
    pub28.publish(data);

}

int main(int argc, char **argv)
{
    vector<double> cntrl(13);
    QCgenerator QC;
    //check _timesteps
    QElapsedTimer timer;
    Robot SURENA;
    TaskSpaceOfflineRamp SURENAOffilneTaskSpaceRamp;
    QList<LinkM> links;
    MatrixXd PoseRoot;
    MatrixXd PoseRFoot;
    MatrixXd PoseLFoot;
    double dt;

    double k1;
    double k2;
    double k3;
    double k4;

    bool aState=false;
    bool bState=false;
    bool cState=false;
    bool dState=false;
    bool LeftFootLanded=false;

    double teta_motor_L=0;
    double teta_motor_R=0;
    double phi_motor_L=0;
    double phi_motor_R=0;

    int numberOfLeftFootSensorData=0;
    double footSensorSaturation=90;//if all sensors data are bigger than this amount, this means the foot is landed on the ground
    double footSensorthreshold=4;
    double hipRoll=0;
    double StartTime=0;
    double WalkTime=0;
    double RollTime=0;
    double  DurationOfStartPhase=6;
    double  DurationOfendPhase=6;
    //SURENAOffilneTaskSpaceRamp.GetAccVelPos();
    bool startPhase=true;
    bool endPhase=true;
    bool walk=true;
    PoseRoot.resize(6,1);
    PoseRFoot.resize(6,1);
    PoseLFoot.resize(6,1);

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    //ros::Subscriber sub = nh.subscribe("/foot", 1000, receiveFootSensor);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);

    //pid1= nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/pid/parameter_updates",1000);

    ros::Rate loop_rate(100);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    k1=0.00009;
    k2=0.00009;
    k3=0.00001;
    k4=0.00001;

    while (ros::ok())
    {
        // qDebug()<<StartTime;

        if (startPhase==true && StartTime<=DurationOfStartPhase) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.95100,0.8300;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<0,DurationOfStartPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpaceRamp._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpaceRamp.GetAccVelPos(CoefZStart,StartTime,0,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }

        if (StartTime>DurationOfStartPhase && StartTime<(DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime)){

            bool walk=true;
            double m1;
            double m2;
            double m3;
            double m4;
            double m5;
            double m6;
            double m7;
            double m8;
            StartTime=StartTime+SURENAOffilneTaskSpaceRamp._timeStep;
            //qDebug()<<StartTime;
            MatrixXd P;
            if(walk==true){
                MatrixXd m=SURENAOffilneTaskSpaceRamp.AnkleTrajectory(SURENAOffilneTaskSpaceRamp.globalTime);
                m1=m(0,0);
                m2=m(1,0);
                m3=m(2,0);
                m4=m(3,0);
                m5=m(4,0);
                m6=m(5,0);
                m7=m(6,0);
                m8=m(7,0);

                P=SURENAOffilneTaskSpaceRamp.PelvisTrajectory (SURENAOffilneTaskSpaceRamp.globalTime);

                // just some samples for plotting!!!
                //                SURENAOffilneTaskSpaceRamp.CoMXVector.append(P(0,0));
                //                SURENAOffilneTaskSpaceRamp.timeVector.append(SURENAOffilneTaskSpaceRamp.globalTime);
                //                SURENAOffilneTaskSpaceRamp.LeftFootXTrajectory.append(m1);

                SURENAOffilneTaskSpaceRamp.globalTime=SURENAOffilneTaskSpaceRamp.globalTime+SURENAOffilneTaskSpaceRamp._timeStep;

                if (round(SURENAOffilneTaskSpaceRamp.globalTime)<=round(SURENAOffilneTaskSpaceRamp.MotionTime)){

                    //cout<<SURENAOffilneTaskSpaceRamp.TSS<<endl;
                    //cout<<RollTime<<endl;

                    PoseRoot<<P(0,0),
                            P(1,0),
                            P(2,0),
                            0,
                            0,
                            0;

                    PoseRFoot<<m5,
                            m6,
                            m7,
                            0,
                            1*m8,
                            0;

                    PoseLFoot<<m1,
                            m2,
                            m3,
                            0,
                            1*m4,
                            0;

                    SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
                    SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

                }

            }
        }

        if (endPhase==true &&  StartTime>=(DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime) && StartTime<=DurationOfendPhase+DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.830,0.95100;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime,DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime+DurationOfendPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpaceRamp._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpaceRamp.GetAccVelPos(CoefZStart,StartTime,DurationOfStartPhase+SURENAOffilneTaskSpaceRamp.MotionTime,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }
        links = SURENA.GetLinks();
        msg.data.clear();

        //        ROS_INFO("I heard motor pitch: [%f]",teta_motor_L);
        //        ROS_INFO("I heard motor roll: [%f]",phi_motor_L);
        msg.data.clear();

                cntrl[0]=0.0;
                cntrl[1]=links[1].JointAngle;
                cntrl[2]=links[2].JointAngle;
                cntrl[3]=links[3].JointAngle;
                cntrl[4]=links[4].JointAngle;
                cntrl[5]=links[5].JointAngle;//+teta_motor_R;//pitch
                cntrl[6]=links[6].JointAngle;//roll
                cntrl[7]=links[7].JointAngle;
                cntrl[8]=links[8].JointAngle;
                cntrl[9]=links[9].JointAngle;
                cntrl[10]=links[10].JointAngle;

                cntrl[11]=links[11].JointAngle;
                cntrl[12]=links[12].JointAngle;

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        for(int  i = 0;i < 12;i++)
        {
            msg.data.push_back(qref[i]);

        }
        // std::string varAsString = std::to_string(qref[i-1]);
        // msg.data =varAsString;
        SendGazebo(links);
        // SendGazeboPID();
        chatter_pub.publish(msg);
        //  ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


#include "../src/ForceFields/GayBerneFF.h"
#include "../src/ForceFields/ForceFieldManager.h"
#include "../src/Simulation/StandardSimulation.h"
#include "../src/Moves/MoveManager.h"
#include "../src/Moves/TranslateMove.h"
#include "../src/Particles/Particle.h"

#include "../src/Worlds/World.h"
#include "../src/Worlds/WorldManager.h"
#include "../src/Constraints/Constraint.h"
#include "TestAccumulator.h"
#include "json/json.h"
#include "gtest/gtest.h"
#include <fstream>

#ifdef MULTI_WALKER
#include <boost/mpi.hpp>
using namespace boost;
#endif

using namespace SAPHRON;

// Verified against OpenMD 2.4.
TEST(GayBerneFF, DefaultBehavior)
{
	double rcut = 55.0;
	GayBerneFF ff(16.27, 16.27, 3.254, 3.254, 418.0, 10.0, 1.0, 0.20, {rcut}, 1.0, 2.0);

	Particle s1({0.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, "L1");
	Particle s2({0.0, 0.0, 0.0}, {0.0, 0.0, 1.0}, "L1");
	s1.AddNeighbor(&s2);
	s2.AddNeighbor(&s1);

	s2.SetPosition({16.6855, 0, 0});
	auto NB = ff.Evaluate(s1, s2, s1.GetPosition() - s2.GetPosition(), 0);
	ASSERT_NEAR(NB.energy, -2823.5, 1e-1);

	s2.SetPosition({0, 0, 32.5468});
	NB = ff.Evaluate(s1, s2, s1.GetPosition() - s2.GetPosition(), 0);
	ASSERT_NEAR(NB.energy, -0.1129, 1e-4);
}

// Tests conservation of energy in a system with GB
// particles and no skin thickness. (rcut = nlist).
TEST(GayBerneFF, NoSkinConservation)
{
	#ifdef MULTI_WALKER
	mpi::environment env;
	mpi::communicator comm;
	#endif

	// Load file (assumes we are in build folder).
	std::ifstream t("../test/gb_no_skin.json");
	std::stringstream buffer;
	buffer << t.rdbuf();

	// Read JSON.
	Json::Reader reader;
	Json::Value root;
	ASSERT_TRUE(reader.parse(buffer, root));

	// Build world.
	WorldManager wm;
	World* w;
	ASSERT_NO_THROW(w = World::Build(root["worlds"][0], root["blueprints"]));
	ASSERT_NE(nullptr, w);
	wm.AddWorld(w);

	ASSERT_EQ(338, w->GetParticleCount());
	ASSERT_EQ(5.0, w->GetNeighborRadius());
	ASSERT_EQ(0.0, w->GetSkinThickness());

	// Gay Berne interaction.
	ForceFieldManager ffm;
	GayBerneFF ff(1.0, 1.0, 3.0, 3.0, 1.0, 1.0, 5.0, 1.0, {5.0}, 2.0, 1.0);
	ffm.AddNonBondedForceField("GB", "GB", ff);

	// Get constraints.
	std::vector<Constraint*> constraints;
	ASSERT_NO_THROW(Constraint::BuildConstraints(root["forcefields"]["constraints"], &ffm, &wm, constraints));

	// Get moves.
	MoveManager mm;
	MoveList moves;
	ASSERT_NO_THROW(Move::BuildMoves(root["moves"], &mm, &wm, moves));

	// Initialize simulation.
	StandardSimulation sim(&wm, &ffm, &mm);

	sim.Run(100);

	// Conservation of energy and pressure.
	auto H = ffm.EvaluateEnergy(*w);
	ASSERT_NEAR(H.energy.total(), w->GetEnergy().total(), 1e-10);

	delete w;
	for(auto& m : mm)
		delete m;

	for(auto& c : constraints)
		delete c;
}


/*
 * spot_prediction_ext.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/small.h>
#include <scitbx/boost_python/container_conversions.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;
  using namespace scitbx::boost_python::container_conversions;

  void export_index_generator();
  void export_reeke_index_generator();
  void export_solve_quad();
  void export_rotation_angles();
  void export_ray_predictor();
  void export_scan_varying_ray_predictor();
  void export_stills_ray_predictor();
  void export_ray_intersection();
  void export_reflection_predictor();
  void export_pixel_labeller();
  void export_pixel_to_miller_index();

  BOOST_PYTHON_MODULE(dials_algorithms_spot_prediction_ext) {
    tuple_mapping_fixed_capacity<scitbx::af::small<double, 2> >();

    export_index_generator();
    export_reeke_index_generator();
    export_solve_quad();
    export_rotation_angles();
    export_ray_predictor();
    export_scan_varying_ray_predictor();
    export_stills_ray_predictor();
    export_ray_intersection();
    export_reflection_predictor();
    export_pixel_labeller();
    export_pixel_to_miller_index();
  }

}}}  // namespace dials::algorithms::boost_python


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_DIGITS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_CONSTANT_INCLUDE_CONSTANTS_DIGITS_HPP_INCLUDED

#include <boost/simd/toolbox/constant/include/constants/mone.hpp>
#include <boost/simd/toolbox/constant/include/constants/mtwo.hpp>
#include <boost/simd/toolbox/constant/include/constants/mthree.hpp>
#include <boost/simd/toolbox/constant/include/constants/mfour.hpp>
#include <boost/simd/toolbox/constant/include/constants/mfive.hpp>
#include <boost/simd/toolbox/constant/include/constants/msix.hpp>
#include <boost/simd/toolbox/constant/include/constants/mseven.hpp>
#include <boost/simd/toolbox/constant/include/constants/meight.hpp>
#include <boost/simd/toolbox/constant/include/constants/mnine.hpp>
#include <boost/simd/toolbox/constant/include/constants/mten.hpp>

#include <boost/simd/toolbox/constant/include/constants/zero.hpp>

#include <boost/simd/toolbox/constant/include/constants/one.hpp>
#include <boost/simd/toolbox/constant/include/constants/two.hpp>
#include <boost/simd/toolbox/constant/include/constants/three.hpp>
#include <boost/simd/toolbox/constant/include/constants/four.hpp>
#include <boost/simd/toolbox/constant/include/constants/five.hpp>
#include <boost/simd/toolbox/constant/include/constants/six.hpp>
#include <boost/simd/toolbox/constant/include/constants/seven.hpp>
#include <boost/simd/toolbox/constant/include/constants/eight.hpp>
#include <boost/simd/toolbox/constant/include/constants/nine.hpp>
#include <boost/simd/toolbox/constant/include/constants/ten.hpp>
#include <boost/simd/toolbox/constant/include/constants/eleven.hpp>
#include <boost/simd/toolbox/constant/include/constants/twelve.hpp>
#include <boost/simd/toolbox/constant/include/constants/twenty.hpp>

#include <boost/simd/toolbox/constant/include/constants/fact_4.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_5.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_6.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_7.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_8.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_9.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_10.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_11.hpp>
#include <boost/simd/toolbox/constant/include/constants/fact_12.hpp>

#include <boost/simd/toolbox/constant/include/constants/int_splat.hpp>

#endif


/**************************************************************************
 * Copyright (c) 2017-2019 by the mfmg authors                            *
 * All rights reserved.                                                   *
 *                                                                        *
 * This file is part of the mfmg library. mfmg is distributed under a BSD *
 * 3-clause license. For the licensing terms see the LICENSE file in the  *
 * top-level directory                                                    *
 *                                                                        *
 * SPDX-License-Identifier: BSD-3-Clause                                  *
 *************************************************************************/

#ifndef MFMG_HIERARCHY_HPP
#define MFMG_HIERARCHY_HPP

#include <mfmg/common/exceptions.hpp>
#include <mfmg/common/level.hpp>
#include <mfmg/common/mesh_evaluator.hpp>
#include <mfmg/common/utils.hpp>
#include <mfmg/dealii/dealii_hierarchy_helpers.hpp>
#include <mfmg/dealii/dealii_matrix_free_hierarchy_helpers.hpp>
#ifdef MFMG_WITH_CUDA
#include <mfmg/cuda/cuda_hierarchy_helpers.cuh>
#include <mfmg/cuda/cuda_matrix_operator.cuh>
#include <mfmg/cuda/cuda_mesh_evaluator.cuh>
#endif

#include <deal.II/base/timer.h>

#include <boost/property_tree/ptree.hpp>

#include <memory>
#include <vector>

namespace mfmg
{
void timer_enter_subsection(std::shared_ptr<dealii::TimerOutput> const &timer,
                            std::string const &section)
{
  if (timer)
    timer->enter_subsection(section);
}

void timer_leave_subsection(std::shared_ptr<dealii::TimerOutput> const &timer)
{
  if (timer)
    timer->leave_subsection();
}

template <typename VectorType>
std::unique_ptr<HierarchyHelpers<VectorType>>
create_hierarchy_helpers(std::shared_ptr<MeshEvaluator const> evaluator)
{
  std::unique_ptr<HierarchyHelpers<VectorType>> hierarchy_helpers;
  std::string evaluator_type = evaluator->get_mesh_evaluator_type();
  if (evaluator_type == "DealIIMeshEvaluator")
  {
    int const dim = evaluator->get_dim();
    if (dim == 2)
      hierarchy_helpers.reset(new DealIIHierarchyHelpers<2, VectorType>());
    else if (dim == 3)
      hierarchy_helpers.reset(new DealIIHierarchyHelpers<3, VectorType>());
    else
      ASSERT_THROW_NOT_IMPLEMENTED();
  }
  else if (evaluator_type == "DealIIMatrixFreeMeshEvaluator")
  {
    int const dim = evaluator->get_dim();
    if (dim == 2)
      hierarchy_helpers.reset(
          new DealIIMatrixFreeHierarchyHelpers<2, VectorType>());
    else if (dim == 3)
      hierarchy_helpers.reset(
          new DealIIMatrixFreeHierarchyHelpers<3, VectorType>());
    else
      ASSERT_THROW_NOT_IMPLEMENTED();
  }
#ifdef MFMG_WITH_CUDA
  else if (evaluator_type == "CudaMeshEvaluator")
  {
    int const dim = evaluator->get_dim();

    if (dim == 2)
    {
      // Downcast evaluator
      auto const cuda_evaluator =
          std::dynamic_pointer_cast<CudaMeshEvaluator<2> const>(evaluator);
      hierarchy_helpers.reset(new CudaHierarchyHelpers<2, VectorType>(
          cuda_evaluator->get_cuda_handle()));
    }
    else if (dim == 3)
    {
      // Downcast evaluator
      auto const cuda_evaluator =
          std::dynamic_pointer_cast<CudaMeshEvaluator<3> const>(evaluator);
      hierarchy_helpers.reset(new CudaHierarchyHelpers<3, VectorType>(
          cuda_evaluator->get_cuda_handle()));
    }
    else
      ASSERT_THROW_NOT_IMPLEMENTED();
  }
#endif
  else
  {
    ASSERT_THROW_NOT_IMPLEMENTED();
  }
  return hierarchy_helpers;
}

#ifdef MFMG_WITH_CUDA
template <>
std::unique_ptr<HierarchyHelpers<dealii::LinearAlgebra::distributed::Vector<
    double, dealii::MemorySpace::CUDA>>>
create_hierarchy_helpers(std::shared_ptr<MeshEvaluator const> evaluator)
{
  std::unique_ptr<HierarchyHelpers<dealii::LinearAlgebra::distributed::Vector<
      double, dealii::MemorySpace::CUDA>>>
      hierarchy_helpers;
  std::string evaluator_type = evaluator->get_mesh_evaluator_type();
  if (evaluator_type == "CudaMeshEvaluator")
  {
    int const dim = evaluator->get_dim();

    if (dim == 2)
    {
      // Downcast evaluator
      auto const cuda_evaluator =
          std::dynamic_pointer_cast<CudaMeshEvaluator<2> const>(evaluator);
      hierarchy_helpers.reset(
          new CudaHierarchyHelpers<2,
                                   dealii::LinearAlgebra::distributed::Vector<
                                       double, dealii::MemorySpace::CUDA>>(
              cuda_evaluator->get_cuda_handle()));
    }
    else if (dim == 3)
    {
      // Downcast evaluator
      auto const cuda_evaluator =
          std::dynamic_pointer_cast<CudaMeshEvaluator<3> const>(evaluator);
      hierarchy_helpers.reset(
          new CudaHierarchyHelpers<3,
                                   dealii::LinearAlgebra::distributed::Vector<
                                       double, dealii::MemorySpace::CUDA>>(
              cuda_evaluator->get_cuda_handle()));
    }
    else
      ASSERT_THROW_NOT_IMPLEMENTED();
  }
  else
    ASSERT_THROW_NOT_IMPLEMENTED();

  return hierarchy_helpers;
}
#endif

template <typename VectorType>
class Hierarchy
{
public:
  Hierarchy(MPI_Comm comm, std::shared_ptr<MeshEvaluator> evaluator,
            std::shared_ptr<boost::property_tree::ptree> params = nullptr,
            std::shared_ptr<dealii::TimerOutput> timer = nullptr)
      : _timer(timer)
  {
    timer_enter_subsection(_timer, "Setup");
    // Replace by a factory
    auto hierarchy_helpers = create_hierarchy_helpers<VectorType>(evaluator);

    _is_preconditioner = params->get("is preconditioner", true);
    _n_smoothing_steps = params->get("smoother.n_smoothing_steps", 1);

    // TODO: add stopping criteria for levels (number of levels / coarse size)
    int const num_levels = params->get("max levels", 2);
    ASSERT(num_levels > 0, "number of levels specified by \"max levels\" "
                           "parameter must be positive");
    _levels.resize(num_levels);

    _levels[0].set_operator(hierarchy_helpers->get_global_operator(evaluator));
    for (int level_index = 0; level_index < num_levels; level_index++)
    {
      auto &level_fine = _levels[level_index];

      auto a = level_fine.get_operator();

      if (level_index == num_levels - 1)
      {
        if (level_index == 0)
        {
          // When using ML for the full hierarchy, do not zero out initial guess
          params->put("coarse.params.zero starting solution",
                      _is_preconditioner);
        }

        timer_enter_subsection(_timer, "Setup: build coarse solver");
        auto coarse_solver = hierarchy_helpers->build_coarse_solver(a, params);
        level_fine.set_solver(coarse_solver);
        timer_leave_subsection(_timer);

        break;
      }

      auto &level_coarse = _levels[level_index + 1];

      timer_enter_subsection(_timer, "Setup: build smoother");
      auto smoother = hierarchy_helpers->build_smoother(a, params);
      level_fine.set_smoother(smoother);
      timer_leave_subsection(_timer);

      timer_enter_subsection(_timer, "Setup: build restrictor");
      auto restrictor =
          hierarchy_helpers->build_restrictor(comm, evaluator, params);
      level_coarse.set_restrictor(restrictor);
      timer_leave_subsection(_timer);

      std::shared_ptr<Operator<VectorType>> ap;
      bool fast_ap = params->get("fast_ap", false);
      if (fast_ap)
      {
        timer_enter_subsection(_timer, "Setup: fast_ap");
        ap = hierarchy_helpers->fast_multiply_transpose();
        timer_leave_subsection(_timer);
      }
      else
      {
        timer_enter_subsection(_timer, "Setup: ap");
        ap = a->multiply_transpose(restrictor);
        timer_leave_subsection(_timer);
      }

      timer_enter_subsection(_timer, "Setup: build coarse matrix");
      auto a_coarse = restrictor->multiply(ap);
      timer_leave_subsection(_timer);

      level_coarse.set_operator(a_coarse);
    }
    timer_leave_subsection(_timer);
  }

  void vmult(VectorType &x, VectorType const &b) const
  {
    // Apply calls itself recursively which trips the timer so put it here
    timer_enter_subsection(_timer, "Apply");
    apply(b, x, 0);
    timer_leave_subsection(_timer);
  }

  void apply(VectorType const &b, VectorType &x, int level_index = 0) const
  {
    auto const num_levels = _levels.size();

    auto &level_fine = _levels[level_index];
    auto a = level_fine.get_operator();

    if (level_index > 0 || _is_preconditioner)
    {
      // Zero out any garbage in x.
      // The only exception is when it's the finest level in a standalone
      // mode.
      x = 0.;
    }

    if (level_index == num_levels - 1)
    {
      timer_enter_subsection(_timer, "Apply: coarsest level");
      // Coarsest level
      auto coarse_solver = level_fine.get_solver();
      coarse_solver->apply(b, x);
      timer_leave_subsection(_timer);
    }
    else
    {
      timer_enter_subsection(_timer, "Apply: fine levels");
      auto &level_coarse = _levels[level_index + 1];

      auto restrictor = level_coarse.get_restrictor();

      // apply pre-smoother
      auto smoother = level_fine.get_smoother();
      for (unsigned int i = 0; i < _n_smoothing_steps; ++i)
        smoother->apply(b, x);

      // compute residual
      // NOTE: we compute negative residual -r = Ax-b, so that we can avoid
      // using sadd and can just use add
      auto res = level_fine.build_vector();
      a->apply(x, *res);
      res->add(-1., b);

      // restrict residual
      auto b_coarse = level_coarse.build_vector();
      restrictor->apply(*res, *b_coarse);

      // compute coarse grid correction
      auto x_coarse = level_coarse.build_vector();
      apply(*b_coarse, *x_coarse, level_index + 1);

      // update solution
      auto x_correction = level_fine.build_vector();
      restrictor->apply(*x_coarse, *x_correction, OperatorMode::TRANS);

      // NOTE: as we used negative residual, we subtract instead of adding
      // here
      x.add(-1., *x_correction);

      // apply post-smoother
      for (unsigned int i = 0; i < _n_smoothing_steps; ++i)
        smoother->apply(b, x);
      timer_leave_subsection(_timer);
    }
  }

  double grid_complexity() const
  {
    // auto const num_levels = _levels.size();

    // if (num_levels == 0)
    //   return -1.0;

    // auto level0_m = _levels[0].get_operator()->grid_complexity();
    // ASSERT(level0_m, "The size of the finest level operator is 0.");

    // double complexity = level0_m;

    // for (int i = 1; i < num_levels; i++)
    // {
    //   if (i < num_levels - 1)
    //     complexity += _levels[i].get_operator()->grid_complexity();
    //   else
    //   {
    //     // Hierarchy may be continued using a different multigrid
    //     // For direct solvers, this would be equivalent to using
    //     //   _levels[i].get_operator()->grid_complexity()
    //     complexity += _levels[i].get_smoother()->grid_complexity();
    //   }
    // }

    // return complexity / level0_m;

    return 0;
  }

  double operator_complexity() const
  {
    // auto const num_levels = _levels.size();

    // if (num_levels == 0)
    //   return -1.0;

    // auto level0_nnz = _levels[0].get_operator()->operator_complexity();
    // ASSERT(level0_nnz, "The nnz of the finest level operator is 0.");

    // double complexity = level0_nnz;
    // for (int i = 1; i < num_levels; i++)
    // {
    //   if (i < num_levels - 1)
    //     complexity += _levels[i].get_operator()->operator_complexity();
    //   else
    //   {
    //     // Hierarchy may be continued using a different multigrid
    //     // For direct solvers, this would be equivalent to using
    //     //   _levels[i].get_operator()->operator_complexity()
    //     complexity += _levels[i].get_smoother()->operator_complexity();
    //   }
    // }
    // return complexity / level0_nnz;
    return 0;
  }

private:
  std::shared_ptr<dealii::TimerOutput> _timer;
  std::vector<Level<VectorType>> _levels;
  bool _is_preconditioner = true;
  unsigned int _n_smoothing_steps;
};
} // namespace mfmg

#endif // ifdef MFMG_HIERARCHY_HPP