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

#include <vw/FileIO.h>
#include <vw/Image.h>
#include <vw/Cartography.h>
#include <vw/InterestPoint.h>
#include <vw/Math.h>
#include <vw/Math/RANSAC.h>
#include <vw/Mosaic/ImageComposite.h>
#include <asp/ControlNetTK/Equalization.h>
#include <asp/Core/Common.h>
#include <asp/Core/Macros.h>
#include <limits>

#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
namespace po = boost::program_options;

using std::cout;
using std::endl;
using std::string;

using namespace vw;
using namespace vw::cartography;
using namespace vw::ip;

// Allows FileIO to correctly read/write these pixel types
namespace vw {
  template<> struct PixelFormatID<Vector3>   { static const PixelFormatEnum value = VW_PIXEL_GENERIC_3_CHANNEL; };
}

template <int dim>
class HomogeneousTransformFunctor : public UnaryReturnSameType {
  Matrix<double,dim+1,dim+1> m_trans;

public:
  HomogeneousTransformFunctor(Matrix<double,dim+1,dim+1> trans) : m_trans(trans) {}

  inline Vector<double,dim> operator()(Vector<double,dim> pt) const {
    if (pt == Vector<double,dim>())
      return pt;

    Vector<double,dim+1> pt_h;
    subvector(pt_h, 0, dim) = pt;
    pt_h[dim] = 1;

    Vector<double,dim+1> result = m_trans * pt_h;
    if (result[dim] != 1)
      result /= result[dim];
    return subvector(result,0,dim);
  }
};

void match_orthoimages(string const& out_prefix,
                       string const& left_image_name,
                       string const& right_image_name,
                       std::vector<InterestPoint> & matched_ip1,
                       std::vector<InterestPoint> & matched_ip2,
                       size_t const& max_points )
{

  vw_out() << "\t--> Finding Interest Points for the orthoimages\n";

  string left_ip_file, right_ip_file;
  ip::ip_filenames(out_prefix, left_image_name, right_image_name,
                   left_ip_file, right_ip_file);
  string match_file = ip::match_filename(out_prefix, left_image_name, right_image_name);

  // Building / Loading Interest point data
  if ( fs::exists(match_file) ) {

    vw_out() << "\t    * Using cached match file.\n";
    read_binary_match_file(match_file, matched_ip1, matched_ip2);
    vw_out() << "\t    * " << matched_ip1.size() << " matches\n";

  } else {
    std::vector<InterestPoint> ip1_copy, ip2_copy;

    if ( !fs::exists(left_ip_file) ||
         !fs::exists(right_ip_file) ) {

      // Worst case, no interest point operations have been performed before
      vw_out() << "\t    * Locating Interest Points\n";
      DiskImageView<PixelGray<float32> > left_disk_image(left_image_name);
      DiskImageView<PixelGray<float32> > right_disk_image(right_image_name);

      // Interest Point module detector code.
      OBALoGInterestOperator obalog_detector(0.03);
      IntegralInterestPointDetector<OBALoGInterestOperator> detector( obalog_detector, 200 );
      std::list<InterestPoint> ip1, ip2;
      vw_out() << "\t    * Processing " << left_image_name << "...\n" << std::flush;
      ip1 = detect_interest_points( left_disk_image, detector );
      vw_out() << "Located " << ip1.size() << " points.\n";
      vw_out() << "\t    * Processing " << right_image_name << "...\n" << std::flush;
      ip2 = detect_interest_points( right_disk_image, detector );
      vw_out() << "Located " << ip2.size() << " points.\n";

      vw_out() << "\t    * Generating descriptors..." << std::flush;
      SGradDescriptorGenerator descriptor;
      descriptor( left_disk_image, ip1 );
      descriptor( right_disk_image, ip2 );
      vw_out() << "done.\n";

      // Writing out the results
      vw_out() << "\t    * Caching interest points: "
               << left_ip_file << " & " << right_ip_file << std::endl;
      write_binary_ip_file( left_ip_file, ip1 );
      write_binary_ip_file( right_ip_file, ip2 );

    }

    vw_out() << "\t    * Using cached IPs.\n";
    ip1_copy = read_binary_ip_file(left_ip_file);
    ip2_copy = read_binary_ip_file(right_ip_file);

    vw_out() << "\t    * Matching interest points\n";
    ip::DefaultMatcher matcher(0.6);

    matcher(ip1_copy, ip2_copy, matched_ip1, matched_ip2,
            TerminalProgressCallback( "asp", "\t    Matching: "));
    ip::remove_duplicates(matched_ip1, matched_ip2);
    vw_out(InfoMessage) << "\t    " << matched_ip1.size() << " putative matches.\n";
    asp::cnettk::equalization( matched_ip1, matched_ip2, max_points );
    vw_out(InfoMessage) << "\t    " << matched_ip1.size() << " thinned matches.\n";

    vw_out() << "\t    * Caching matches: " << match_file << "\n";
    write_binary_match_file( match_file, matched_ip1, matched_ip2);
  }

}

struct Options : public asp::BaseOptions {
  Options() : dem1_nodata(std::numeric_limits<double>::quiet_NaN()), dem2_nodata(std::numeric_limits<double>::quiet_NaN()) {}
  // Input
  string dem1_name, dem2_name, ortho1_name, ortho2_name;
  double dem1_nodata, dem2_nodata;
  size_t max_points;

  // Output
  string output_prefix;
};

void handle_arguments( int argc, char *argv[], Options& opt ) {
  po::options_description general_options("");
  general_options.add_options()
    ("max-match-points", po::value(&opt.max_points)->default_value(800), "The max number of points that will be enforced after matching.")
    ("default-value", po::value(&opt.dem1_nodata), "The value of missing pixels in the first dem")
    ("output-prefix,o", po::value(&opt.output_prefix), "Specify the output prefix.");
  general_options.add( asp::BaseOptionsDescription(opt) );

  po::options_description positional("");
  positional.add_options()
    ("dem1", po::value(&opt.dem1_name), "Explicitly specify the first dem")
    ("dem2", po::value(&opt.dem2_name), "Explicitly specify the second dem")
    ("ortho1", po::value(&opt.ortho1_name), "Explicitly specify the first orthoimage")
    ("ortho2", po::value(&opt.ortho2_name), "Explicitly specify the second orthoimage");

  po::positional_options_description positional_desc;
  positional_desc.add("dem1", 1);
  positional_desc.add("ortho1", 1);
  positional_desc.add("dem2", 1);
  positional_desc.add("ortho2", 1);

  std::string usage("<dem1> <ortho1> <dem2> <ortho2>");
  po::variables_map vm =
    asp::check_command_line( argc, argv, opt, general_options, general_options,
                             positional, positional_desc, usage );

  if ( opt.dem1_name.empty() || opt.dem2_name.empty() ||
       opt.ortho1_name.empty() || opt.ortho2_name.empty() )
    vw_throw( ArgumentErr() << "Missing input files.\n"
              << usage << general_options );
  if ( opt.output_prefix.empty() )
    opt.output_prefix = change_extension(fs::path(opt.dem1_name), "").string();
}

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

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

    DiskImageResourceGDAL ortho1_rsrc(opt.ortho1_name), ortho2_rsrc(opt.ortho2_name),
      dem1_rsrc(opt.dem1_name), dem2_rsrc(opt.dem2_name);

    // Pull out dem nodatas
    if ( dem1_rsrc.has_nodata_read() ) {
      opt.dem1_nodata = dem1_rsrc.nodata_read();
      vw_out() << "\tFound DEM1 input nodata value: "
               << opt.dem1_nodata << endl;
    }
    if ( dem2_rsrc.has_nodata_read() ) {
      opt.dem2_nodata = dem2_rsrc.nodata_read();
      vw_out() << "\tFound DEM2 input nodata value: "
               << opt.dem2_nodata << endl;
    } else {
      vw_out() << "\tMissing nodata value for DEM2. Using DEM1 nodata value.\n";
      opt.dem2_nodata = opt.dem1_nodata;
    }

    typedef DiskImageView<float> dem_type;
    dem_type dem1(opt.dem1_name), dem2(opt.dem2_name);

    GeoReference ortho1_georef, ortho2_georef, dem1_georef, dem2_georef;
    read_georeference(ortho1_georef, ortho1_rsrc);
    read_georeference(ortho2_georef, ortho2_rsrc);
    read_georeference(dem1_georef, dem1_rsrc);
    read_georeference(dem2_georef, dem2_rsrc);

    ImageViewRef<Vector3> pointcloud1 =
      geodetic_to_cartesian( dem_to_geodetic( apply_mask(dem1, opt.dem1_nodata), dem1_georef ), dem1_georef.datum() );
    ImageViewRef<Vector3> pointcloud2 =
      geodetic_to_cartesian( dem_to_geodetic( apply_mask(dem2, opt.dem2_nodata), dem2_georef ), dem2_georef.datum() );

    std::vector<InterestPoint> matched_ip1, matched_ip2;

    match_orthoimages("",
                      opt.ortho1_name, opt.ortho2_name,
                      matched_ip1, matched_ip2, opt.max_points);

    vw_out() << "\t--> Rejecting outliers using RANSAC.\n";
    std::vector<Vector4> ransac_ip1, ransac_ip2;
    for (size_t i = 0; i < matched_ip1.size(); i++) {

      Vector3 xyz1 = pointcloud1( matched_ip1[i].x, matched_ip1[i].y );
      Vector3 xyz2 = pointcloud2( matched_ip2[i].x, matched_ip2[i].y );

      if ( xyz1 != Vector3() &&
           xyz2 != Vector3() ) {
        ransac_ip1.push_back(Vector4(xyz1.x(), xyz1.y(), xyz1.z(), 1));
        ransac_ip2.push_back(Vector4(xyz2.x(), xyz2.y(), xyz2.z(), 1));
      } else {
        vw_out() << "Actually dropped something.\n";
      }
    }

    std::vector<size_t> indices;
    Matrix<double> trans;
    math::RandomSampleConsensus<math::SimilarityFittingFunctorN<3>,math::L2NormErrorMetric>
      ransac( math::SimilarityFittingFunctorN<3>(), math::L2NormErrorMetric(), 2000, 200, 2*ransac_ip1.size()/3, true);
    trans = ransac(ransac_ip2, ransac_ip1);
    indices = ransac.inlier_indices(trans, ransac_ip2, ransac_ip1);

    vw_out() << "\t    * Ransac Result: " << trans << "\n";
    vw_out() << "\t                     # inliers: " << indices.size() << "\n";

    if (1) {
      for ( size_t i = 0; i < ransac_ip1.size(); i++ ) {
        std::cout << ransac_ip1[i] << " " << ransac_ip2[i] << " " << trans * ransac_ip2[i] << " "
                  << norm_2(ransac_ip1[i]-ransac_ip2[i]) << " "
                  << norm_2(ransac_ip1[i]-trans*ransac_ip2[i]) << std::endl;
      }
    }

    { // Saving transform to human readable text
      std::string filename = (fs::path(opt.dem1_name).branch_path() / (fs::basename(fs::path(opt.dem1_name)) + "__" + fs::basename(fs::path(opt.dem2_name)) + "-Matrix.txt")).string();
      std::ofstream ofile( filename.c_str() );
      ofile << std::setprecision(15) << std::flush;
      ofile << "# inliers: " << indices.size() << endl;
      ofile << trans << endl;
      ofile.close();
    }

    // ImageViewRef<Vector3> point_cloud =
    //   geodetic_to_cartesian( dem_to_geodetic( create_mask(dem1_dmg, opt.dem1_nodata), dem1_georef ), dem1_georef.datum() );

    // ImageViewRef<Vector3> point_cloud_trans =
    //   per_pixel_filter(point_cloud, HomogeneousTransformFunctor<3>(trans));

    // DiskImageResourceGDAL point_cloud_rsrc(opt.output_prefix + "-PC.tif",
    //                                        point_cloud_trans.format(),
    //                                        opt.raster_tile_size,
    //                                        opt.gdal_options);
    // block_write_image(point_cloud_rsrc, point_cloud_trans,
    //                   TerminalProgressCallback("asp", "\t--> Transforming: "));
  } ASP_STANDARD_CATCHES;

  return 0;
}


#include <boost/hana/scan_right.hpp>


// *****************************************************************************
//
// Copyright (c) 2014, Southwest Research Institute® (SwRI®)
// 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 Southwest Research Institute® (SwRI®) 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 <COPYRIGHT HOLDER> 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.
//
// *****************************************************************************

#include <swri_image_util/geometry_util.h>

#include <cmath>
#include <limits>
#include <vector>

#include <QPolygonF>
#include <QPointF>

#include <opencv2/imgproc/imgproc.hpp>

#include <Eigen/Dense>
#include <Eigen/SVD>

#include <swri_math_util/constants.h>

namespace swri_image_util
{
  bool Intersects(const BoundingBox& box1, const BoundingBox& box2)
  {
    return (box1 & box2).area() > 0;
  }

  double GetOverlappingArea(const cv::Rect& rect, const cv::Mat& rigid_transform)
  {
    // List of points corresponding to the input rectangle.
    std::vector<cv::Vec2f> points;

    // List of points correspondng to the transformed rectangle.
    std::vector<cv::Vec2f> points_t;

    // Create a point for each corner of the input rectangle.
    points.push_back(cv::Vec2f(rect.x, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y));
    points.push_back(cv::Vec2f(rect.x + rect.width, rect.y + rect.height));
    points.push_back(cv::Vec2f(rect.x, rect.y + rect.height));

    // Transform the input points to the transformed points using the rigid
    // transform.
    cv::transform(cv::InputArray(points), cv::OutputArray(points_t), rigid_transform);

    // Use the QPolygon object to get the intersecting area of the input
    // rectangle and the transformed rectangle.

    // Build the polygon corresponding to the input rectangle.
    QPolygonF polygon;
    polygon << QPointF(points[0][0], points[0][1]);
    polygon << QPointF(points[1][0], points[1][1]);
    polygon << QPointF(points[2][0], points[2][1]);
    polygon << QPointF(points[3][0], points[3][1]);
    polygon << QPointF(points[0][0], points[0][1]);

    // Build the polygon corresponding to the transformed rectangle.
    QPolygonF transformed_polygon;
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);
    transformed_polygon << QPointF(points_t[1][0], points_t[1][1]);
    transformed_polygon << QPointF(points_t[2][0], points_t[2][1]);
    transformed_polygon << QPointF(points_t[3][0], points_t[3][1]);
    transformed_polygon << QPointF(points_t[0][0], points_t[0][1]);

    // Get the polygon representing the intersection of the input rectangle and
    // the transformed rectangle.
    QPolygonF intersection = polygon.intersected(transformed_polygon);

    // If the intersection is empty, then just return 0 area.
    if (intersection.size() == 0)
    {
      return 0;
    }

    // Build an OpenCV contour to measure the area of the intersection.
    std::vector<cv::Point2f> contour;
    for (int i = 0; i < intersection.size(); i++)
    {
      contour.push_back(cv::Point2f(intersection[i].x(), intersection[i].y()));
    }

    // Scale the area based on the scale factor to get the correct value.
    return cv::contourArea(contour);
  }

  cv::Mat ProjectEllipsoid(const cv::Mat& ellipsoid)
  {
    cv::Mat ellipse;

    if (ellipsoid.rows == 3 && ellipsoid.cols == 3 && ellipsoid.type() == CV_32FC1)
    {
      if (ellipsoid.at<float>(2, 2) >= std::numeric_limits<double>::max() * 0.5)
      {
        ellipse.create(2, 2, CV_32FC1);
        ellipse.at<float>(0, 0) = ellipsoid.at<float>(0, 0);
        ellipse.at<float>(0, 1) = ellipsoid.at<float>(0, 1);
        ellipse.at<float>(1, 0) = ellipsoid.at<float>(1, 0);
        ellipse.at<float>(1, 1) = ellipsoid.at<float>(1, 1);

        return ellipse;
      }

      Eigen::Matrix3d A;
      for (size_t r = 0; r < 3; r++)
      {
        for (size_t c = 0; c < 3; c++)
        {
          A(r, c) = ellipsoid.at<float>(r, c);
        }
      }

      // Specify the main vector directions (x and y)
      Eigen::Vector3d ax1;
      Eigen::Vector3d ax2;
      ax1 << 1, 0, 0;
      ax2 << 0, 1, 0;

      // Initialize the normal vector (here the normal vector, in the state
      // space is in the theta direction).
      Eigen::Vector3d n_ax;
      n_ax << 0, 0, 1;

      ///////////////////////
      // Calculate A prime //
      ///////////////////////

      Eigen::Matrix3d A_sym_temp = A.transpose() + A;

      // N_ax_temp = (n_ax*n_ax')
      Eigen::Matrix3d N_ax_temp = n_ax * n_ax.transpose();

      // A_prime_1 = A_sym_temp*N_ax_temp
      //           = (A+A')*(n_ax*n_ax')
      Eigen::Matrix3d A_prime_1 = A_sym_temp * N_ax_temp;

      // A_prime_2 = A_prime_1*A_sym_temp
      //           = (A+A')*(n_ax*n_ax')*(A+A')
      Eigen::Matrix3d A_prime_2 = A_prime_1 * A_sym_temp;

      // scale_1 = n_ax'*A
      Eigen::RowVector3d scale_1 = n_ax.transpose() * A;

      // scale_2 = (scale_1*n_ax)*-4
      //         = (n_ax'*A*n_ax)*-4
      double scale = (scale_1 * n_ax)(0,0) * -4.0;

      // A_temp = scale*A_temp
      //        = scale_2*A = -4*(n_ax'*A*n_ax)*A
      Eigen::Matrix3d A_temp = A * scale;

      // Aprime = A_prime_2 + A_temp
      //        = (A+A')*(n_ax*n_ax')*(A+A') - 4*(n_ax'*A*n_ax)*A
      Eigen::Matrix3d Aprime = A_prime_2 + A_temp;

      ///////////////////////
      // Calculate C prime //
      ///////////////////////

      // C_temp = n_ax'*A
      Eigen::RowVector3d C_temp = n_ax.transpose() * A;

      // Cprime = -4.0*C_temp*n_ax
      //        = -4.0*n_ax'*A*n_ax
      double cp = (-4.0 * C_temp) * n_ax;

      // Bprime = Aprime/Cprime;
      Eigen::Matrix3d Bprime = Aprime / cp;

      // Jp = axes_def;
      //    = [ax1(:),ax2(:)] = [1,0;0,1;0,0];
      Eigen::Matrix<double, 3, 2> Jp;
      Jp << 1, 0,
            0, 1,
            0, 0;

      ///////////////////////
      // Calculate D prime //
      ///////////////////////

      // Dprime_temp = Jp'*Bprime
      Eigen::Matrix<double, 2, 3> Dprime_temp = Jp.transpose() * Bprime;

      // Dprime = Dprime_temp * Jp
      //        = Jp'*Bprime*Jp
      Eigen::Matrix2d Dprime = Dprime_temp * Jp;

      for (size_t r = 0; r < 2; r++)
      {
        for (size_t c = 0; c < 2; c++)
        {
          if (Dprime(r, c) != Dprime(r, c))
          {
            return ellipse;
          }
        }
      }

      ellipse.create(2, 2, CV_32FC1);
      ellipse.at<float>(0, 0) = Dprime(0, 0);
      ellipse.at<float>(0, 1) = Dprime(0, 1);
      ellipse.at<float>(1, 0) = Dprime(1, 0);
      ellipse.at<float>(1, 1) = Dprime(1, 1);
    }

    return ellipse;
  }

  std::vector<tf::Vector3> GetEllipsePoints(
      const cv::Mat& ellipse,
      const tf::Vector3& center,
      double scale,
      int32_t num_points)
  {
    std::vector<tf::Vector3> perimeter;

    if (ellipse.rows == 2 && ellipse.cols == 2 && ellipse.type() == CV_32FC1 &&
        num_points > 2)
    {
      Eigen::Matrix2d Dprime;
      Dprime(0, 0) = ellipse.at<float>(0, 0);
      Dprime(0, 1) = ellipse.at<float>(0, 1);
      Dprime(1, 0) = ellipse.at<float>(1, 0);
      Dprime(1, 1) = ellipse.at<float>(1, 1);

      Eigen::JacobiSVD<Eigen::Matrix2d> svd(Dprime, Eigen::ComputeFullV);

      Eigen::Vector2d Sigma = svd.singularValues();
      Eigen::Matrix2d Vt = svd.matrixV().transpose();

      double xprime_scale = std::sqrt(Sigma(0));
      double yprime_scale = std::sqrt(Sigma(1));

      if (xprime_scale <= 0 || yprime_scale <= 0)
      {
        return perimeter;
      }

      Eigen::MatrixX2d Xp1(num_points, 2);
      for (int32_t i = 0; i < num_points; i++)
      {
        double phi =
            (static_cast<double>(i) / static_cast<double>(num_points))
            * swri_math_util::_2pi;

        Xp1(i, 0) = xprime_scale * std::cos(phi) * scale;
        Xp1(i, 1) = yprime_scale * std::sin(phi) * scale;
      }

      // Xell = Xp1*(V')'
      Eigen::MatrixX2d Xell = Xp1 * Vt;

      perimeter.resize(num_points);
      for (int32_t i = 0; i < num_points; i++)
      {
        perimeter[i].setX(Xell(i, 0) + center.x());
        perimeter[i].setY(Xell(i, 1) + center.y());
      }
    }

    return perimeter;
  }
}


//==================================================================================================
/*!

  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)
*/
//==================================================================================================
#include <boost/simd/constant/expx2c1.hpp>
#include <boost/simd/as.hpp>
#include <stf.hpp>

STF_CASE_TPL( "Check expx2c1 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::expx2c1;
  using boost::simd::Expx2c1;
  T ref = T(0);
  STF_TYPE_IS(decltype(Expx2c1<T>()), T);
  STF_EQUAL(Expx2c1<T>(), ref);
  STF_EQUAL(expx2c1( as(T{}) ),ref);
}

STF_CASE_TPL( "Check expx2c1 behavior for double"
            , (double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::expx2c1;
  using boost::simd::Expx2c1;
  T ref = T(128);

  STF_TYPE_IS(decltype(Expx2c1<T>()), T);
  STF_IEEE_EQUAL(Expx2c1<T>(), ref);
  STF_IEEE_EQUAL(expx2c1( as(T{}) ), ref);
}

STF_CASE_TPL( "Check expx2c1 behavior for float"
            , (float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::expx2c1;
  using boost::simd::Expx2c1;
  T ref = T(32);
  STF_TYPE_IS(decltype(Expx2c1<T>()), T);
  STF_IEEE_EQUAL(Expx2c1<T>(), ref);
  STF_IEEE_EQUAL(expx2c1( as(T{}) ), ref);
}


/**
 * \file dcs/math/policies/policy.hpp
 *
 * \brief Mathematical policy.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2009 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_POLICIES_POLICY_HPP
#define DCS_MATH_POLICIES_POLICY_HPP

#include <boost/math/policies/policy.hpp>

namespace dcs { namespace math { namespace policies {

using ::boost::math::policies::policy;

}}} // Namespace dcs::math::policies

#endif // DCS_MATH_POLICIES_POLICY_HPP


/* ============================================================================

   recurrences_ntl.cpp:  recurrences solved via NTL polynomial arithmetic

   This file is part of hypellfrob (version 2.1.1).

   Copyright (C) 2007, 2008, David Harvey

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License along
   with this program; if not, write to the Free Software Foundation, Inc.,
   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

============================================================================ */

#include <NTL/ZZ_pX.h>
#include <NTL/mat_ZZ_p.h>
#include <NTL/lzz_pX.h>
#include <NTL/mat_lzz_p.h>
#include <cassert>
#include "recurrences_ntl.h"

NTL_CLIENT

namespace hypellfrob {

/* ============================================================================

   Some template stuff

The matrix evaluation code is templated so that it can work over either ZZ_p
or zz_p. There are several template parameters, which can have two settings:

   SCALAR:       ZZ_p           zz_p
   POLY:         ZZ_pX          zz_pX
   VECTOR:       vec_ZZ_p       vec_zz_p
   MATRIX:       mat_ZZ_p       mat_zz_p
   POLYMODULUS:  ZZ_pXModulus   zz_pXModulus
   FFTREP:       FFTRep         fftRep

For the most part NTL uses the same function names for both columns, which
makes life easy, but there's a few I needed to define explicitly:

   to_scalar() convert a ZZ or int into a SCALAR
   forward_fft() runs a forward FFT (either ToFFTRep() or TofftRep())
   inverse_fft() runs an inverse FFT (either FromFFTRep() or FromfftRep())

============================================================================ */

// to_scalar(ZZ)

template <typename SCALAR> SCALAR to_scalar(const ZZ& input);

template<> inline ZZ_p to_scalar<ZZ_p>(const ZZ& input)
{
   return to_ZZ_p(input);
}

template<> inline zz_p to_scalar<zz_p>(const ZZ& input)
{
   return to_zz_p(input);
}

// to_scalar(int)

template <typename SCALAR> SCALAR to_scalar(int input);

template<> inline ZZ_p to_scalar<ZZ_p>(int input)
{
   return to_ZZ_p(input);
}

template<> inline zz_p to_scalar<zz_p>(int input)
{
   return to_zz_p(input);
}

// forward_fft

template <typename POLY, typename FFTREP>
void forward_fft(FFTREP& y, const POLY& x, long k, long lo, long hi);

template<> inline void
forward_fft<ZZ_pX, FFTRep>(FFTRep& y, const ZZ_pX& x, long k, long lo, long hi)
{
   ToFFTRep(y, x, k, lo, hi);
}

template<> inline void
forward_fft<zz_pX, fftRep>(fftRep& y, const zz_pX& x, long k, long lo, long hi)
{
   TofftRep(y, x, k, lo, hi);
}

// inverse_fft

template <typename POLY, typename FFTREP>
void inverse_fft(POLY& x, FFTREP& y, long lo, long hi);

template<> inline void
inverse_fft<ZZ_pX, FFTRep>(ZZ_pX& x, FFTRep& y, long lo, long hi)
{
   FromFFTRep(x, y, lo, hi);
}

template<> inline void
inverse_fft<zz_pX, fftRep>(zz_pX& x, fftRep& y, long lo, long hi)
{
   FromfftRep(x, y, lo, hi);
}

/* ============================================================================

   Dyadic evaluation stuff

This section essentially implements Theorem 8 of [BGS], for the particular
case of the parameters that are used in Theorem 15.

============================================================================ */

/*
Assume that f has degree d = 2^n and that g has degree 2d.

This function computes a polynomial h whose x^d through x^{2d} coefficients
(inclusive) are the same as those of f*g. The bottom d coefficients of h
will be junk.

The parameter g_fft should be the precomputed length 2d FFT of g.

The algorithm in this function is based on the paper "The middle product
algorithm" by Hanrot, Quercia and Zimmermann. (Many thanks to Victor Shoup
for writing such wonderfully modular FFT code. It really made my day.)

*/
template <typename SCALAR, typename POLY, typename FFTREP>
void middle_product(POLY& h, const POLY& f,
                    const POLY& g, const FFTREP& g_fft, int n)
{
   int d = 1 << n;
   h.rep.SetLength(2*d + 1);

   FFTREP f_fft(INIT_SIZE, n+1);

   // Compute length 2d cyclic convolutions of f and g, letting the top
   // third of f*g "wrap around" to the bottom half of the output.
   forward_fft<POLY, FFTREP>(f_fft, f, n+1, 0, 2*d);
   mul(f_fft, f_fft, g_fft);
   inverse_fft<POLY, FFTREP>(h, f_fft, 0, 2*d);

   // Need to correct for the x^{2d} term of g which got wrapped around to the
   // constant term.
   h.rep[d] -= g.rep[2*d] * f.rep[d];

   // Now h contains terms x^d through x^{2d-1} of f*g.
   // To finish off, we just need the x^{2d} term.
   SCALAR temp;
   SCALAR& sum = h.rep[2*d];
   sum = 0;
   for (int i = 0; i <= d; i++)
   {
      mul(temp, f.rep[i], g.rep[2*d-i]);
      add(sum, sum, temp);
   }
}

/*
This struct stores precomputed information that can then be used to shift
evaluation values of a polynomial F(x) of degree d = 2^n.

Specifically, given the values
  F(0), F(b), F(2*b), ..., F(d*b),
the shift() method computes
  F(a), F(a + b), F(a + 2*b), ..., F(a + d*b).

PRECONDITIONS:
   n >= 1
   1, 2, ..., d + 1 are invertible
   a + i*b are invertible for -d <= i <= d

*/
template <typename SCALAR, typename POLY, typename VECTOR, typename FFTREP>
struct DyadicShifter
{
   int d, n;

   // input_twist is a vector of length d/2 + 1.
   // The i-th entry is \prod_{0 <= j <= d, j != i} (i-j)^(-1).
   VECTOR input_twist;

   // output_twist is a vector of length d+1.
   // The i-th entry is b^(-d) \prod_{0 <= j <= d} (a + (i-j)*b).
   VECTOR output_twist;

   // kernel is a polynomial of degree 2d.
   // The coefficients are (a + k*b)^(-1) for -d <= k <= d.
   // We also store its length 2d FFT.
   POLY kernel;
   FFTREP kernel_fft;

   // Polynomials for scratch space in shift()
   POLY scratch, scratch2;

   // Constructor (performs various precomputations)
   DyadicShifter(int n, const SCALAR& a, const SCALAR& b)
   {
      assert(n >= 1);
      this->n = n;
      d = 1 << n;

      // ------------------------ compute input_twist -------------------------

      input_twist.SetLength(d/2 + 1);

      // prod = (d!)^(-1)
      SCALAR prod;
      prod = 1;
      for (int i = 2; i <= d; i++)
         mul(prod, prod, i);
      prod = 1 / prod;

      // input_twist[i] = ((d-i)!)^(-1)
      input_twist[0] = prod;
      for (int i = 1; i <= d/2; i++)
         mul(input_twist[i], input_twist[i-1], d-(i-1));

      // input_twist[i] = ((d-i)!*i!)^(-1)
      prod = input_twist[d/2];
      for (int i = d/2; i >= 0; i--)
      {
         mul(input_twist[i], input_twist[i], prod);
         mul(prod, prod, i);
      }

      // input_twist[i] = \prod_{0 <= j <= d, j != i} (i-j)^(-1)    :-)
      for (int i = 1; i <= d/2; i += 2)
         NTL::negate(input_twist[i], input_twist[i]);

      // ----------------- compute output_twist and kernel --------------------

      output_twist.SetLength(d+1);

      // c[i] = c_i = a + (i-d)*b     for 0 <= i <= 2d
      VECTOR c;
      c.SetLength(2*d+1);
      c[0] = a - d*b;
      for (int i = 1; i <= 2*d; i++)
         add(c[i], c[i-1], b);

      // accum[i] = c_0 * c_1 * ... * c_i    for 0 <= i <= 2d
      VECTOR accum;
      accum.SetLength(2*d+1);
      accum[0] = c[0];
      for (int i = 1; i <= 2*d; i++)
         mul(accum[i], accum[i-1], c[i]);

      // accum_inv[i] = (c_0 * c_1 * ... * c_i)^(-1)    for 0 <= i <= 2d
      VECTOR accum_inv;
      accum_inv.SetLength(2*d+1);
      accum_inv[2*d] = 1 / accum[2*d];
      for (int i = 2*d-1; i >= 0; i--)
         mul(accum_inv[i], accum_inv[i+1], c[i+1]);

      // kernel[i] = (c_i)^(-1)    for 0 <= i <= 2d
      kernel.rep.SetLength(2*d+1);
      kernel.rep[0] = accum_inv[0];
      for (int i = 1; i <= 2*d; i++)
         mul(kernel.rep[i], accum_inv[i], accum[i-1]);

      // precompute transform of kernel
      forward_fft<POLY, FFTREP>(kernel_fft, kernel, n+1, 0, 2*d);

      // output_twist[i] = b^{-d} * c_i * c_{i+1} * ... * c_{i+d}
      // for 0 <= i <= d
      SCALAR factor = power(b, -d);
      SCALAR temp;
      output_twist.SetLength(d+1);
      output_twist[0] = factor * accum[d];
      for (int i = 1; i <= d; i++)
      {
         mul(temp, factor, accum[i+d]);
         mul(output_twist[i], temp, accum_inv[i-1]);
      }
   }

   // Shifts evaluation values as described above.
   // Assumes both output and input have length d + 1.
   void shift(VECTOR& output, const VECTOR& input)
   {
      assert(input.length() == d+1);
      assert(output.length() == d+1);

      // multiply inputs pointwise by input_twist
      scratch.rep.SetLength(d+1);
      for (int i = 0; i <= d/2; i++)
         mul(scratch.rep[i], input[i], input_twist[i]);
      for (int i = 1; i <= d/2; i++)
         mul(scratch.rep[i+d/2], input[i+d/2], input_twist[d/2-i]);

      middle_product<SCALAR, POLY, FFTREP>(scratch2, scratch, kernel,
                                           kernel_fft, n);

      // multiply outputs pointwise by output_twist
      for (int i = 0; i <= d; i++)
         mul(output[i], scratch2.rep[i+d], output_twist[i]);
   }
};

/*
Let M0 and M1 be square matrices of size n*n. Let M(x) = M0 + x*M1; this is a
matrix of linear polys in x. Let P(x) = M(x+1) M(x+2) ... M(x+2^s); this is a
matrix of polynomials of degree 2^s. This function computes the values
    P(a), P(a + 2^t), P(a + 2*2^t), ..., P(a + 2^s*2^t).

The output array should have length n^2. Each entry should be a vector of
length 2^s+1, pre-initialised to all zeroes. The (y*n + x)-th vector will be
the values of the (y, x) entries of the above list of matrices. (This data
format is optimised for the case that 2^s+1 is much larger than n.)

PRECONDITIONS:
   0 <= s <= t
   2, 3, ..., 2^t + 1 must be invertible

*/
template <typename SCALAR, typename POLY, typename VECTOR,
          typename MATRIX, typename FFTREP>
void dyadic_evaluation(vector<VECTOR>& output,
                       const MATRIX& M0, const MATRIX& M1,
                       int s, int t, const SCALAR& a)
{
   int n = M0.NumRows();

   // base cases; just evaluate naively
   if (s <= 1)
   {
      MATRIX X[3];

      if (s == 0)
      {
         X[0] = M0 + (a+1) * M1;
         X[1] = M0 + (a+1 + (1 << t)) * M1;
      }
      else
      {
         for (int i = 0; i <= 2; i++)
            X[i] = (M0 + (a+1 + (i << t)) * M1) * (M0 + (a+2 + (i << t)) * M1);
      }

      for (int x = 0; x < n; x++)
      for (int y = 0; y < n; y++)
      for (int i = 0; i < output[0].length(); i++)
         output[y*n + x][i] = X[i][y][x];

      return;
   }

   // General case.
   // Let Q(x) = M(x+1) M(x+2) ... M(x+2^(s-1)).

   // Recursively compute Q(a), Q(a + 2^t), ..., Q(a + 2^(s-1)*2^t).
   vector<VECTOR> X(n*n);
   for (int i = 0; i < n*n; i++)
      X[i].SetLength((1 << (s-1)) + 1);
   dyadic_evaluation<SCALAR, POLY, VECTOR, MATRIX, FFTREP>
                           (X, M0, M1, s-1, t, a);

   // Do precomputations for shifting by 2^(s-1) and by (2^(s-1)+1)*2^t
   SCALAR c, b;
   c = 1 << (s-1);
   b = 1 << t;
   DyadicShifter<SCALAR, POLY, VECTOR, FFTREP> shifter1(s-1, c, b);
   DyadicShifter<SCALAR, POLY, VECTOR, FFTREP> shifter2(s-1, (c + 1) * b, b);

   // Shift by 2^(s-1) to obtain
   // Q(a + 2^(s-1)), Q(a + 2^t + 2^(s-1)), ..., Q(a + 2^(s-1)*2^t + 2^(s-1))
   vector<VECTOR> Y(n*n);
   for (int i = 0; i < n*n; i++)
   {
      Y[i].SetLength((1 << (s-1)) + 1);
      shifter1.shift(Y[i], X[i]);
   }

   // Multiply matrices to obtain
   // P(a), P(a + 2^t), ..., P(a + 2^(s-1)*2^t).
   SCALAR temp;
   for (int i = 0; i <= (1 << (s-1)); i++)
   for (int x = 0; x < n; x++)
   for (int y = 0; y < n; y++)
   for (int z = 0; z < n; z++)
   {
      mul(temp, X[y*n + z][i], Y[z*n + x][i]);
      output[y*n + x][i] += temp;
   }

   // Shift original sequence by (2^(s-1)+1)*2^t to obtain
   // Q(a + (2^(s-1)+1)*2^t), Q(a + (2^(s-1)+2)*2^t), ..., Q(a + (2^s+1)*2^t).
   for (int i = 0; i < n*n; i++)
      shifter2.shift(Y[i], X[i]);

   // Shift again by 2^(s-1) to obtain
   // Q(a + (2^(s-1)+1)*2^t + 2^(s-1)), Q(a + (2^(s-1)+2)*2^t + 2^(s-1)), ...,
   //                                             Q(a + (2^s+1)*2^t + 2^(s-1)).
   for (int i = 0; i < n*n; i++)
      shifter1.shift(X[i], Y[i]);

   // Multiply matrices to obtain
   // P(a + (2^(s-1)+1)*2^t), P(a + (2^(s-1)+2)*2^t), ..., P(a + (2^s+1)*2^t).
   // (we throw out the last one since it's surplus to requirements)
   for (int i = 0; i < (1 << (s-1)); i++)
   for (int x = 0; x < n; x++)
   for (int y = 0; y < n; y++)
   for (int z = 0; z < n; z++)
   {
      mul(temp, Y[y*n + z][i], X[z*n + x][i]);
      output[y*n + x][i + (1 << (s-1)) + 1] += temp;
   }
}

/* ============================================================================

   General evaluation stuff

This section essentially implements Corollary 10 of [BGS].

============================================================================ */

/*
This struct stores the product tree associated to a vector a[0], ..., a[n-1].

The top node stores the polynomial product
     (x - a[0]) ... (x - a[n-1]).
The two children nodes store
     (x - a[0]) ... (x - a[m-1])
and
     (x - a[m]) ... (x - a[n-1])
where m = floor(n/2). This continues recursively until we reach n = 1,
in which case just the polynomial x - a[0] is stored, and no children.

*/
template <typename SCALAR, typename POLY, typename VECTOR>
struct ProductTree
{
   // polynomial product stored at this node
   POLY poly;

   // children for left and right halves, if deg(poly) > 1
   ProductTree* child1;
   ProductTree* child2;

   // These are temp polys used by the Evaluator and Interpolator classes.
   // It's not very hygienic to keep them here... but it makes things more
   // efficient, because we need two temps for each node, and this prevent
   // unnecessary reallocations. (The lengths will be the same on repeated
   // calls to evaluate() and interpolate().)
   POLY scratch1, scratch2;

   // Constructs product tree for the supplied vector.
   ProductTree(const VECTOR& points)
   {
      build(points, 0, points.length());
   }

   ProductTree(const VECTOR& points, int start, int end)
   {
      build(points, start, end);
   }

   // Constructs product tree recursively for the subset [start, end) of
   // the supplied vector.
   void build(const VECTOR& points, int start, int end)
   {
      assert(end - start >= 1);
      assert(start >= 0);
      assert(end <= points.length());

      if (end - start == 1)
      {
         SetCoeff(poly, 1, 1);
         SetCoeff(poly, 0, -points[start]);
      }
      else
      {
         int m = (end - start) / 2;
         child1 = new ProductTree(points, start, start + m);
         child2 = new ProductTree(points, start + m, end);
         mul(poly, child1->poly, child2->poly);
      }
   }

   ~ProductTree()
   {
      if (deg(poly) > 1)
      {
         delete child1;
         delete child2;
      }
   }
};

/*
Given a list of evaluation points a[0], ..., a[n-1], this struct stores some
precomputed information to permit evaluating an arbitrary polynomial at those
points.
*/
template <typename SCALAR, typename POLY,
          typename POLYMODULUS, typename VECTOR>
struct Evaluator
{
   // The product tree for the evaluation points
   ProductTree<SCALAR, POLY, VECTOR>* tree;

   // A list of NTL ZZ_pXModulus/zz_pXModulus objects corresponding to the
   // polynomials in the product tree, in the order that they get used as the
   // tree is traversed in recursive_evaluate().
   vector<POLYMODULUS> moduli;

   // Constructs evaluator object for the given list of evaluation points
   Evaluator(const VECTOR& points)
   {
      assert(points.length() >= 1);
      tree = new ProductTree<SCALAR, POLY, VECTOR>(points);
      moduli.reserve(2*points.length());
      build(tree);
      assert(moduli.size() <= static_cast<size_t>(2*points.length()));
   }

   // Compute modulus objects for each polynomial under the supplied node of
   // the product tree; appends them in traversal order to "moduli".
   void build(const ProductTree<SCALAR, POLY, VECTOR>* node)
   {
      if (deg(node->poly) > 1)
      {
         moduli.push_back(POLYMODULUS(node->poly));
         build(node->child1);
         build(node->child2);
      }
   }

   ~Evaluator()
   {
      delete tree;
   }

   // Evaluates the input polynomial at the evaluation points, writes the
   // results to output. The output array must have the correct length.
   void evaluate(VECTOR& output, const POLY& input)
   {
      recursive_evaluate(output, input, tree, 0, 0);
   }

   // Evaluates the input polynomial at the subset [start, end) of the
   // evaluation points, which should correspond to the supplied product tree
   // node. (The length of the interval is implied by the degree of the poly
   // at that node.) Writes the output to the subset [start, end) of the
   // output array. The index parameter indicates which modulus in "moduli"
   // to use for this node of the tree. The return value is the index for
   // the modulus that should be used immediately after this call.
   int recursive_evaluate(VECTOR& output, const POLY& input,
                          ProductTree<SCALAR, POLY, VECTOR>* node,
                          int start, int index)
   {
      if (deg(node->poly) == 1)
      {
         eval(output[start], input, -coeff(node->poly, 0));
      }
      else
      {
         rem(node->scratch1, input, moduli[index++]);
         index = recursive_evaluate(output, node->scratch1, node->child1,
                                    start, index);
         index = recursive_evaluate(output, node->scratch1, node->child2,
                                    start + deg(node->child1->poly), index);
      }
      return index;
   }
};

/*
Given an integer L >= 1, this struct does some precomputations to permit
interpolating a polynomial whose values at 0, 1, ..., L are known.

PRECONDITIONS:
   1, 2, ..., L must be invertible.

*/
template <typename SCALAR, typename POLY, typename VECTOR>
struct Interpolator
{
   ProductTree<SCALAR, POLY, VECTOR>* tree;
   int L;

   // input_twist is a vector of length L+1.
   // The i-th entry is \prod_{0 <= j <= L, j != i} (i-j)^(-1).
   VECTOR input_twist;

   // vector of length L+1, used in interpolate()
   VECTOR temp;

   // Performs various precomputations for the given L.
   Interpolator(int L)
   {
      this->L = L;
      temp.SetLength(L+1);

      // Build a product tree for the evaluation points
      for (int i = 0; i <= L; i++)
         temp[i] = i;
      tree = new ProductTree<SCALAR, POLY, VECTOR>(temp);

      // prod = (L!)^(-1)
      SCALAR prod;
      prod = 1;
      for (int i = 2; i <= L; i++)
         mul(prod, prod, i);
      prod = 1 / prod;

      // input_twist[i] = (i!)^(-1),   0 <= i <= L
      input_twist.SetLength(L+1);
      input_twist[L] = prod;
      for (int i = L; i >= 1; i--)
         mul(input_twist[i-1], input_twist[i], i);

      // input_twist[i] = \prod_{0 <= j <= L, j != i} (i-j)^(-1).
      for (int i = 0; i <= L/2; i++)
      {
         mul(input_twist[i], input_twist[i], input_twist[L-i]);
         input_twist[L-i] = input_twist[i];
      }
      for (int i = L-1; i >= 0; i -= 2)
         NTL::negate(input_twist[i], input_twist[i]);
   }

   ~Interpolator()
   {
      delete tree;
   }

   // Returns the polynomial
   //    \sum_{i=start}^{end-1} values[i] * (x-start) (x-start+1) ... (x-end-1)
   // where [start, end) is the interval associated to the supplied product
   // tree node, and where the (x-i) term is omitted in each product.
   void combine(POLY& output, const VECTOR& values,
                ProductTree<SCALAR, POLY, VECTOR>* node, int start)
   {
      if (deg(node->poly) == 1)
      {
         // base case
         clear(output);
         SetCoeff(output, 0, values[start]);
      }
      else
      {
         // recursively build up from two halves
         // i.e. if f1, f2 are the results of "combine" for the two halves,
         // and if p1, p2 are the associated product tree polys, we compute
         // f1*p2 + f2*p1

         combine(node->scratch1, values, node->child1, start);
         mul(output, node->scratch1, node->child2->poly);

         combine(node->scratch1, values, node->child2,
                 start + deg(node->child1->poly));
         mul(node->scratch2, node->scratch1, node->child1->poly);

         add(output, output, node->scratch2);
      }
   }

   // Returns a polynomial F(x) of degree at most L such that F(i) = values[i]
   // for each 0 <= i <= L.
   void interpolate(POLY& output, const VECTOR& values)
   {
      assert(values.length() == L+1);

      // multiply input values pointwise by input_twist; this corrects for
      // the factor (i-0) (i-1) ... (i-L) (where the i-i factor is omitted).
      for (int i = 0; i <= L; i++)
         mul(temp[i], values[i], input_twist[i]);

      // do the interpolation
      combine(output, temp, tree, 0);
   }
};

/* ============================================================================

   Matrix products over arbitrary, relatively short intervals

This section implements something similar to steps 1, 2, ... and the final
refining step of Theorem 15 of [BGS].

============================================================================ */

/*
Let M0 and M1 be matrices of constants. This function evaluates
   M(x) = M0 + x*M1
at x = a.

The output matrix must already have the correct dimensions.

*/
template <typename SCALAR, typename MATRIX>
void eval_matrix(MATRIX& output, const MATRIX& M0, const MATRIX& M1,
                 const SCALAR& a)
{
   int n = M0.NumRows();
   for (int x = 0; x < n; x++)
   for (int y = 0; y < n; y++)
   {
      mul(output[x][y], a, M1[x][y]);
      add(output[x][y], output[x][y], M0[x][y]);
   }
}

/*
Similar to ntl_interval_products. This is used as a subroutine of
ntl_interval_products() to handle the smaller "refining" subintervals.
Its asymptotic complexity theoretically has an extra logarithmic factor
over that of ntl_interval_products().

PRECONDITIONS:
   Let d = sum of lengths of intervals. Then 2, 3, ... 1 + floor(sqrt(d)) must
   all be invertible.

*/
template <typename SCALAR, typename POLY, typename POLYMODULUS,
          typename VECTOR, typename MATRIX>
void ntl_short_interval_products(vector<MATRIX>& output,
                                 const MATRIX& M0, const MATRIX& M1,
                                 const vector<ZZ>& target)
{
   output.clear();

   if (target.size() == 0)
      return;

   int dim = M0.NumRows();
   int num_intervals = target.size() / 2;

   // Determine maximum target interval length
   int max_length = -1;
   for (size_t i = 0; i < target.size(); i += 2)
   {
      int temp = to_ulong(target[i+1] - target[i]);
      if (temp > max_length)
         max_length = temp;
   }

   // Select an appropriate length for the matrix products we'll use
   int L, max_eval_points;
   if (max_length > 2*num_intervals)
   {
      // The intervals are still pretty long relative to the number of
      // intervals, so we're only going to do a single multipoint
      // evaluation.
      L = 1 + to_ulong(SqrRoot(num_intervals * to_ZZ(max_length)));
      max_eval_points = L;
   }
   else
   {
      // The intervals are getting pretty short, so we probably will need
      // to do several shorter multipoint evaluations.
      L = 1 + max_length/2;
      max_eval_points = num_intervals;
   }

   // =========================================================================
   // Step 1: compute entries of M(X, X+L) as polynomials in X.

   vector<POLY> polys(dim*dim);
   {
      // left_accum[i]  = M(L-i-1, L)  for 0 <= i <= L-1
      // right_accum[i] = M(L, L+i+1)  for 0 <= i <= L-1
      vector<MATRIX> left_accum(L), right_accum(L);

      MATRIX temp;
      temp.SetDims(dim, dim);

      left_accum[0].SetDims(dim, dim);
      eval_matrix<SCALAR, MATRIX>(left_accum[0], M0, M1, to_scalar<SCALAR>(L));
      for (int i = L-1; i >= 1; i--)
      {
         eval_matrix<SCALAR, MATRIX>(temp, M0, M1, to_scalar<SCALAR>(i));
         mul(left_accum[L-i], temp, left_accum[L-i-1]);
      }

      right_accum[0].SetDims(dim, dim);
      eval_matrix<SCALAR, MATRIX>(right_accum[0], M0, M1,
                                  to_scalar<SCALAR>(L+1));
      for (int i = 1; i <= L-1; i++)
      {
         eval_matrix<SCALAR, MATRIX>(temp, M0, M1, to_scalar<SCALAR>(L+1+i));
         mul(right_accum[i], right_accum[i-1], temp);
      }

      // Use left_accum and right_accum to compute:
      // initial[i] = M(i, L+i)    for 0 <= i <= L
      // i.e. initial[i] are the values of M(X, X+L) at X = 0, 1, ..., L.
      vector<MATRIX> initial(L+1);
      initial[0] = left_accum.back();
      initial[L] = right_accum.back();
      for (int i = 1; i <= L-1; i++)
         mul(initial[i], left_accum[L-1-i], right_accum[i-1]);

      // Now interpolate entries of initial[i] to get entries of M(X, X+L)
      // as polynomials of degree L.
      Interpolator<SCALAR, POLY, VECTOR> interpolator(L);
      VECTOR values;
      values.SetLength(L+1);
      for (int x = 0; x < dim; x++)
      for (int y = 0; y < dim; y++)
      {
         for (int j = 0; j <= L; j++)
            values[j] = initial[j][y][x];
         interpolator.interpolate(polys[y*dim + x], values);
      }
   }

   // =========================================================================
   // Step 2: decompose intervals into subintervals of length L which we'll
   // attack by direct multipoint evaluation, plus leftover pieces that we'll
   // handle with a recursive call to ntl_short_interval_products().

   // eval_points holds all the values of X for which we want to
   // evaluate M(X, X+L)
   VECTOR eval_points;
   eval_points.SetMaxLength(max_eval_points);

   // leftover_target is the list of leftover intervals that we're going to
   // later do recursively
   vector<ZZ> leftover_target;
   leftover_target.reserve(target.size());

   ZZ current, next;
   for (size_t i = 0; i < target.size(); i += 2)
   {
      current = target[i];
      next = current + L;
      while (next <= target[i+1])
      {
         // [current, next) fits inside this interval, so peel it off into
         // eval_points
         append(eval_points, to_scalar<SCALAR>(current));
         swap(current, next);
         next = current + L;
      }
      if (current < target[i+1])
      {
         // the rest of this interval is too short to handle with M(X, X+L),
         // so put it in the leftover bin
         leftover_target.push_back(current);
         leftover_target.push_back(target[i+1]);
      }
   }

   // =========================================================================
   // Step 3: recursively handle leftover pieces

   // leftover_matrices[i] holds the matrix for leftover interval #i
   vector<MATRIX> leftover_matrices;
   ntl_short_interval_products<SCALAR, POLY, POLYMODULUS, VECTOR, MATRIX>
                    (leftover_matrices, M0, M1, leftover_target);

   // =========================================================================
   // Step 4: evaluate M(X, X+L) at each of the evaluation points. We do this
   // by breaking up the list of evaluation points into blocks of length at
   // most L+1, and using multipoint evaluation on each block.

   // main_matrices[i] will hold M(X, X+L) for the i-th evaluation point X.
   vector<MATRIX> main_matrices(eval_points.length());
   for (size_t i = 0; i < main_matrices.size(); i++)
      main_matrices[i].SetDims(dim, dim);

   VECTOR block, values;
   block.SetMaxLength(L+1);
   values.SetMaxLength(L+1);

   // for each block...
   for (int i = 0; i < eval_points.length(); i += (L+1))
   {
      // determine length of this block, which is at most L+1
      int length = eval_points.length() - i;
      if (length >= (L+1))
         length = (L+1);
      block.SetLength(length);

      // construct Evaluator object for evaluating at these points
      for (int j = 0; j < length; j++)
         block[j] = eval_points[i+j];
      Evaluator<SCALAR, POLY, POLYMODULUS, VECTOR> evaluator(block);

      // evaluate each entry of M(X, X+L) at those points
      for (int x = 0; x < dim; x++)
      for (int y = 0; y < dim; y++)
      {
         evaluator.evaluate(values, polys[y*dim + x]);
         for (int k = 0; k < length; k++)
            main_matrices[i+k][y][x] = values[k];
      }
   }

   // =========================================================================
   // Step 5: merge together the matrices obtained from the multipoint
   // evaluation step and the recursive leftover interval step.

   output.clear();
   output.resize(target.size() / 2);
   for (size_t i = 0; i < target.size()/2; i++)
      output[i].SetDims(dim, dim);

   int main_index = 0;       // index into main_matrices
   int leftover_index = 0;   // index into leftover_matrices

   MATRIX temp;
   temp.SetDims(dim, dim);

   for (size_t i = 0; i < target.size(); i += 2)
   {
      current = target[i];
      next = current + L;
      ident(output[i/2], dim);

      while (next <= target[i+1])
      {
         // merge in a matrix from multipoint evaluation step
         mul(temp, output[i/2], main_matrices[main_index++]);
         swap(temp, output[i/2]);
         swap(current, next);
         next = current + L;
      }
      if (current < target[i+1])
      {
         // merge in a matrix from leftover interval step
         mul(temp, output[i/2], leftover_matrices[leftover_index++]);
         swap(temp, output[i/2]);
      }
   }
}

/* ============================================================================

   Matrix products over arbitrary, long intervals

This section implements an algorithm similar to Theorem 15 of [BGS].

============================================================================ */

/*
See interval_products_wrapper().

NOTE:
   This algorithm works best if the intervals are very long and don't have
   much space between them. The case where the gaps are relatively large is
   best handled by ntl_short_interval_products().

*/
template <typename SCALAR, typename POLY, typename POLYMODULUS,
          typename VECTOR, typename MATRIX, typename FFTREP>
void ntl_interval_products(vector<MATRIX>& output,
                           const MATRIX& M0, const MATRIX& M1,
                           const vector<ZZ>& target)
{
   assert(target.size() % 2 == 0);
   output.resize(target.size() / 2);

   int dim = M0.NumRows();
   assert(dim == M0.NumCols());
   assert(dim == M1.NumRows());
   assert(dim == M1.NumCols());

   // =========================================================================
   // Step 0: get as many intervals as possible using dyadic_evaluation().

   // step0_matrix[i] is the transition matrix between step0_index[2*i]
   // and step0_index[2*i+1].
   vector<MATRIX> step0_matrix;
   vector<ZZ> step0_index;
   // preallocate the maximum number of matrices that could arise (plus safety)
   size_t reserve_size = target.size() +
                      4*NumBits(target.back() - target.front());
   step0_matrix.reserve(reserve_size);
   step0_index.reserve(2 * reserve_size);

   ZZ current_index = target.front();
   size_t next_target = 0;   // index into "target" array

   // This flag indicates whether the last entry of step0_matrix is
   // still accumulating matrices (in which the right endpoint of the
   // corresponding interval hasn't been written to step0_index yet).
   int active = 0;

   MATRIX temp_mat;
   temp_mat.SetDims(dim, dim);

   while (current_index < target.back() - 3)
   {
      // find largest t such that 2^t*(2^t + 1) <= remaining distance to go
      ZZ remaining = target.back() - current_index;
      int t = 0;
      while ((to_ZZ(1) << (2*t)) + (1 << t) <= remaining)
         t++;
      t--;

      // evaluate matrices for 2^t+1 intervals of length 2^t
      vector<VECTOR> dyadic_output(dim*dim);
      for (int i = 0; i < dim*dim; i++)
         dyadic_output[i].SetLength((1 << t) + 1);
      dyadic_evaluation<SCALAR, POLY, VECTOR, MATRIX, FFTREP>
               (dyadic_output, M0, M1, t, t, to_scalar<SCALAR>(current_index));

      // Walk through the intervals we just computed. Find maximal subsequences
      // of intervals none of which contain any target endpoints. Merge them
      // together (by multiplying the appropriate matrices) and store results
      // in step0_matrix, step0_index.

      SCALAR scratch;

      for (int i = 0; i <= (1 << t); i++, current_index += (1 << t))
      {
         assert(next_target == target.size() ||
                target[next_target] >= current_index);

         // Skip over target endpoints which are exactly at the beginning
         // of this interval
         while ((next_target < target.size()) &&
                (target[next_target] == current_index))
         {
            // if there's an active matrix, don't forget to close it off
            if (active)
            {
               step0_index.push_back(current_index);
               active = 0;
            }
            next_target++;
         }

         // Test if any target endpoints are strictly within this interval.
         if ((next_target == target.size()) ||
             (target[next_target] >= current_index + (1 << t)))
         {
            // There are no target endpoints in this interval.
            if (active)
            {
               // Merge this matrix with the active one
               MATRIX& active_mat = step0_matrix.back();
               for (int y = 0; y < dim; y++)
               for (int x = 0; x < dim; x++)
               {
                  SCALAR& accum = temp_mat[y][x];
                  accum = 0;
                  for (int z = 0; z < dim; z++)
                  {
                     mul(scratch, active_mat[y][z],
                                  dyadic_output[z*dim + x][i]);
                     add(accum, accum, scratch);
                  }
               }

               swap(temp_mat, active_mat);
            }
            else
            {
               // Make this matrix into a new active one
               step0_index.push_back(current_index);
               step0_matrix.resize(step0_matrix.size() + 1);
               MATRIX& X = step0_matrix.back();
               X.SetDims(dim, dim);
               for (int y = 0; y < dim; y++)
               for (int x = 0; x < dim; x++)
                  X[y][x] = dyadic_output[y*dim + x][i];
               active = 1;
            }
         }
         else
         {
            // There are target endpoints in this interval.
            if (active)
            {
               // If there is still an active matrix, close it off.
               step0_index.push_back(current_index);
               active = 0;
            }

            // skip over any other endpoints in this interval
            while ((next_target < target.size()) &&
                   (target[next_target] < current_index + (1 << t)))
            {
               next_target++;
            }
         }
      }
   }

   // If there is still an active matrix, close it off.
   if (active)
      step0_index.push_back(current_index);

   assert(step0_index.size() == 2*step0_matrix.size());

   // =========================================================================
   // Step 1: Make a list of all subintervals that we are going to need in
   // the refining steps.

   size_t next_step0 = 0;        // index into step0_index
   vector<ZZ> step1_index;    // list of pairs of endpoints of needed intervals
   step1_index.reserve(2*target.size());

   // add sentinel endpoints to make the next loop simpler:
   step0_index.push_back(target.back() + 10);
   step0_index.push_back(target.back() + 20);

   for (next_target = 0; next_target < target.size(); next_target += 2)
   {
      // skip dyadic intervals that come before this target interval
      while (step0_index[next_step0+1] <= target[next_target])
         next_step0 += 2;

      if (step0_index[next_step0] < target[next_target+1])
      {
         // The next dyadic interval starts before the end of this target
         // interval.
         if (step0_index[next_step0] > target[next_target])
         {
            // The next dyadic interval starts strictly within this target
            // interval, so we need to create a refining subinterval for the
            // initial segment of this target interval.
            step1_index.push_back(target[next_target]);
            step1_index.push_back(step0_index[next_step0]);
         }

         // Skip over dyadic intervals to find the last one still contained
         // within this target interval.
         while (step0_index[next_step0+3] <= target[next_target+1])
            next_step0 += 2;

         if (step0_index[next_step0+1] < target[next_target+1])
         {
            // The next dyadic interval finishes strictly within this target
            // interval, so we need to create a refining subinterval for the
            // final segment of this target interval.
            step1_index.push_back(step0_index[next_step0+1]);
            step1_index.push_back(target[next_target+1]);
         }

         // Move on to next dyadic interval
         next_step0 += 2;
      }
      else
      {
         // The next dyadic interval starts beyond (or just at the end of)
         // this target interval, so we need to create a refining subinterval
         // for this *whole* target interval.
         step1_index.push_back(target[next_target]);
         step1_index.push_back(target[next_target+1]);
      }
   }

   // remove sentinels for my sanity
   step0_index.pop_back();
   step0_index.pop_back();

   // Step 1b: Compute matrix products over those refining subintervals.
   vector<MATRIX> step1_matrix;
   ntl_short_interval_products<SCALAR, POLY, POLYMODULUS, VECTOR, MATRIX>
                                   (step1_matrix, M0, M1, step1_index);

   assert(step1_index.size() == 2 * step1_matrix.size());

   // =========================================================================
   // Step 2: Merge together the dyadic intervals and refining intervals into
   // a single list, in sorted order.

   vector<MATRIX> step2_matrix(step0_matrix.size() + step1_matrix.size());
   vector<ZZ> step2_index(step0_index.size() + step1_index.size());

   // add sentinels to make the next loop simpler
   step0_index.push_back(target.back() + 10);
   step0_index.push_back(target.back() + 20);
   step1_index.push_back(target.back() + 10);
   step1_index.push_back(target.back() + 20);

   next_step0 = 0;       // index into step0_matrix
   size_t next_step1 = 0;   // index into step1_matrix

   for (size_t next_step2 = 0; next_step2 < step2_matrix.size(); next_step2++)
   {
      if (step0_index[2*next_step0] < step1_index[2*next_step1])
      {
         // grab a matrix and pair of indices from step0
         swap(step2_matrix[next_step2], step0_matrix[next_step0]);
         step2_index[2*next_step2] = step0_index[2*next_step0];
         step2_index[2*next_step2+1] = step0_index[2*next_step0+1];
         next_step0++;
      }
      else
      {
         // grab a matrix and pair of indices from step1
         swap(step2_matrix[next_step2], step1_matrix[next_step1]);
         step2_index[2*next_step2] = step1_index[2*next_step1];
         step2_index[2*next_step2+1] = step1_index[2*next_step1+1];
         next_step1++;
      }
   }

   // remove sentinels for my sanity
   step0_index.pop_back();
   step0_index.pop_back();
   step1_index.pop_back();
   step1_index.pop_back();

   assert(step2_index.size() == 2*step2_matrix.size());

   // =========================================================================
   // Step 3: Walk through target intervals, and merge together appropriate
   // intervals from step2 to get those target intervals.

   size_t next_step2 = 0;    // index into step2_matrix

   // add sentinels to make the next loop simpler
   step2_index.push_back(target.back() + 1);
   step2_index.push_back(target.back() + 2);

   for (size_t next_target = 0; next_target < target.size(); next_target += 2)
   {
      // search for step2 interval matching the start of this target interval
      while (step2_index[2*next_step2] < target[next_target])
         next_step2++;

      assert(step2_index[2*next_step2] == target[next_target]);

      // merge together matrices for step2 intervals contained in this target
      // interval
      swap(step2_matrix[next_step2++], output[next_target/2]);
      while (step2_index[2*next_step2+1] <= target[next_target+1])
      {
         mul(temp_mat, output[next_target/2], step2_matrix[next_step2]);
         swap(temp_mat, output[next_target/2]);
         next_step2++;
      }
   }
}

// explicit instantiations for zz_p and ZZ_p versions:

template void ntl_interval_products
      <ZZ_p, ZZ_pX, ZZ_pXModulus, vec_ZZ_p, mat_ZZ_p, FFTRep>
      (vector<mat_ZZ_p>& output, const mat_ZZ_p& M0, const mat_ZZ_p& M1,
                             const vector<ZZ>& target);

template void ntl_interval_products
      <zz_p, zz_pX, zz_pXModulus, vec_zz_p, mat_zz_p, fftRep>
      (vector<mat_zz_p>& output, const mat_zz_p& M0, const mat_zz_p& M1,
                             const vector<ZZ>& target);

};    // namespace hypellfrob

// ----------------------- end of file


// copyright (c) 2012-2013 the dzcoin core developers
// distributed under the mit software license, see the accompanying
// file copying or http://www.opensource.org/licenses/mit-license.php.

#include "compressor.h"
#include "util.h"
#include "test/test_dzcoin.h"

#include <stdint.h>

#include <boost/test/unit_test.hpp>

// amounts 0.00000001 .. 0.00100000
#define num_multiples_unit 100000

// amounts 0.01 .. 100.00
#define num_multiples_cent 10000

// amounts 1 .. 10000
#define num_multiples_1btc 10000

// amounts 50 .. 21000000
#define num_multiples_50btc 420000

boost_fixture_test_suite(compress_tests, basictestingsetup)

bool static testencode(uint64_t in) {
    return in == ctxoutcompressor::decompressamount(ctxoutcompressor::compressamount(in));
}

bool static testdecode(uint64_t in) {
    return in == ctxoutcompressor::compressamount(ctxoutcompressor::decompressamount(in));
}

bool static testpair(uint64_t dec, uint64_t enc) {
    return ctxoutcompressor::compressamount(dec) == enc &&
           ctxoutcompressor::decompressamount(enc) == dec;
}

boost_auto_test_case(compress_amounts)
{
    boost_check(testpair(            0,       0x0));
    boost_check(testpair(            1,       0x1));
    boost_check(testpair(         cent,       0x7));
    boost_check(testpair(         coin,       0x9));
    boost_check(testpair(      50*coin,      0x32));
    boost_check(testpair(21000000*coin, 0x1406f40));

    for (uint64_t i = 1; i <= num_multiples_unit; i++)
        boost_check(testencode(i));

    for (uint64_t i = 1; i <= num_multiples_cent; i++)
        boost_check(testencode(i * cent));

    for (uint64_t i = 1; i <= num_multiples_1btc; i++)
        boost_check(testencode(i * coin));

    for (uint64_t i = 1; i <= num_multiples_50btc; i++)
        boost_check(testencode(i * 50 * coin));

    for (uint64_t i = 0; i < 100000; i++)
        boost_check(testdecode(i));
}

boost_auto_test_suite_end()


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_SUCCESSOR_HPP_INCLUDED
#define NT2_TOOLBOX_IEEE_FUNCTION_SIMD_COMMON_SUCCESSOR_HPP_INCLUDED
#include <nt2/sdk/constant/infinites.hpp>
#include <nt2/sdk/constant/real.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/constant/properties.hpp>
#include <nt2/sdk/constant/digits.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/select.hpp>
#include <nt2/include/functions/bitinteger.hpp>
#include <nt2/include/functions/bitfloating.hpp>
#include <nt2/include/functions/tofloat.hpp>
#include <nt2/include/functions/seladd.hpp>
#include <nt2/include/functions/select.hpp>
#include <nt2/include/functions/fast_frexp.hpp>
#include <nt2/include/functions/fast_ldexp.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/next.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_ unary
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::successor_, tag::cpu_,
                              (A0)(X),
                              ((simd_<arithmetic_<A0>,X>))
                             );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::successor_(tag::simd_<tag::arithmetic_, X>),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
      struct result<This(A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(1)
      {
      return  seladd(neq(a0, Valmax<A0>()), a0, One<A0>());
      }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_ unary
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::successor_, tag::cpu_,
                              (A0)(X),
                              ((simd_<real_<A0>,X>))
                             );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::successor_(tag::simd_<tag::real_, X>),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
      struct result<This(A0)> : meta::strip<A0>{};

    NT2_FUNCTOR_CALL(1)
      {
      return next(a0);
      }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is integer_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::successor_, tag::cpu_,
                            (A0)(X),
                            ((simd_<integer_<A0>,X>))
                            ((simd_<integer_<A0>,X>))
                           );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::successor_(tag::simd_<tag::integer_, X>,
			      tag::simd_<tag::integer_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
      struct result<This(A0, A1)>: meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
      {
      return seladd( gt(Valmax<A0>()-nt2::abs(a1), a0), a0, nt2::abs(a1));
      }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is real_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::successor_, tag::cpu_,
		      (A0)(A1)(X),
		      ((simd_<real_<A0>,X>))
		      ((simd_<integer_<A1>,X>))
		      );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::successor_(tag::simd_<tag::real_, X>,
			      tag::simd_<tag::integer_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0,class A1>
    struct result<This(A0, A1)>: meta::strip<A0>{};

    NT2_FUNCTOR_CALL(2)
    {
      return sel(eq(a0, Inf<A0>()), a0,  bitfloating(bitinteger(a0)+a1));
//       typedef typename meta::as_integer<A0, signed>::type itype;
//       A0 m;
//       itype expon;
//       const A0 fac =  nt2::abs(tofloat(a1));
//       boost::fusion::tie(m, expon) = fast_frexp(a0);
//       expon =  seladd(iseq(m, Mhalf<A0>()), expon, Mone<itype>());
//       A0 diff =  fast_ldexp(One<A0>(), expon-Nbdigits<A0>());
//       diff = b_and(sel(iseqz(diff)||iseqz(a0),  Mindenormal<A0>(), diff), isfin(a0));
//       return sel(iseq(a0, Minf<A0>()), fac*Valmin<A0>(), a0+fac*diff);
    }

  };
} }

#endif
// modified by jt the 04/01/2011


#include <boost/numeric/odeint/algebra/norm_result_type.hpp>


/*=========================================================================

   Library: iMSTK

   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
   & Imaging in Medicine, Rensselaer Polytechnic Institute.

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0.txt

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.

=========================================================================*/

#include "imstkRigidObjectController.h"
#include "imstkDeviceClient.h"
#include "imstkLogger.h"
#include "imstkRbdConstraint.h"
#include "imstkRigidObject2.h"

#include <Eigen/Eigenvalues>

namespace imstk
{
RigidObjectController::RigidObjectController(std::shared_ptr<RigidObject2> rigidObject,
                                             std::shared_ptr<DeviceClient> trackingDevice) :
    SceneObjectController(rigidObject, trackingDevice),
    m_rigidObject(rigidObject)
{
    /*m_currentPos = rigidObject->getRigidBody()->getPosition();
    m_currentRot = rigidObject->getRigidBody()->getOrientation();*/
}

void
RigidObjectController::setControlledSceneObject(std::shared_ptr<SceneObject> obj)
{
    SceneObjectController::setControlledSceneObject(obj);
    m_rigidObject = std::dynamic_pointer_cast<RigidObject2>(obj);
}

void
RigidObjectController::update(const double dt)
{
    if (!isTrackerUpToDate())
    {
        if (!updateTrackingData(dt))
        {
            LOG(WARNING) << "warning: could not update tracking info.";
            return;
        }
    }

    if (m_rigidObject == nullptr)
    {
        return;
    }

    // Implementation partially from otaduy lin's paper eq14
    // "A Modular Haptic Rendering Algorithm for Stable and Transparent 6 - DOF Manipulation"
    if (m_deviceClient->getTrackingEnabled() && m_useSpring)
    {
        const Vec3d& currPos = m_rigidObject->getRigidBody()->getPosition();
        const Quatd& currOrientation     = m_rigidObject->getRigidBody()->getOrientation();
        const Vec3d& currVelocity        = m_rigidObject->getRigidBody()->getVelocity();
        const Vec3d& currAngularVelocity = m_rigidObject->getRigidBody()->getAngularVelocity();
        Vec3d&       currForce  = *m_rigidObject->getRigidBody()->m_force;
        Vec3d&       currTorque = *m_rigidObject->getRigidBody()->m_torque;

        const Vec3d& devicePos = getPosition();
        const Quatd& deviceOrientation = getOrientation();
        const Vec3d& deviceOffset      = Vec3d(0.0, 0.0, 0.0);

        // If using critical damping automatically compute kd
        if (m_useCriticalDamping)
        {
            const double mass     = m_rigidObject->getRigidBody()->getMass();
            const double linearKs = m_linearKs.maxCoeff();
            m_linearKd = 2.0 * std::sqrt(mass * linearKs);

            const Mat3d inertia = m_rigidObject->getRigidBody()->getIntertiaTensor();
            // Currently kd is not a 3d vector though it could be.
            // So here we make an approximation. Either:
            //  - Use one colums eigenvalue (maxCoeff)
            //  - cbrt(eigenvalue0*eigenvalue1*eigenvalue2). (det)
            // Both may behave weird on anistropic inertia tensors
            //const double inertiaScale = inertia.eigenvalues().real().maxCoeff();
            const double inertiaScale = std::cbrt(inertia.determinant());
            const double angularKs    = m_angularKs.maxCoeff();
            m_angularKd = 2.0 * std::sqrt(inertiaScale * angularKs);
        }

        // If kd > 2 * sqrt(mass * ks); The system is overdamped (may be intentional)
        // If kd < 2 * sqrt(mass * ks); The system is underdamped (never intended)

        // Uses non-relative force
        {
            // Compute force
            m_fS = m_linearKs.cwiseProduct(devicePos - currPos - deviceOffset);
            m_fD = m_linearKd * (-currVelocity - currAngularVelocity.cross(deviceOffset));
            Vec3d force = m_fS + m_fD;

            // Computer torque
            const Quatd dq = deviceOrientation * currOrientation.inverse();
            const Rotd  angleAxes = Rotd(dq);
            m_tS = deviceOffset.cross(force) + m_angularKs.cwiseProduct(angleAxes.axis() * angleAxes.angle());
            m_tD = m_angularKd * -currAngularVelocity;
            Vec3d torque = m_tS + m_tD;

            currForce  += force;
            currTorque += torque;
        }

        // Uses relative force
        //{
        //    const Vec3d& deviceVelocity = getVelocity();
        //    const Vec3d& deviceAngularVelocity = getAngularVelocity();

        //    // Compute force
        //    m_fS = m_linearKs.cwiseProduct(devicePos - currPos - deviceOffset);
        //    m_fD = m_linearKd * (deviceVelocity - currVelocity - currAngularVelocity.cross(deviceOffset));
        //    Vec3d force = m_fS + m_fD;

        //    // Compute torque
        //    const Quatd dq = deviceOrientation * currOrientation.inverse();
        //    const Rotd angleAxes = Rotd(dq);
        //    m_tS = deviceOffset.cross(force) + m_angularKs.cwiseProduct(angleAxes.axis() * angleAxes.angle());
        //    m_tD = m_angularKd * (deviceAngularVelocity - currAngularVelocity);
        //    Vec3d torque = m_tS + m_tD;

        //    currForce += force;
        //    currTorque += torque;
        //}
    }
    else
    {
        // Zero out external force/torque
        *m_rigidObject->getRigidBody()->m_force  = Vec3d(0.0, 0.0, 0.0);
        *m_rigidObject->getRigidBody()->m_torque = Vec3d(0.0, 0.0, 0.0);
        // Directly set position/rotation
        (*m_rigidObject->getRigidBody()->m_pos) = getPosition();
        (*m_rigidObject->getRigidBody()->m_orientation) = getOrientation();
    }

    this->postEvent(Event(RigidObjectController::modified()));
}

void
RigidObjectController::applyForces()
{
    if (!m_deviceClient->getButton(0))
    {
        // Apply force back to device
        if (m_rigidObject != nullptr && m_useSpring)
        {
            const Vec3d force = -getDeviceForce();
            if (m_forceSmoothening)
            {
                m_forces.push_back(force);
                m_forceSum += force;
                if (static_cast<int>(m_forces.size()) > m_smoothingKernelSize)
                {
                    m_forceSum -= m_forces.front();
                    m_forces.pop_front();
                }
                const Vec3d avgForce = m_forceSum / m_forces.size();

                // Render only the spring force (not the other forces the body has)
                m_deviceClient->setForce(avgForce);
            }
            else
            {
                // Render only the spring force (not the other forces the body has)
                m_deviceClient->setForce(force);
            }
        }
    }
    else
    {
        m_deviceClient->setForce(Vec3d(0.0, 0.0, 0.0));
    }
}
} // namespace imstk


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Rice University
*  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 Rice University 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: Mark Moll */

#include <ompl/base/spaces/ReedsSheppStateSpace.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ClothoidStateSpace.h>
#include <ompl/base/ScopedState.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/rrt/DORRTstar.h>
#include <boost/program_options.hpp>
#include <Eigen/Core>

using namespace std;
using namespace Eigen;
namespace ob = ompl::base;
namespace og = ompl::geometric;
namespace po = boost::program_options;

void magneticVectorForce(double x, double y, double theta,
        VectorXd& ret)
{
    /*
	double ln = 60.0; 	// Length of conductors
	double d = 1.61/2.0;		// Distance between the conductors m & n
	int m = 1; 		// Direction of current through the conductor, m(Right) 1(INTO), -1(OUT)
	int n = -1; 		// Direction of current through the conductor, n(Left) 1(INTO), -1(OUT)
	int N = 12; 		// Number sections/elements in the conductors
	int dl = ln/N; 		// Length of each element

	// XYZ coordinates/Location of each element from the origin (0,0,0), i.e 'd/2' is taken as origin..
    MatrixXd xCm = (d/2.0)*MatrixXd::Ones(1,N);
    MatrixXd xCn = (-d/2.0)*MatrixXd::Ones(1,N);

	// Y Coordinate of each element from origin, half on +Y & other half on -Y and also sam for both conductors
	VectorXd yC = VectorXd::LinSpaced(N, -ln/2.0+dl/2.0, ln/2.0-dl/2.0);

	// zC remains 0 throughout the length, as conductors are lying on XY plane
	MatrixXd zC = MatrixXd::Zero(1,N);

	// Length(Projection) 7 Direction of each current element in Vector form
	MatrixXd Lx = MatrixXd::Zero(1,N);	// Length of each element is zero on X axis
	MatrixXd Ly = dl*MatrixXd::Ones(1,N);	// Length of each element is dl on Y axis
	MatrixXd Lz = MatrixXd::Zero(1,N);	// Length of each element is zero on Z axis

	// Points/Locations in space (here XZ plane) where B is to be computed
	int NP = 96;	//Detector points..
	int xPmax = 12;	//Dimensions of detector space.., arbitrary..
	int zPmax = 12;

	MatrixXd resVec_x = MatrixXd::Zero(NP,NP);
	MatrixXd resVec_y = MatrixXd::Zero(NP,NP);
	MatrixXd resPos_x = MatrixXd::Zero(NP,NP);
	MatrixXd resPos_y = MatrixXd::Zero(NP,NP);

	//Divide space with NP points..
	RowVectorXd xP = RowVectorXd::LinSpaced(NP, -xPmax, xPmax);
	VectorXd zP = VectorXd::LinSpaced(NP, -zPmax, zPmax);

	// Creating the Mesh..
	MatrixXd xxP = xP.replicate(NP,1);
	MatrixXd zzP = zP.replicate(1,NP);

	//Initialize B..
	MatrixXd Bx = MatrixXd::Zero(NP,NP);
	MatrixXd By = MatrixXd::Zero(NP,NP);
	MatrixXd Bz = MatrixXd::Zero(NP,NP);

	// Computation of Magnetic Field (B) using Superposition principle..
	// Compute B at each detector points due to each small cond elements & integrate them..
	for( int q=0; q<N; q++ )
	{
		MatrixXd rxm = xxP.array() - xCm(0,q); // Displacement Vector along X direction, from cond m..
		MatrixXd rxn = xxP.array() - xCn(0,q); // Displacement Vector along X direction, from cond n..

		double ry = yC(q,0);	// Same for m & n, no detector points on Y direction..

		MatrixXd rz = zzP.array() - zC(0,q); // Same for m & n..

		MatrixXd rm = (rxm.array()*rxm.array()+ry*ry+rz.array()*rz.array()).sqrt();	// Displacement Magnitude for an element on cond m..
		MatrixXd rn = (rxn.array()*rxn.array()+ry*ry+rz.array()*rz.array()).sqrt();	// Displacement Magnitude for an element on cond n..

		MatrixXd r3m = rm.array()*rm.array()*rm.array();
		MatrixXd r3n = rn.array()*rn.array()*rn.array();

		Bx = Bx.array() + m*Ly(0,q)*rz.array()/r3m.array() + n*Ly(0,q)*rz.array()/r3n.array();	// m & n, direction of current element..
		// By = 0;
		Bz = Bz.array() - m*Ly(0,q)*rxm.array()/r3m.array() - n*Ly(0,q)*rxn.array()/r3n.array();
	}

	MatrixXd Rot(2,2);
	Rot << cos(_theta), -sin(_theta),
	       sin(_theta),  cos(_theta);

	MatrixXd tmpMat(2, NP);
	for(int i=0; i<NP; i++)
	{
		//tmpMat << Bx.row(i)+xxP.row(i),
		//		Bz.row(i)+zzP.row(i);
		tmpMat << Bx.row(i),
				Bz.row(i);
		tmpMat = Rot * tmpMat;

		// Block Operation
		resVec_x.block(i,0,1,NP)  = tmpMat.block(0,0,1,NP).array();
		resVec_y.block(i,0,1,NP)  = tmpMat.block(1,0,1,NP).array();

		tmpMat << xxP.row(i),
				zzP.row(i);
		tmpMat = Rot * tmpMat;

		// Block Operation
		resPos_x.block(i,0,1,NP)  = (tmpMat.block(0,0,1,NP)).array()+_x;
		resPos_y.block(i,0,1,NP)  = (tmpMat.block(1,0,1,NP)).array()+_y;
	}

	///////////////////////////////////////////////////////////////////
	geometry_msgs::PoseArray poseArray;

    double grid_offset=grid_dim_/2.0*m_per_cell_;
    double K_rep_mag = 100.0f;

	for(int i=0; i<NP; i++)
	{
		for( int j=0; j<NP; j++)
		{
			int i_ = int((resPos_x(i,j) + grid_offset - m_per_cell_/2.0) / m_per_cell_);
			int j_ = int((resPos_y(i,j) + grid_offset - m_per_cell_/2.0) / m_per_cell_);

			if( i_ >= 0 && j_ >= 0 && i_ < NP && j_ < NP )
			{
				PotentialMap[i_][j_].bInit = true;

				double vSize = sqrtf(resVec_x(i,j)*resVec_x(i,j) + resVec_y(i,j)*resVec_y(i,j));
				double vDist = sqrtf((_x-resPos_x(i,j))*(_x-resPos_x(i,j)) + (_y-resPos_y(i,j))*(_y-resPos_y(i,j)));
				if( vDist < 0.1 ) vDist = 0.1;
				PotentialMap[i_][j_].F_sum.x += K_rep_mag*resVec_x(i,j)/vSize/vDist;
				PotentialMap[i_][j_].F_sum.y += K_rep_mag*resVec_y(i,j)/vSize/vDist;
			}
		}
	}
	*/
}
double computeRepulsiveForce(double K, double dist, double range, double x, double tar_x)
{
    // cout <<"A "<< dist << " " << range <<" " << x << " " << tar_x << " " << K*((1.0f/dist)-(1.0f/range))*(1.0f/(dist*dist*dist))*(x-tar_x) << endl;

    return K*((1.0f/dist)-(1.0f/range))*(1.0f/(dist*dist*dist))*(x-tar_x);

}

double computeAttractiveForce(double K, double x, double tar_x)
{
    return -1.0f * K * (x - tar_x);
}
/** Basic unit-norm rotation field. */
VectorXd vectorfield(const ob::State *from,const ob::State *to)
{
//    const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
    VectorXd v(2);
  //  v[0] = x[1];
   // v[1] = -x[0];
   // v.normalize();
    return v;
}

/** Basic unit-norm rotation field. */
VectorXd magneticfield(const ob::State *from,const ob::State *to)
{
    //const ob::RealVectorStateSpace::StateType &x = *state->as<ob::RealVectorStateSpace::StateType>();
    VectorXd v(2);
   // v[0] = x[1];
   // v[1] = -x[0];
   // v.normalize();
    return v;
}

// The easy problem is the standard narrow passage problem: two big open
// spaces connected by a narrow passage. The hard problem is essentially
// one long narrow passage with the robot facing towards the long walls
// in both the start and goal configurations.

bool isStateValid(const ob::SpaceInformation *si, const ob::State *state)
{
    const ob::SE2StateSpace::StateType *s = state->as<ob::SE2StateSpace::StateType>();
    double x=s->getX(), y=s->getY(), yaw=s->getYaw();

	// 1. Collision Checking
	// CollisionChecking(x, y);

	//bool isFreeSpace = (x<40|| x>60 || (y>10.0 && y<12.0));
	bool isFreeSpace = (x<40|| x>60 || (y>10.8 && y<11.2));
    return si->satisfiesBounds(s) && isFreeSpace;
}

bool isStateValid_Clothoid(const ob::SpaceInformation *si, const ob::State *state)
{
    const ob::ClothoidStateSpace::StateType *s = state->as<ob::ClothoidStateSpace::StateType>();
    double x=s->getX(), y=s->getY(), yaw=s->getYaw();

	// 1. Collision Checking
	// CollisionChecking(x, y);

	//bool isFreeSpace = (x<40|| x>60 || (y>10.0 && y<12.0));
	bool isFreeSpace = (x<40|| x>60 || (y>10.8 && y<11.2));
    return si->satisfiesBounds(s) && isFreeSpace;
}

void plan(ob::StateSpacePtr space,double time, bool isClothoid)
{

    // 0. Build KdTree
	// =BuildTree()

	ob::ScopedState<> start(space), goal(space);
    ob::RealVectorBounds bounds(2);
    bounds.low[0] = 0;
    bounds.high[0] = 100.0;

    bounds.low[1] = 0;
    bounds.high[1] = 50.0;

	//bounds.setLow(0);
    //bounds.setHigh(100);

	if( isClothoid )
    	space->as<ob::ClothoidStateSpace>()->setBounds(bounds);
	else
    	space->as<ob::SE2StateSpace>()->setBounds(bounds);

    // define a simple setup class
    og::SimpleSetup ss(space);

    // set state validity checking for this space
    ob::SpaceInformationPtr si(ss.getSpaceInformation());

	ss.setStateValidityChecker(std::bind(
        isClothoid ? &isStateValid_Clothoid : &isStateValid, si.get(), std::placeholders::_1));

    // set the start and goal states
	start[0] =85.0;
	start[1] = 8.0;
	start[2] = 0.99*boost::math::constants::pi<double>();
	goal[0] = 50.0;
	goal[1] = 11.0;
	//goal[2] = 0.99*boost::math::constants::pi<double>();
	goal[2] = 0.0*boost::math::constants::pi<double>();
	if( isClothoid )
	{
		start[3] = 0.0;
		goal[3] = 0.0;
	}

	ss.setStartAndGoalStates(start, goal);

    ///////////////////////////////////////////////////////////////////
    // 1. Potential Field

    double m_per_cell_ = 0.25;
    int grid_dim_x = ceil((bounds.high[0] - bounds.low[0])/m_per_cell_);
    int grid_dim_y = ceil((bounds.high[1] - bounds.low[1])/m_per_cell_);

    double num_obs[grid_dim_x][grid_dim_y];

    for (int x = 0; x < grid_dim_x; x++) {
        for (int y = 0; y < grid_dim_y; y++) {
            num_obs[x][y]=0;
        }
    }

    vector<Vector2d> obs;
    for (unsigned int i=0; i<obs.size(); i++)
    {
        int x = ((grid_dim_x/2)+obs[i](0)/m_per_cell_);
        int y = ((grid_dim_y/2)+obs[i](1)/m_per_cell_);
        if( x >= 0 && x < grid_dim_x && y >= 0 && y < grid_dim_y )
        {
            num_obs[x][y]++;
        }
    }

    vector<vector<VectorXd> > VectorMap;
	// Initialize Potential Map
    for (int x = 0; x < grid_dim_x; x++)
    {
		vector<VectorXd> v;
		VectorMap.push_back(v);

        for (int y = 0; y < grid_dim_y; y++)
        {
            VectorXd vec = VectorXd::Zero(3);
            VectorMap[x].push_back(vec);
        }
    }

    // Parameters
    double grid_offset_x=grid_dim_x/2.0*m_per_cell_;
    double grid_offset_y=grid_dim_y/2.0*m_per_cell_;
    double Range_obs = 10.0;
    double K_rep_obs = 1.3f;

    double Range_rep=180.0;
    double K_rep = 50.0;

    double K_att = 30.0;

    for (int i = 0; i<grid_dim_x; i++)
    {
        for (int j = 0; j<grid_dim_y; j++)
        {
            // transform index to local coordinate (x,y)
            double x = -grid_offset_x + (i*m_per_cell_+m_per_cell_/2.0);
            double y = -grid_offset_y + (j*m_per_cell_+m_per_cell_/2.0);

            // Generate obstacle potential
            for (unsigned int k = 0; k <= obs.size(); k++)
            {
                double obs_x = obs[k](0);
                double obs_y = obs[k](1);

                if(obs_x == 0.0 && obs_y == 0.0)
                    continue;

                double obs_dist = sqrt(((x - obs_x) * (x - obs_x))
                        + ((y - obs_y) * (y - obs_y)));

                if (obs_dist > 0.0 && obs_dist < Range_obs)
                {
                    VectorMap[i][j](2) = 1;
                    VectorMap[i][j](0) += computeRepulsiveForce(K_rep_obs, obs_dist, Range_obs, x, obs_x);
                    VectorMap[i][j](1) += computeRepulsiveForce(K_rep_obs, obs_dist, Range_obs, y, obs_y);
                }
            }

            // Summation of attractive and repulsive potential values
            if( VectorMap[i][j](2) == 1)
            {

                double x_att = computeAttractiveForce(K_att, x, goal[0]);
                double y_att = computeAttractiveForce(K_att, y, goal[1]);

                double start_dist = sqrt(((x - start[0]) * (x - start[0]))
                        + ((y - start[1]) * (y - start[1])));
                double x_rep = computeRepulsiveForce(K_rep, start_dist,
                        Range_rep, x, start[0]);
                double y_rep = computeRepulsiveForce(K_rep, start_dist,
                        Range_rep, y, start[1]);

                // Vector Force (Unit Vector)
                VectorMap[i][j](0) +=(x_att + x_rep);
                VectorMap[i][j](1) +=(y_att + y_rep);
            }

        }
    }
    //////////////////////////////////////////////////////////////////

//    ss.setPlanner(std::make_shared<ompl::geometric::RRTstar>(ss.getSpaceInformation()));
    ss.setPlanner(std::make_shared<ompl::geometric::DORRTstar>(ss.getSpaceInformation()
                ,vectorfield, magneticfield));
	//b, l::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getiSpaceInformation())), range);
    // this call is optional, but we put it in to get more output information
    ss.getSpaceInformation()->setStateValidityCheckingResolution(0.005);
    ss.setup();
    ss.print();

    // attempt to solve the problem within 30 seconds of planning time
    ob::PlannerStatus solved = ss.solve(time);

    if (solved)
    {
        std::vector<double> reals;

        std::cout << "Found solution:" << std::endl;
        ss.simplifySolution();
        og::PathGeometric path = ss.getSolutionPath();
        path.interpolate(1000);
        path.printAsMatrix(std::cout);
    }
    else
        std::cout << "No solution found" << std::endl;
}

void printTrajectory(ob::StateSpacePtr space, const std::vector<double>& pt)
{
    if (pt.size()!=3) throw ompl::Exception("3 arguments required for trajectory option");
    const unsigned int num_pts = 50;
    ob::ScopedState<> from(space), to(space), s(space);
    std::vector<double> reals;

    from[0] = from[1] = from[2] = 0.;

    to[0] = pt[0];
    to[1] = pt[1];
    to[2] = pt[2];

    std::cout << "distance: " << space->distance(from(), to()) << "\npath:\n";
    for (unsigned int i=0; i<=num_pts; ++i)
    {
        space->interpolate(from(), to(), (double)i/num_pts, s());
        reals = s.reals();
        std::cout << "path " << reals[0] << ' ' << reals[1] << ' ' << reals[2] << ' ' << std::endl;
    }
}

void printDistanceGrid(ob::StateSpacePtr space)
{
    // print the distance for (x,y,theta) for all points in a 3D grid in SE(2)
    // over [-5,5) x [-5, 5) x [-pi,pi).
    //
    // The output should be redirected to a file, say, distance.txt. This
    // can then be read and plotted in Matlab like so:
    //     x = reshape(load('distance.txt'),200,200,200);
    //     for i=1:200,
    //         contourf(squeeze(x(i,:,:)),30);
    //         axis equal; axis tight; colorbar; pause;
    //     end;
    const unsigned int num_pts = 200;
    ob::ScopedState<> from(space), to(space);
    from[0] = from[1] = from[2] = 0.;

    for (unsigned int i=0; i<num_pts; ++i)
        for (unsigned int j=0; j<num_pts; ++j)
            for (unsigned int k=0; k<num_pts; ++k)
            {
                to[0] = 5. * (2. * (double)i/num_pts - 1.);
                to[1] = 5. * (2. * (double)j/num_pts - 1.);
                to[2] = boost::math::constants::pi<double>() * (2. * (double)k/num_pts - 1.);
                std::cout << space->distance(from(), to()) << '\n';
            }

}

int main(int argc, char* argv[])
{
    try
    {
        po::options_description desc("Options");
        desc.add_options()
            ("help", "show help message")
            ("clothoid", "use Clothoid state space")
            ("clothoidrev", "use reverse Clothoid state space")
            ("dubins", "use Dubins state space")
            ("dubinssym", "use symmetrized Dubins state space")
            ("reedsshepp", "use Reeds-Shepp state space (default)")
            ("time", po::value<double>(),"plannig time")
            ("trajectory", po::value<std::vector<double > >()->multitoken(),
                "print trajectory from (0,0,0) to a user-specified x, y, and theta")
            ("distance", "print distance grid")
        ;

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc,
            po::command_line_style::unix_style ^ po::command_line_style::allow_short), vm);
        po::notify(vm);

        if (vm.count("help") || argc==1)
        {
            std::cout << desc << "\n";
            return 1;
        }

        ob::StateSpacePtr space(new ob::ReedsSheppStateSpace(5.88));

		bool isClothoid = false;

        if (vm.count("clothoidrev"))
		{
            space = ob::StateSpacePtr(new ob::ClothoidStateSpace(0.17, true));
			isClothoid = true;
		}

		if (vm.count("clothoid"))
		{
			space = ob::StateSpacePtr(new ob::ClothoidStateSpace(0.17, false));
			isClothoid = true;
		}

		if (vm.count("dubins"))
            space = ob::StateSpacePtr(new ob::DubinsStateSpace(5.88, false));
        if (vm.count("dubinssym"))
            space = ob::StateSpacePtr(new ob::DubinsStateSpace(5.88, true));

		double time = 30.0;

		if (vm.count("time"))
			time = vm["time"].as<double>();

		plan(space,time,isClothoid);

        if (vm.count("trajectory"))
            printTrajectory(space, vm["trajectory"].as<std::vector<double> >());
        if (vm.count("distance"))
            printDistanceGrid(space);
    }
    catch(std::exception& e) {
        std::cerr << "error: " << e.what() << "\n";
        return 1;
    }
    catch(...) {
        std::cerr << "Exception of unknown type!\n";
    }

    return 0;
}


#pragma once
#include <memory>
#include <boost/serialization/access.hpp>
#include <boost/serialization/base_object.hpp>
#include "SimpleLayer.hpp"
#include "../optimizer/NeuralNetworkOptimizer.hpp"
#include "neuron/GatedRecurrentUnit.hpp"

namespace snn::internal
{
    class GruLayer final : public SimpleLayer<GatedRecurrentUnit>
    {
    private:
        friend class boost::serialization::access;
        template <class Archive>
        void serialize(Archive& ar, unsigned version);

    public:
        GruLayer() = default;  // use restricted to Boost library only
        GruLayer(LayerModel& model, std::shared_ptr<NeuralNetworkOptimizer> optimizer);
        GruLayer(const GruLayer&) = default;
        ~GruLayer() = default;
        [[nodiscard]] std::unique_ptr<BaseLayer> clone(std::shared_ptr<NeuralNetworkOptimizer> optimizer) const override;
    };

    template <class Archive>
    void GruLayer::serialize(Archive& ar, const unsigned version)
    {
        boost::serialization::void_cast_register<GruLayer, SimpleLayer>();
        ar & boost::serialization::base_object<SimpleLayer>(*this);
    }
}