// Copyright (c) 2008-2017 Emil Dotchevski and Reverge Studios, Inc.

// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_QVM_187092DA6454CEB1853E6915F8466A4
#define BOOST_QVM_187092DA6454CEB1853E6915F8466A4

// This file was generated by a program. Do not edit manually.

#include <boost/qvm/deduce_vec.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>
#include <boost/qvm/mat_traits.hpp>
#include <boost/qvm/vec_traits.hpp>

namespace boost {
namespace qvm {
template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<A>::rows == 4 &&
                                  mat_traits<A>::cols == 4 &&
                                  vec_traits<B>::dim == 4,
                              deduce_vec2<A, B, 4>>::type
    operator*(A const &a, B const &b) {
  typedef typename mat_traits<A>::scalar_type Ta;
  typedef typename vec_traits<B>::scalar_type Tb;
  Ta const a00 = mat_traits<A>::template read_element<0, 0>(a);
  Ta const a01 = mat_traits<A>::template read_element<0, 1>(a);
  Ta const a02 = mat_traits<A>::template read_element<0, 2>(a);
  Ta const a03 = mat_traits<A>::template read_element<0, 3>(a);
  Ta const a10 = mat_traits<A>::template read_element<1, 0>(a);
  Ta const a11 = mat_traits<A>::template read_element<1, 1>(a);
  Ta const a12 = mat_traits<A>::template read_element<1, 2>(a);
  Ta const a13 = mat_traits<A>::template read_element<1, 3>(a);
  Ta const a20 = mat_traits<A>::template read_element<2, 0>(a);
  Ta const a21 = mat_traits<A>::template read_element<2, 1>(a);
  Ta const a22 = mat_traits<A>::template read_element<2, 2>(a);
  Ta const a23 = mat_traits<A>::template read_element<2, 3>(a);
  Ta const a30 = mat_traits<A>::template read_element<3, 0>(a);
  Ta const a31 = mat_traits<A>::template read_element<3, 1>(a);
  Ta const a32 = mat_traits<A>::template read_element<3, 2>(a);
  Ta const a33 = mat_traits<A>::template read_element<3, 3>(a);
  Tb const b0 = vec_traits<B>::template read_element<0>(b);
  Tb const b1 = vec_traits<B>::template read_element<1>(b);
  Tb const b2 = vec_traits<B>::template read_element<2>(b);
  Tb const b3 = vec_traits<B>::template read_element<3>(b);
  typedef typename deduce_vec2<A, B, 4>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 4);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      a00 * b0 + a01 * b1 + a02 * b2 + a03 * b3;
  vec_traits<R>::template write_element<1>(r) =
      a10 * b0 + a11 * b1 + a12 * b2 + a13 * b3;
  vec_traits<R>::template write_element<2>(r) =
      a20 * b0 + a21 * b1 + a22 * b2 + a23 * b3;
  vec_traits<R>::template write_element<3>(r) =
      a30 * b0 + a31 * b1 + a32 * b2 + a33 * b3;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_mv_defined;

template <> struct mul_mv_defined<4, 4> { static bool const value = true; };
} // namespace qvm_detail

template <class A, class B>
BOOST_QVM_INLINE_OPERATIONS
    typename lazy_enable_if_c<mat_traits<B>::rows == 4 &&
                                  mat_traits<B>::cols == 4 &&
                                  vec_traits<A>::dim == 4,
                              deduce_vec2<A, B, 4>>::type
    operator*(A const &a, B const &b) {
  typedef typename vec_traits<A>::scalar_type Ta;
  typedef typename mat_traits<B>::scalar_type Tb;
  Ta const a0 = vec_traits<A>::template read_element<0>(a);
  Ta const a1 = vec_traits<A>::template read_element<1>(a);
  Ta const a2 = vec_traits<A>::template read_element<2>(a);
  Ta const a3 = vec_traits<A>::template read_element<3>(a);
  Tb const b00 = mat_traits<B>::template read_element<0, 0>(b);
  Tb const b01 = mat_traits<B>::template read_element<0, 1>(b);
  Tb const b02 = mat_traits<B>::template read_element<0, 2>(b);
  Tb const b03 = mat_traits<B>::template read_element<0, 3>(b);
  Tb const b10 = mat_traits<B>::template read_element<1, 0>(b);
  Tb const b11 = mat_traits<B>::template read_element<1, 1>(b);
  Tb const b12 = mat_traits<B>::template read_element<1, 2>(b);
  Tb const b13 = mat_traits<B>::template read_element<1, 3>(b);
  Tb const b20 = mat_traits<B>::template read_element<2, 0>(b);
  Tb const b21 = mat_traits<B>::template read_element<2, 1>(b);
  Tb const b22 = mat_traits<B>::template read_element<2, 2>(b);
  Tb const b23 = mat_traits<B>::template read_element<2, 3>(b);
  Tb const b30 = mat_traits<B>::template read_element<3, 0>(b);
  Tb const b31 = mat_traits<B>::template read_element<3, 1>(b);
  Tb const b32 = mat_traits<B>::template read_element<3, 2>(b);
  Tb const b33 = mat_traits<B>::template read_element<3, 3>(b);
  typedef typename deduce_vec2<A, B, 4>::type R;
  BOOST_QVM_STATIC_ASSERT(vec_traits<R>::dim == 4);
  R r;
  vec_traits<R>::template write_element<0>(r) =
      a0 * b00 + a1 * b10 + a2 * b20 + a3 * b30;
  vec_traits<R>::template write_element<1>(r) =
      a0 * b01 + a1 * b11 + a2 * b21 + a3 * b31;
  vec_traits<R>::template write_element<2>(r) =
      a0 * b02 + a1 * b12 + a2 * b22 + a3 * b32;
  vec_traits<R>::template write_element<3>(r) =
      a0 * b03 + a1 * b13 + a2 * b23 + a3 * b33;
  return r;
}

namespace sfinae {
using ::boost::qvm::operator*;
}

namespace qvm_detail {
template <int R, int C> struct mul_vm_defined;

template <> struct mul_vm_defined<4, 4> { static bool const value = true; };
} // namespace qvm_detail

} // namespace qvm
} // namespace boost

#endif


#include <Eigen/Geometry>

#include <astar_planner/astar.h>
#include <astar_planner/plugin.h>
#include <astar_planner/visualisation.h>
#include <gridmap/operations/rasterize.h>
#include <gridmap/operations/raytrace.h>
#include <nav_msgs/OccupancyGrid.h>
#include <navigation_interface/params.h>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <pluginlib/class_list_macros.h>
#include <visualization_msgs/MarkerArray.h>

#include <chrono>

PLUGINLIB_EXPORT_CLASS(astar_planner::AStarPlanner, navigation_interface::PathPlanner)

namespace astar_planner
{

namespace
{

double pathCost(const navigation_interface::Path& path, const astar_planner::CollisionChecker& collision_checker,
                const double backwards_mult, const double strafe_mult, const double rotation_mult)
{
    double cost = 0.0;
    for (std::size_t i = 0; i < path.nodes.size() - 1; ++i)
    {
        const State3D state{path.nodes[i].translation().x(), path.nodes[i].translation().y(),
                            Eigen::Rotation2Dd(path.nodes[i].linear()).smallestAngle()};
        if (!collision_checker.isValid(state))
            return std::numeric_limits<double>::max();

        const Eigen::Vector2d dir = path.nodes[i + 1].translation() - path.nodes[i].translation();
        const Eigen::Vector2d dir_wrt_robot = path.nodes[i].linear().inverse() * dir;
        const Eigen::Rotation2Dd rotation(path.nodes[i].linear().inverse() * path.nodes[i + 1].linear());

        const double x_cost = std::abs((dir_wrt_robot[0] > 0) ? dir_wrt_robot[0] : backwards_mult * dir_wrt_robot[0]);
        const double y_cost = std::abs(strafe_mult * dir_wrt_robot[1]);

        const Eigen::Array2i map_point = collision_checker.costmap().getCellIndex(path.nodes[i].translation());

        const double collision_cost = collisionCost(map_point.x(), map_point.y(), collision_checker);
        const double traversal_cost = traversalCost(map_point.x(), map_point.y(), collision_checker.costmap());

        const double d_to_collision_m = collision_checker.clearance(state) * collision_checker.costmap().resolution;
        const double rotation_collision_cost = rotationCollisionCost(d_to_collision_m);

        cost += (x_cost + y_cost) * traversal_cost * collision_cost;
        cost += std::abs(rotation.smallestAngle()) * rotation_mult * rotation_collision_cost;
    }
    return cost;
}
}  // namespace

AStarPlanner::AStarPlanner()
{
}

AStarPlanner::~AStarPlanner()
{
}

navigation_interface::PathPlanner::Result  // cppcheck-suppress unusedFunction
    AStarPlanner::plan(const Eigen::Isometry2d& start, const Eigen::Isometry2d& goal, const GoalSampleSettings& sample)
{
    navigation_interface::PathPlanner::Result result;

    {
        // cppcheck-suppress unreadVariable
        auto lock = map_data_->grid.getLock();
        costmap_ = std::make_shared<Costmap>(*map_data_, robot_radius_);
    }

    // clear the robot footprint
    const int radius_px = static_cast<int>(robot_radius_ / costmap_->resolution);
    for (const auto& offset : offsets_)
    {
        const Eigen::Vector2d p = start * offset;
        const Eigen::Array2i map_cell = costmap_->getCellIndex({p.x(), p.y()});
        cv::circle(costmap_->obstacle_map, cv::Point(map_cell.x(), map_cell.y()), radius_px, cv::Scalar(0), -1);
    }

    costmap_->processObstacleMap();

    ROS_ASSERT(traversal_cost_);
    costmap_->traversal_cost = traversal_cost_;
    const astar_planner::CollisionChecker collision_checker(*costmap_, offsets_, conservative_robot_radius_);

    const size_t max_iterations = 3e5;
    const double linear_resolution = 0.04;
    const double angular_resolution = M_PI / 16;

    const auto t0 = std::chrono::steady_clock::now();

    const astar_planner::PathResult astar_result =
        astar_planner::hybridAStar(start, goal, max_iterations, collision_checker, linear_resolution,
                                   angular_resolution, sample, backwards_mult_, strafe_mult_, rotation_mult_);

    ROS_INFO_STREAM(
        "Hybrid A Star took "
        << std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::steady_clock::now() - t0).count()
        << " iterations: " << astar_result.iterations << " nodes: " << astar_result.explore_3d.size());

    if (debug_viz_)
    {
        if (explore_pub_.getNumSubscribers() > 0)
        {
            cv::Mat disp = astar_planner::visualise(*costmap_, astar_result);
            cv::cvtColor(disp, disp, cv::COLOR_BGR2GRAY);

            nav_msgs::OccupancyGrid og;
            og.header.stamp = ros::Time::now();
            og.info.resolution = costmap_->resolution;
            og.info.width = costmap_->width;
            og.info.height = costmap_->height;
            og.data.resize(static_cast<size_t>(og.info.width * og.info.height));
            og.info.origin.position.x = costmap_->origin_x;
            og.info.origin.position.y = costmap_->origin_y;
            og.info.origin.orientation.w = 1.0;

            unsigned char* input = (unsigned char*)(disp.data);
            std::copy_n(input, og.data.size(), &og.data[0]);

            explore_pub_.publish(og);
        }
    }

    if (astar_result.success)
    {
        result.path.nodes.push_back(start);

        for (auto r_it = astar_result.path.crbegin(); r_it != astar_result.path.crend(); ++r_it)
        {
            const Eigen::Isometry2d p =
                Eigen::Translation2d((*r_it)->state.x, (*r_it)->state.y) * Eigen::Rotation2Dd((*r_it)->state.theta);
            result.path.nodes.push_back(p);
        }

        result.cost = astar_result.path.front()->cost_so_far;
        result.outcome = navigation_interface::PathPlanner::Outcome::SUCCESSFUL;
    }
    else
    {
        if (astar_result.start_in_collision)
            ROS_WARN("Start in collision!");
        if (astar_result.goal_in_collision)
            ROS_WARN("Goal in collision!");
        result.outcome = navigation_interface::PathPlanner::Outcome::FAILED;
    }
    return result;
}

// cppcheck-suppress unusedFunction
bool AStarPlanner::valid(const navigation_interface::Path& path) const
{
    // assume this is called immediately after plan to re-use the data structures
    ROS_ASSERT(costmap_);

    const astar_planner::CollisionChecker collision_checker(*costmap_, offsets_, conservative_robot_radius_);
    return pathCost(path, collision_checker, backwards_mult_, strafe_mult_, rotation_mult_) <
           std::numeric_limits<double>::max();
}

double AStarPlanner::cost(const navigation_interface::Path& path) const
{
    // assume this is called immediately after plan to re-use the data structures
    ROS_ASSERT(costmap_);

    const astar_planner::CollisionChecker collision_checker(*costmap_, offsets_, conservative_robot_radius_);
    return pathCost(path, collision_checker, backwards_mult_, strafe_mult_, rotation_mult_);
}

// cppcheck-suppress unusedFunction
void AStarPlanner::onInitialize(const YAML::Node& parameters)
{
    debug_viz_ = parameters["debug_viz"].as<bool>(debug_viz_);
    robot_radius_ = parameters["robot_radius"].as<double>(robot_radius_);
    conservative_robot_radius_ = parameters["conservative_robot_radius"].as<double>(conservative_robot_radius_);
    avoid_zone_cost_ = parameters["avoid_zone_cost"].as<double>(avoid_zone_cost_);
    path_cost_ = parameters["path_cost"].as<double>(path_cost_);

    backwards_mult_ = parameters["backwards_mult"].as<double>(backwards_mult_);
    strafe_mult_ = parameters["strafe_mult"].as<double>(strafe_mult_);
    rotation_mult_ = parameters["rotation_mult"].as<double>(rotation_mult_);

    offsets_ = navigation_interface::get_point_list(parameters, "robot_radius_offsets",
                                                    {{-0.268, 0.000},
                                                     {0.268, 0.000},
                                                     {0.265, -0.185},
                                                     {0.077, -0.185},
                                                     {-0.077, -0.185},
                                                     {-0.265, -0.185},
                                                     {0.265, 0.185},
                                                     {-0.265, 0.185},
                                                     {-0.077, 0.185},
                                                     {0.077, 0.185}});

    if (debug_viz_)
    {
        ros::NodeHandle nh("~");
        explore_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("expansion", 100);
    }
}

// cppcheck-suppress unusedFunction
void AStarPlanner::onMapDataChanged()
{
    ROS_INFO("Building avoid zone traversal costmap");

    // need to generate a data structure for zones
    traversal_cost_ = std::make_shared<cv::Mat>(map_data_->grid.dimensions().size().y(),
                                                map_data_->grid.dimensions().size().x(), CV_32F, cv::Scalar(1.0));

    for (const hd_map::Zone& zone : map_data_->hd_map.zones)
    {
        if (zone.zone_type == hd_map::Zone::AVOID_ZONE)
        {
            int min_x = std::numeric_limits<int>::max();
            int max_x = 0;

            int min_y = std::numeric_limits<int>::max();
            int max_y = 0;

            std::vector<Eigen::Array2i> map_polygon;
            for (const geometry_msgs::Point32& p : zone.polygon.points)
            {
                const Eigen::Array2i map_point = map_data_->grid.dimensions().getCellIndex({p.x, p.y});
                min_x = std::min(map_point.x(), min_x);
                max_x = std::max(map_point.x(), max_x);
                min_y = std::min(map_point.y(), min_y);
                max_y = std::max(map_point.y(), max_y);
                map_polygon.push_back(map_point);
            }
            if (!map_polygon.empty())
                map_polygon.push_back(map_polygon.front());

            const std::vector<Eigen::Array2i> connected = gridmap::connectPolygon(map_polygon);

            cv::Mat& traversal_cost = *(traversal_cost_.get());
            const float avoid_zone_cost = static_cast<float>(avoid_zone_cost_);
            auto append_raster = [&traversal_cost, avoid_zone_cost](const int x, const int y) {
                if (x >= 0 && x < traversal_cost.cols && y >= 0 && y < traversal_cost.rows)
                {
                    traversal_cost.at<float>(y, x) = avoid_zone_cost;
                }
            };

            gridmap::rasterPolygonFill(append_raster, connected, min_x, max_x, min_y, max_y);
        }
    }

    for (const hd_map::Path& path : map_data_->hd_map.paths)
    {
        ROS_INFO_STREAM("Loading path: " << path.name);
        for (size_t i = 0; i < path.nodes.size() - 1; ++i)
        {
            const std::string first_id = path.nodes[i];
            const std::string next_id = path.nodes[i + 1];

            auto first_it = std::find_if(map_data_->hd_map.nodes.begin(), map_data_->hd_map.nodes.end(),
                                         [&first_id](const hd_map::Node& n) { return n.id == first_id; });
            ROS_ASSERT(first_it != map_data_->hd_map.nodes.end());
            hd_map::Node start_node = *first_it;

            auto next_it = std::find_if(map_data_->hd_map.nodes.begin(), map_data_->hd_map.nodes.end(),
                                        [&next_id](const hd_map::Node& n) { return n.id == next_id; });
            ROS_ASSERT(next_it != map_data_->hd_map.nodes.end());
            hd_map::Node end_node = *next_it;

            const Eigen::Array2i start_mp = map_data_->grid.dimensions().getCellIndex({start_node.x, start_node.y});
            const Eigen::Array2i end_mp = map_data_->grid.dimensions().getCellIndex({end_node.x, end_node.y});

            cv::line(*traversal_cost_, cv::Point(start_mp[0], start_mp[1]), cv::Point(end_mp[0], end_mp[1]), path_cost_,
                     10);
        }
    }

    // blue the traversal cost map to help provide a smooth manifold for planning
    cv::GaussianBlur(*traversal_cost_, *traversal_cost_, cv::Size(11, 11), 0);
}

}  // namespace astar_planner


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <sdf_tools/sdf.hpp>

#ifndef SDF_GENERATION_HPP
#define SDF_GENERATION_HPP

namespace sdf_generation
{
    struct bucket_cell
    {
        double distance_square;
        int32_t update_direction;
        uint32_t location[3];
        uint32_t closest_point[3];
    };

    typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;

    inline int GetDirectionNumber(const int dx, const int dy, const int dz)
    {
        return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
    }

    inline std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods()
    {
        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods;
        neighborhoods.resize(2);
        for (size_t n = 0; n < neighborhoods.size(); n++)
        {
            neighborhoods[n].resize(27);
            // Loop through the source directions
            for (int dx = -1; dx <= 1; dx++)
            {
                for (int dy = -1; dy <= 1; dy++)
                {
                    for (int dz = -1; dz <= 1; dz++)
                    {
                        int direction_number = GetDirectionNumber(dx, dy, dz);
                        // Loop through the target directions
                        for (int tdx = -1; tdx <= 1; tdx++)
                        {
                            for (int tdy = -1; tdy <= 1; tdy++)
                            {
                                for (int tdz = -1; tdz <= 1; tdz++)
                                {
                                    if (tdx == 0 && tdy == 0 && tdz == 0)
                                    {
                                        continue;
                                    }
                                    if (n >= 1)
                                    {
                                        if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
                                        {
                                            continue;
                                        }
                                        if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
                                        {
                                            continue;
                                        }
                                    }
                                    std::vector<int> new_point;
                                    new_point.resize(3);
                                    new_point[0] = tdx;
                                    new_point[1] = tdy;
                                    new_point[2] = tdz;
                                    neighborhoods[n][direction_number].push_back(new_point);
                                }
                            }
                        }
                    }
                }
            }
        }
        return neighborhoods;
    }

    inline double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2)
    {
        int32_t dx = x1 - x2;
        int32_t dy = y1 - y2;
        int32_t dz = z1 - z2;
        return double((dx * dx) + (dy * dy) + (dz * dz));
    }

    inline DistanceField BuildDistanceField(const Eigen::Isometry3d& grid_origin_tranform,
                                            const double grid_resolution,
                                            const int64_t grid_num_x_cells,
                                            const int64_t grid_num_y_cells,
                                            const int64_t grid_num_z_cells,
                                            const std::vector<VoxelGrid::GRID_INDEX>& points)
    {
        // Make the DistanceField container
        bucket_cell default_cell;
        default_cell.distance_square = std::numeric_limits<double>::infinity();
        DistanceField distance_field(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, default_cell);
        // Compute maximum distance square
        long max_distance_square = (distance_field.GetNumXCells() * distance_field.GetNumXCells()) + (distance_field.GetNumYCells() * distance_field.GetNumYCells()) + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
        // Make bucket queue
        std::vector<std::vector<bucket_cell>> bucket_queue(max_distance_square + 1);
        bucket_queue[0].reserve(points.size());
        // Set initial update direction
        int initial_update_direction = GetDirectionNumber(0, 0, 0);
        // Mark all points with distance zero and add to the bucket queue
        for (size_t index = 0; index < points.size(); index++)
        {
            const VoxelGrid::GRID_INDEX& current_index = points[index];
            std::pair<bucket_cell&, bool> query = distance_field.GetMutable(current_index);
            if (query.second)
            {
                query.first.location[0] = current_index.x;
                query.first.location[1] = current_index.y;
                query.first.location[2] = current_index.z;
                query.first.closest_point[0] = current_index.x;
                query.first.closest_point[1] = current_index.y;
                query.first.closest_point[2] = current_index.z;
                query.first.distance_square = 0.0;
                query.first.update_direction = initial_update_direction;
                bucket_queue[0].push_back(query.first);
            }
            // If the point is outside the bounds of the SDF, skip
            else
            {
                throw std::runtime_error("Point for BuildDistanceField out of bounds");
            }
        }
        // Process the bucket queue
        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods = MakeNeighborhoods();
        for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
        {
            std::vector<bucket_cell>::iterator queue_itr = bucket_queue[bq_idx].begin();
            while (queue_itr != bucket_queue[bq_idx].end())
            {
                // Get the current location
                bucket_cell& cur_cell = *queue_itr;
                double x = cur_cell.location[0];
                double y = cur_cell.location[1];
                double z = cur_cell.location[2];
                // Pick the update direction
                int D = bq_idx;
                if (D > 1)
                {
                    D = 1;
                }
                // Make sure the update direction is valid
                if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
                {
                    ++queue_itr;
                    continue;
                }
                // Get the current neighborhood list
                std::vector<std::vector<int>>& neighborhood = neighborhoods[D][cur_cell.update_direction];
                // Update the distance from the neighboring cells
                for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
                {
                    // Get the direction to check
                    int dx = neighborhood[nh_idx][0];
                    int dy = neighborhood[nh_idx][1];
                    int dz = neighborhood[nh_idx][2];
                    int nx = x + dx;
                    int ny = y + dy;
                    int nz = z + dz;
                    std::pair<bucket_cell&, bool> neighbor_query = distance_field.GetMutable((int64_t)nx, (int64_t)ny, (int64_t)nz);
                    if (!neighbor_query.second)
                    {
                        // "Neighbor" is outside the bounds of the SDF
                        continue;
                    }
                    // Update the neighbor's distance based on the current
                    int new_distance_square = ComputeDistanceSquared(nx, ny, nz, cur_cell.closest_point[0], cur_cell.closest_point[1], cur_cell.closest_point[2]);
                    if (new_distance_square > max_distance_square)
                    {
                        // Skip these cases
                        continue;
                    }
                    if (new_distance_square < neighbor_query.first.distance_square)
                    {
                        // If the distance is better, time to update the neighbor
                        neighbor_query.first.distance_square = new_distance_square;
                        neighbor_query.first.closest_point[0] = cur_cell.closest_point[0];
                        neighbor_query.first.closest_point[1] = cur_cell.closest_point[1];
                        neighbor_query.first.closest_point[2] = cur_cell.closest_point[2];
                        neighbor_query.first.location[0] = nx;
                        neighbor_query.first.location[1] = ny;
                        neighbor_query.first.location[2] = nz;
                        neighbor_query.first.update_direction = GetDirectionNumber(dx, dy, dz);
                        // Add the neighbor into the bucket queue
                        bucket_queue[new_distance_square].push_back(neighbor_query.first);
                    }
                }
                // Increment the queue iterator
                ++queue_itr;
            }
            // Clear the current queue now that we're done with it
            bucket_queue[bq_idx].clear();
        }
        return distance_field;
    }

    template<typename T>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const Eigen::Isometry3d& grid_origin_tranform,
                                                                                const double grid_resolution,
                                                                                const int64_t grid_num_x_cells,
                                                                                const int64_t grid_num_y_cells,
                                                                                const int64_t grid_num_z_cells,
                                                                                const std::function<bool(const VoxelGrid::GRID_INDEX&)>& is_filled_fn,
                                                                                const float oob_value,
                                                                                const std::string& frame)
    {
        std::vector<VoxelGrid::GRID_INDEX> filled;
        std::vector<VoxelGrid::GRID_INDEX> free;
        for (int64_t x_index = 0; x_index < grid_num_x_cells; x_index++)
        {
            for (int64_t y_index = 0; y_index < grid_num_y_cells; y_index++)
            {
                for (int64_t z_index = 0; z_index < grid_num_z_cells; z_index++)
                {
                    const VoxelGrid::GRID_INDEX current_index(x_index, y_index, z_index);
                    if (is_filled_fn(current_index))
                    {
                        // Mark as filled
                        filled.push_back(current_index);
                    }
                    else
                    {
                        // Mark as free space
                        free.push_back(current_index);
                    }
                }
            }
        }
        // Make two distance fields (one for distance to filled voxels, one for distance to free voxels
        const DistanceField filled_distance_field = BuildDistanceField(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, filled);
        const DistanceField free_distance_field = BuildDistanceField(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, free);
        // Generate the SDF
        sdf_tools::SignedDistanceField new_sdf(grid_origin_tranform, frame, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, oob_value);
        double max_distance = -std::numeric_limits<double>::infinity();
        double min_distance = std::numeric_limits<double>::infinity();
        for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
        {
            for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
            {
                for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
                {
                    const double distance1 = std::sqrt(filled_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                    const double distance2 = std::sqrt(free_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                    const double distance = distance1 - distance2;
                    if (distance > max_distance)
                    {
                        max_distance = distance;
                    }
                    if (distance < min_distance)
                    {
                        min_distance = distance;
                    }
                    new_sdf.SetValue(x_index, y_index, z_index, distance);
                }
            }
        }
        std::pair<double, double> extrema(max_distance, min_distance);
        return std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>>(new_sdf, extrema);
    }

    template<typename T, typename BackingStore=std::vector<T>>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const VoxelGrid::VoxelGrid<T, BackingStore>& grid, const std::function<bool(const VoxelGrid::GRID_INDEX&)>& is_filled_fn, const float oob_value, const std::string& frame, const bool add_virtual_border)
    {
      (void)(add_virtual_border);
      const Eigen::Vector3d cell_sizes = grid.GetCellSizes();
      if ((cell_sizes.x() != cell_sizes.y()) || (cell_sizes.x() != cell_sizes.z()))
      {
        throw std::invalid_argument("Grid must have uniform resolution");
      }
      if (add_virtual_border == false)
      {
        // This is the conventional single-pass result
        return ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), grid.GetNumXCells(), grid.GetNumYCells(), grid.GetNumZCells(), is_filled_fn, oob_value, frame);
      }
      else
      {
        const int64_t x_axis_size_offset = (grid.GetNumXCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t x_axis_query_offset = (grid.GetNumXCells() > 1) ? (int64_t)1 : (int64_t)0;
        const int64_t y_axis_size_offset = (grid.GetNumYCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t y_axis_query_offset = (grid.GetNumYCells() > 1) ? (int64_t)1 : (int64_t)0;
        const int64_t z_axis_size_offset = (grid.GetNumZCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t z_axis_query_offset = (grid.GetNumZCells() > 1) ? (int64_t)1 : (int64_t)0;
        // We need to lie about the size of the grid to add a virtual border
        const int64_t num_x_cells = grid.GetNumXCells() + x_axis_size_offset;
        const int64_t num_y_cells = grid.GetNumYCells() + y_axis_size_offset;
        const int64_t num_z_cells = grid.GetNumZCells() + z_axis_size_offset;
        // Make some deceitful helper functions that hide our lies about size
        // For the free space SDF, we lie and say the virtual border is filled
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> free_is_filled_fn
            = [&] (const VoxelGrid::GRID_INDEX& virtual_border_grid_index)
        {
          // Is there a virtual border on our axis?
          if (x_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.x == 0)
                || (virtual_border_grid_index.x == (num_x_cells - 1)))
            {
              return true;
            }
          }
          // Is there a virtual border on our axis?
          if (y_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.y == 0)
                || (virtual_border_grid_index.y == (num_y_cells - 1)))
            {
              return true;
            }
          }
          // Is there a virtual border on our axis?
          if (z_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.z == 0)
                || (virtual_border_grid_index.z == (num_z_cells - 1)))
            {
              return true;
            }
          }
          const VoxelGrid::GRID_INDEX real_grid_index(
                virtual_border_grid_index.x - x_axis_query_offset,
                virtual_border_grid_index.y - y_axis_query_offset,
                virtual_border_grid_index.z - z_axis_query_offset);
          return is_filled_fn(real_grid_index);
        };
        // For the filled space SDF, we lie and say the virtual border is empty
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> filled_is_filled_fn
            = [&] (const VoxelGrid::GRID_INDEX& virtual_border_grid_index)
        {
          // Is there a virtual border on our axis?
          if (x_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.x == 0)
                || (virtual_border_grid_index.x == (num_x_cells - 1)))
            {
              return false;
            }
          }
          // Is there a virtual border on our axis?
          if (y_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.y == 0)
                || (virtual_border_grid_index.y == (num_y_cells - 1)))
            {
              return false;
            }
          }
          // Is there a virtual border on our axis?
          if (z_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.z == 0)
                || (virtual_border_grid_index.z == (num_z_cells - 1)))
            {
              return false;
            }
          }
          const VoxelGrid::GRID_INDEX real_grid_index(
                virtual_border_grid_index.x - x_axis_query_offset,
                virtual_border_grid_index.y - y_axis_query_offset,
                virtual_border_grid_index.z - z_axis_query_offset);
          return is_filled_fn(real_grid_index);
        };
        // Make both SDFs
        auto free_sdf_result = ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), num_x_cells, num_y_cells, num_z_cells, free_is_filled_fn, oob_value, frame);
        auto filled_sdf_result = ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), num_x_cells, num_y_cells, num_z_cells, filled_is_filled_fn, oob_value, frame);
        // Combine to make a single SDF
        sdf_tools::SignedDistanceField combined_sdf(grid.GetOriginTransform(), frame, cell_sizes.x(), grid.GetNumXCells(), grid.GetNumYCells(), grid.GetNumZCells(), oob_value);
        for (int64_t x_idx = 0; x_idx < combined_sdf.GetNumXCells(); x_idx++)
        {
          for (int64_t y_idx = 0; y_idx < combined_sdf.GetNumYCells(); y_idx++)
          {
            for (int64_t z_idx = 0; z_idx < combined_sdf.GetNumZCells(); z_idx++)
            {
              const int64_t query_x_idx = x_idx + x_axis_query_offset;
              const int64_t query_y_idx = y_idx + y_axis_query_offset;
              const int64_t query_z_idx = z_idx + z_axis_query_offset;
              const float free_sdf_value
                  = free_sdf_result.first.GetImmutable(
                      query_x_idx, query_y_idx, query_z_idx).first;
              const float filled_sdf_value
                  = filled_sdf_result.first.GetImmutable(
                      query_x_idx, query_y_idx, query_z_idx).first;
              if (free_sdf_value >= 0.0)
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, free_sdf_value);
              }
              else if (filled_sdf_value <= -0.0)
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, filled_sdf_value);
              }
              else
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, 0.0f);
              }
            }
          }
        }
        // Get the combined max/min values
        const std::pair<double, double> combined_extrema(
              free_sdf_result.second.first, filled_sdf_result.second.second);
        return std::make_pair(combined_sdf, combined_extrema);
      }
    }

    template<typename T, typename BackingStore=std::vector<T>>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const VoxelGrid::VoxelGrid<T, BackingStore>& grid, const std::function<bool(const T&)>& is_filled_fn, const float oob_value, const std::string& frame)
    {
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> real_is_filled_fn = [&] (const VoxelGrid::GRID_INDEX& index)
        {
            const T& stored = grid.GetImmutable(index).first;
            // If it matches an object to use OR there are no objects supplied
            if (is_filled_fn(stored))
            {
                // Mark as filled
                return true;
            }
            else
            {
                // Mark as free space
                return false;
            }
        };
        return ExtractSignedDistanceField(grid, real_is_filled_fn, oob_value, frame, false);
    }
}

#endif // SDF_GENERATION_HPP


// (C) Copyright Shou Hao Ho   2018
// Distributed under the MIT Software License (See accompanying LICENSE file)

#include <Eigen/Dense>
#include <boost/range/combine.hpp>
#include <boost/range/adaptors.hpp>
#include <numeric>

#include "placement.h"
#include "plan.h"

namespace Utils {

    namespace impl {

        Plan::coord get_pin_coord(const Plan &plan, const Atom &atom, const Plan::plan_region &region) {
            Plan::coord nearest_coord;

            if (auto c = plan.get_coord(static_cast<const IPin&>(atom))) {
                nearest_coord = *c;
            }
            else if (auto c = plan.get_coord(static_cast<const OPin&>(atom))) {
                nearest_coord = *c;
            }
            else {
                nearest_coord = plan.get_coord(atom);
            }

            if (nearest_coord.x < region.first.begin) {
                nearest_coord.x = region.first.begin;
            }
            else if (nearest_coord.x > region.first.end) {
                nearest_coord.x = region.first.end;
            }

            if (nearest_coord.y < region.second.begin) {
                nearest_coord.y = region.second.begin;
            }
            else if (nearest_coord.y > region.second.end) {
                nearest_coord.y = region.second.end;
            }

            return nearest_coord;
        }

    }

    void dump_plan(const Plan &plan, std::ostream &os) {
        for (const auto &entry : plan.board()) {
            os << "(" << entry.second.x << "," << entry.second.y << ")\n";
        }
    }

    Plan quadratic_placement(std::size_t width, std::size_t height, const Netlist &netlist, int num_iter,
        Plan::partitioning_method method, std::size_t expected_phases, metric_consumer* met) {
        double pin_weight_factor = 1.0 / expected_phases;

        Plan plan{ width, height, netlist };
        auto has_fanin = [](const IPort &iport) { return iport.has_fanin(); };

        std::int64_t avg_conn_per_ipin = std::accumulate(netlist.begin_ipins(), netlist.end_ipins(), 0,
            [](std::int64_t prev, const IPin &ipin) { return prev + ipin.get_oport().size(); });
        avg_conn_per_ipin /= netlist.num_ipins();

        bool split_vertically = true;
        for (int i = 0; i < num_iter; ++i) {
            if (i > 0) {
                plan.recursive_partition(split_vertically, method);
                split_vertically = !split_vertically;
            }

            std::vector<std::vector<Plan::coord>> solutions;
            for (const auto &entry : boost::combine(plan.partitions(), plan.bounds())) {
                const Plan::Partition &partition = boost::get<0>(entry);
                const Plan::plan_region &region = boost::get<1>(entry);
                if (partition.size() == 0) continue;

                std::unordered_map<const Atom*, std::size_t> atom_to_index;
                std::size_t i = 0;
                for (const Atom* atom : partition) {
                    atom_to_index[atom] = i++;
                }

                Eigen::MatrixXd A = Eigen::MatrixXd::Zero(partition.size(), partition.size());

                Eigen::VectorXd b_x = Eigen::VectorXd::Zero(partition.size());
                Eigen::VectorXd b_y = Eigen::VectorXd::Zero(partition.size());

                for (std::size_t x = 0; x < partition.size(); ++x) {
                    const Atom* atom = partition[x];

                    auto register_target = [&](double inv_weight, const Atom &target_atom) {
                        if (&target_atom == atom) return;
                        A(x, x) += inv_weight;

                        auto idx_iter = atom_to_index.find(&target_atom);
                        if (idx_iter != atom_to_index.end()) {
                            std::size_t y = idx_iter->second;
                            RUNTIME_ASSERT(x != y);
                            A(x, y) += -inv_weight;
                        }
                        else {
                            auto coord = impl::get_pin_coord(plan, target_atom, region);
                            inv_weight *= pin_weight_factor;
                            if (target_atom.get_type() == Atom::type::OPIN) inv_weight *= avg_conn_per_ipin;

                            b_x(x) += inv_weight * coord.x;
                            b_y(x) += inv_weight * coord.y;
                        }
                    };

                    for (const IPort &iport : atom->inputs() | boost::adaptors::filtered(has_fanin)) {
                        const OPort* target_oport = iport.fanin();
                        double inv_weight = 1.0 / target_oport->size();
                        const Atom &target_atom = target_oport->get_atom();

                        register_target(inv_weight, target_atom);
                    }

                    for (const OPort &oport : atom->outputs()) {
                        if (oport.empty()) continue;
                        double inv_weight = 1.0 / oport.size();

                        for (const IPort* target_iport : oport) {
                            const Atom &target_atom = target_iport->get_atom();

                            register_target(inv_weight, target_atom);
                        }
                    }
                }

                A.ldlt().solveInPlace(b_x);
                A.ldlt().solveInPlace(b_y);

                std::vector<Plan::coord> sol(partition.size());
                for (std::size_t j = 0; j < partition.size(); ++j) {
                    sol[j] = Plan::coord{ b_x(j), b_y(j) };
                }

                solutions.emplace_back(std::move(sol));
            }

            for (std::size_t i = 0; i < solutions.size(); ++i) {
                plan.assign_coords(plan.partitions()[i], solutions[i], plan.bounds()[i]);
            }

            if (met != nullptr) {
                met->snapshot() << "ss " << i << " (" << width << "," << height << "):\n";
                dump_plan(plan, met->snapshot());
            }
        }

        return plan;
    }

}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C)  2017 Cord Harms

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file localcorrsurface.hpp
    \brief Local Correlation surface derived ....
*/

#include <ql/experimental/termstructures/Helper/ParticleMethodUtils.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/experimental/templatemodels/multiasset/localcorrelationSLVmodel.hpp>
#include <ql/experimental/templatemodels/montecarlo/montecarlomodells.hpp>
#include <ql/experimental/templatemodels/montecarlo/mcpayoffT.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <math.h>
#include <boost/math/distributions.hpp>
#include <boost/shared_ptr.hpp>
#include <ql\pricingengines\blackscholescalculator.hpp>

namespace QuantLib {

	void ParticleMethodUtils::calibrateFX(Handle<LocalCorrSurfaceABFFX>& surface, const std::string& kernelIn, unsigned int numberOfPaths, Time maxTime,
		Time deltaT, Time tMin, Real kappa, Real sigmaAVR, Real exponentN, Real gridMinQuantile,
		Real gridMaxQuantile, unsigned int ns1, unsigned int ns2) {

		boost::shared_ptr<KernelInterface> kernel;

		if (kernelIn == "QuarticKernel") {
			kernel = boost::shared_ptr<KernelInterface>(new QuarticKernel());
		}
		else {
			QL_REQUIRE(false, "Kernel not supported. Supported is: QuarticKernel");
		}

		std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>> processes = surface->getProcesses();
		boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>			    processToCal= surface->getProcessToCal();

		//build assetModel for simulation
		boost::shared_ptr<YieldTermStructure> yld(new FlatForward(processToCal->blackVolatility()->referenceDate(),0, processToCal->blackVolatility()->dayCounter()));
		Handle<YieldTermStructure> yldH = Handle<YieldTermStructure>(yld);
		std::vector<std::string> aliases(2);
		aliases[0] = "fx1";
		aliases[1] = "fx2";
		Handle<LocalCorrTermStructure> surfaceGen(surface.currentLink());
		boost::shared_ptr<LocalCorrelationSLVModel> assetModel = boost::shared_ptr<LocalCorrelationSLVModel>((new LocalCorrelationSLVModel(yldH, aliases, processes, surfaceGen)));

		std::vector<Time>& times = surface->getTimes();
		std::vector<std::vector<Real>>& strikes = surface->getStrikes();
		std::vector<std::vector<Real>>& surfaceF = surface->getSurfaceF();

		//time grid from t=0 to maxTime:
		times.resize(1);
		times[0] = 0;
		size_t i = 0;
		while (times[i] < maxTime) {
			times.push_back(times[i]+deltaT);
			i++;
		}
		surface->setInterpolationTime<Linear>();

		RealMCSimulation simulation(assetModel, times, times, numberOfPaths,1,true,true,false);
		simulation.prepareForSlicedSimulation();
		simulation.simulate(0, false);

		//create strike grid.
		//the strike grid depends on simulation results (min and max quantile)
		//the simulation itself depends on local correlation which itself is calibrated using this function.
		//Consequence: strike grid can merely be calculated iteratively during calibration

		size_t numberStrikes;
		Real strikeStep;
		surfaceF.resize(times.size());
		strikes.resize(times.size());
		std::vector<Real> state;
		std::vector<Real> assets(2);
		std::vector<Real> crossFX(numberOfPaths);

		Real vol1;
		Real vol2;
		std::vector<Real> vol3;
		std::vector<Real> eNum;
		std::vector<Real> eDen;
		std::vector<Real> eScale;
		Real a = 0;
		Real b = 0;
		Real kernelV = 0;
		Real bandwidth = 0;
		Real minPosBw;
		Real maxNegBw;
		Real bwIn;
		Real bwRatio;

		//Calculate local correlation successively over time

		for (size_t i = 1; i < surfaceF.size()-1; i++) //iteration over time, for i=0 nothing to do as correlation independent of a,b,f. In last entry, no additional simulation necessary.
		{
			numberStrikes = numberStrikeGrid(times[i],ns1,ns2);
			QL_REQUIRE(numberStrikes>1,"ns1 or ns2 has to be increased, strike grid cannot be calculated.");
			simulation.simulate(i,false);

			//Now strike grid can be calculated

			surfaceF[i].resize(numberStrikes);
			strikes[i].resize(numberStrikes);
			for (size_t k = 0; k < numberOfPaths; k++)
			{
				state = simulation.state(k, times[i]);
				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);
				crossFX[k] = getCrossFX(assets[0] , assets[1]);
			}

			std::sort(crossFX.begin(),crossFX.end());

			strikes[i][0] = crossFX[(size_t)(numberOfPaths*gridMinQuantile)];
			strikes[i][strikes[i].size()-1] = crossFX[(size_t)(numberOfPaths*gridMaxQuantile)];

			strikeStep = (strikes[i][strikes[i].size() - 1] - strikes[i][0]) / (numberStrikes-1);

			QL_ASSERT(strikeStep > 0, "Error. StrikeStep shouldn't be zero for time " << times[i] << ". Check volatility.");

			bandwidth = ParticleMethodUtils::bandwidth(times[i], getCrossFX(processes[0]->s0()->value(),processes[1]->s0()->value()), kappa, sigmaAVR, tMin, numberOfPaths, exponentN);

			for (size_t j = 1; j < strikes[i].size()-1; j++)
			{
				strikes[i][j] = strikes[i][j-1] + strikeStep;
			}

			//Particle method for that time step

			eNum.resize(strikes[i].size());
			eDen.resize(strikes[i].size());
			eScale.resize(strikes[i].size());
			vol3.resize(strikes[i].size());

			for (size_t j = 0; j < strikes[i].size(); j++)
			{
				eNum[j] = 0;
				eDen[j] = 0;
				eScale[j] = 0;
				vol3[j] = processToCal->localVolatility()->localVol(times[i], strikes[i][j], true);
			}

			for (size_t k = 0; k < numberOfPaths; k++) //particle method: over MC paths
			{
				state = simulation.state(k, times[i]);

				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);

				state[2] = state[2] < 0 ? 0.0001 : state[2];//heston vol might become negative (dep. on feller constant)
				state[3] = state[3] < 0 ? 0.0001 : state[3];

				vol1 = processes[0]->leverageFct()->localVol(times[i], assets[0], true)*std::sqrt(state[2]); //*ai from Heston
				vol2 = processes[1]->leverageFct()->localVol(times[i], assets[1], true)*std::sqrt(state[3]);

				a = surface->localA(times[i], assets, true);
				b = surface->localB(times[i], assets, true);

				if (vol1 != vol1 || vol2 != vol2)
					QL_FAIL("surface of asset1 oder asset2 not well defined");

				for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
				{

					minPosBw = 10000000;
					maxNegBw = -10000000;

					bwIn = getCrossFX(assets[0], assets[1]) - strikes[i][j];
					bwRatio = bwIn / bandwidth;

					if (bwRatio > maxNegBw && bwRatio <= 0) maxNegBw = bwRatio;
					if (bwRatio < minPosBw && bwRatio >= 0) minPosBw = bwRatio;

					kernelV = assets[1] * ParticleMethodUtils::kernel(bandwidth, bwIn, kernel);

					eNum[j] += (vol1*vol1 + vol2*vol2 + 2 * a*vol1*vol2 / b)*kernelV;
					eDen[j] += vol1*vol2*kernelV / b;
					eScale[j] += kernelV;
				}
			}

			for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
			{
				QL_REQUIRE(eScale[j] != 0, std::string("ParticleMethodUtils::calibrateFX: resulting bandwidth is too small for calibration (support: < ") + std::to_string(maxNegBw)
					+ std::string(" and > ") + std::to_string(minPosBw)
					+ std::string("). Either decrease number of MC paths or increase exponentN or kappa."));
				if (eNum[j] != eNum[j] || vol3 != vol3 || eScale[j] != eScale[j] || eDen[j] != eDen[j] || eDen[j] == 0)
					QL_FAIL("surface not well defined.");
				surfaceF[i][j] = (eNum[j] - vol3[j] * vol3[j]* eScale[j]) / (2 * eDen[j]);
			}
			//set interpolation on new dimension:
			surface->setInterpolationStrike<Linear>(i);
		}
	}

	void ParticleMethodUtils::calibrateIndex(Handle<LocalCorrSurfaceABFIndex>& surface, const std::string& kernelIn, unsigned int numberOfPaths, Time maxTime,
		Time deltaT, Time tMin, Real kappa, Real sigmaAVR, Real exponentN, Real gridMinQuantile,
		Real gridMaxQuantile, unsigned int ns1, unsigned int ns2) {

		boost::shared_ptr<KernelInterface> kernel;

		if (kernelIn == "QuarticKernel") {
			kernel = boost::shared_ptr<KernelInterface>(new QuarticKernel());
		}
		else {
			QL_REQUIRE(false, "Kernel not supported. Supported is: QuarticKernel");
		}

		std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>> processes = surface->getProcesses();
		boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>			    processToCal = surface->getProcessToCal();

		//build assetModel for simulation
		boost::shared_ptr<YieldTermStructure> yld(new FlatForward(processToCal->blackVolatility()->referenceDate(), 0, processToCal->blackVolatility()->dayCounter()));
		Handle<YieldTermStructure> yldH = Handle<YieldTermStructure>(yld);
		std::vector<std::string> aliases(2);
		aliases[0] = "index1";
		aliases[1] = "index2";
		Handle<LocalCorrTermStructure> surfaceGen(surface.currentLink());
		boost::shared_ptr<LocalCorrelationSLVModel> assetModel = boost::shared_ptr<LocalCorrelationSLVModel>((new LocalCorrelationSLVModel(yldH, aliases, processes, surfaceGen)));

		std::vector<Time>& times = surface->getTimes();
		std::vector<std::vector<Real>>& strikes = surface->getStrikes();
		std::vector<std::vector<Real>>& surfaceF = surface->getSurfaceF();

		//time grid from t=0 to maxTime:
		times.resize(1);
		times[0] = 0;
		size_t i = 0;
		while (times[i] < maxTime) {
			times.push_back(times[i] + deltaT);
			i++;
		}
		surface->setInterpolationTime<Linear>();

		RealMCSimulation simulation(assetModel, times, times, numberOfPaths, 1, true, true, false);
		simulation.prepareForSlicedSimulation();
		simulation.simulate(0, false);

		//create strike grid.
		//the strike grid depends on simulation results (min and max quantile)
		//the simulation itself depends on local correlation which itself is calibrated using this function.
		//Consequence: strike grid can merely be calculated iteratively during calibration

		size_t numberStrikes;
		Real strikeStep;
		surfaceF.resize(times.size());
		strikes.resize(times.size());
		std::vector<Real> state;
		std::vector<Real> assets(processes.size());
		std::vector<Real> indexVals(numberOfPaths);

		std::vector<Real> vol(processes.size());
		std::vector<Real> vol3;
		std::vector<Real> eNum;
		std::vector<Real> eDen;
		std::vector<Real> eScale;
		Real a = 0;
		Real b = 0;
		Real kernelV = 0;
		Real bandwidth = 0;
		Real minPosBw;
		Real maxNegBw;
		Real bwIn;
		Real bwRatio;
		double indexStart;

		//Calculate local correlation successively over time

		for (size_t i = 1; i < surfaceF.size() - 1; i++) //iteration over time, for i=0 nothing to do as correlation independent of a,b,f. In last entry, no additional simulation necessary.
		{
			numberStrikes = numberStrikeGrid(times[i], ns1, ns2);
			QL_REQUIRE(numberStrikes>1, "ns1 or ns2 has to be increased, strike grid cannot be calculated.");
			simulation.simulate(i, false);

			//Now strike grid can be calculated

			surfaceF[i].resize(numberStrikes);
			strikes[i].resize(numberStrikes);
			for (size_t k = 0; k < numberOfPaths; k++)
			{
				state = simulation.state(k, times[i]);
				indexVals[k] = surface->localFStrike(times[i],state);
			}

			std::sort(indexVals.begin(), indexVals.end());

			strikes[i][0] = indexVals[(size_t)(numberOfPaths*gridMinQuantile)];
			strikes[i][strikes[i].size() - 1] = indexVals[(size_t)(numberOfPaths*gridMaxQuantile)];

			strikeStep = (strikes[i][strikes[i].size() - 1] - strikes[i][0]) / (numberStrikes - 1);
			QL_ASSERT(strikeStep > 0, "Error. StrikeStep shouldn't be zero for time " << times[i] << ". Check volatility.");
			indexStart = surface->localFStrike(times[i], std::vector<Real>(processes.size(), 0));

			bandwidth = ParticleMethodUtils::bandwidth(times[i], abs(indexStart)>1 ? abs(indexStart) : 1, kappa, sigmaAVR, tMin, numberOfPaths, exponentN);

			for (size_t j = 1; j < strikes[i].size() - 1; j++)
			{
				strikes[i][j] = strikes[i][j - 1] + strikeStep;
			}

			//Particle method for that time step

			eNum.resize(strikes[i].size());
			eDen.resize(strikes[i].size());
			eScale.resize(strikes[i].size());
			vol3.resize(strikes[i].size());

			for (size_t j = 0; j < strikes[i].size(); j++)
			{
				eNum[j] = 0;
				eDen[j] = 0;
				eScale[j] = 0;
				if (surface->possibleNegativeIndex()) {
					vol3[j] = getLocalVolFromPriceFormula(processToCal,strikes[i][j],times[i],surface->getProcessToCalBlackVolShift());
				}
				else
					vol3[j] = processToCal->localVolatility()->localVol(times[i], strikes[i][j], true);
			}

			for (size_t k = 0; k < numberOfPaths; k++) //particle method: over MC paths
			{
				state = simulation.state(k, times[i]);

				for (size_t j = 0; j < assets.size(); j++)
				{
					assets[j] = processes[j]->s0()->value() * std::exp(state[j]);
					state[j+processes.size()] = state[j + processes.size()] < 0 ? 0.0001 : state[j + processes.size()];//heston vol might become negative (dep. on feller constant)
					vol[j] = processes[j]->leverageFct()->localVol(times[i], assets[j], true)*std::sqrt(state[j + processes.size()]); //*ai from Heston
					if (vol[j] != vol[j])
						QL_FAIL(std::string("surface of asset ") + std::to_string(j) + std::string(" not well defined"));
				}

				Real vol0 = surface->getIndexCovariance(LocalCorrSurfaceABFIndex::CTSIndexCovarianceType::CORR0, assets, vol);
				Real vol1 = surface->getIndexCovariance(LocalCorrSurfaceABFIndex::CTSIndexCovarianceType::CORR1, assets, vol);

				a = surface->localA(times[i], assets, true);
				b = surface->localB(times[i], assets, true);

				Real indexVal = surface->localFStrike(times[i], state);

				for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
				{

					minPosBw = 10000000;
					maxNegBw = -10000000;

					bwIn = indexVal - strikes[i][j];
					bwRatio = bwIn / bandwidth;

					if (bwRatio > maxNegBw && bwRatio <= 0) maxNegBw = bwRatio;
					if (bwRatio < minPosBw && bwRatio >= 0) minPosBw = bwRatio;

					kernelV = ParticleMethodUtils::kernel(bandwidth, bwIn, kernel);

					eNum[j] += (vol0 - a*(vol1-vol0) / b)*kernelV;
					eDen[j] += (vol1 - vol0)*kernelV / b;
					eScale[j] += kernelV;
				}
			}

			for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
			{
				QL_REQUIRE(eScale[j] != 0, std::string("ParticleMethodUtils::calibrateIndex: resulting bandwidth is too small for calibration (support: < ") + std::to_string(maxNegBw)
					+ std::string(" and > ") + std::to_string(minPosBw)
					+ std::string("). Either decrease number of MC paths or increase exponentN or kappa."));
				if (eNum[j] != eNum[j] || vol3[j] != vol3[j] || eScale[j] != eScale[j] || eDen[j] != eDen[j] || eDen[j] == 0)
					QL_FAIL("surface not well defined.");
				if (surface->possibleNegativeIndex()) {
					surfaceF[i][j] = (vol3[j] * eScale[j] - eNum[j]) / (eDen[j]);
				}
				else
					surfaceF[i][j] = ( vol3[j] * vol3[j] * strikes[i][j]* strikes[i][j] * eScale[j]- eNum[j]) / (eDen[j]);
			}
			//set interpolation on new dimension:
			surface->setInterpolationStrike<Linear>(i);
		}
	}

	Real ParticleMethodUtils::bandwidth(Time t, Real s0, Real kappa, Real sigmaAVR, Real tMin, unsigned int numberOfPaths, Real exponentN) {
		Real mult = pow(numberOfPaths, exponentN);
		return kappa*sigmaAVR* s0 * sqrt(t>tMin ? t : tMin) * mult;
	}

	Real ParticleMethodUtils::kernel(Real bandwidth, Real x, boost::shared_ptr<KernelInterface>& kernel) {
		QL_REQUIRE(bandwidth != 0, "Error in ParticleMethodUtils: bandwidth is not allowed to be zero.");
		return kernel->value(x / bandwidth) / bandwidth;
	}
	size_t ParticleMethodUtils::numberStrikeGrid(Time t, unsigned int ns1, unsigned int ns2) {
		Real numberOfStrikes = ns1*sqrt(t);
		return (numberOfStrikes > ns2) ? ((size_t)numberOfStrikes) : (ns2);
	}

	Real ParticleMethodUtils::getCrossFX(Real asset1, Real asset2) {
		return asset1 / asset2;
	}

	Real ParticleMethodUtils::getLocalVolFromPriceFormula(boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>& processToCal, Real strike, Time t, Real indShift) {
		double negIndVol;
		double negIndPriceT;
		double negIndPrice;
		double negIndPriceUp;
		double negIndPriceDn;
		double negIndTimeBmp = 0.0001;
		double negIndAssetBmp = 0.001;
		double negIndAsset;
		double dCdt;
		double dCdKdK;
		double dCdK;

		negIndVol = processToCal->blackVolatility()->blackVol(t, strike, true);
		negIndPrice = BlackScholesCalculator(Option::Type::Call, strike + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndVol = processToCal->blackVolatility()->blackVol(t + negIndTimeBmp, strike, true);
		negIndPriceT = BlackScholesCalculator(Option::Type::Call, strike + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t + negIndTimeBmp),
			negIndVol*sqrt(t + negIndTimeBmp), processToCal->riskFreeRate()->discount(t + negIndTimeBmp)).value();

		dCdt = (negIndPriceT - negIndPrice) / negIndTimeBmp;

		negIndAsset = strike>1 ? strike * (1 + negIndAssetBmp) : strike + negIndAssetBmp;
		negIndVol = processToCal->blackVolatility()->blackVol(t, negIndAsset, true);
		negIndPriceUp = BlackScholesCalculator(Option::Type::Call, negIndAsset + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndAsset = strike>1 ? strike * (1 - negIndAssetBmp) : strike - negIndAssetBmp;
		negIndVol = processToCal->blackVolatility()->blackVol(t, negIndAsset, true);
		negIndPriceDn = BlackScholesCalculator(Option::Type::Call, negIndAsset + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndAsset = strike>1 ? negIndAssetBmp*strike : negIndAssetBmp;
		dCdKdK = (negIndPriceUp - 2 * negIndPrice + negIndPriceDn) / (negIndAsset * negIndAsset);

		dCdK = (negIndPriceUp - negIndPriceDn) / (2 * negIndAsset);

		Real rate = processToCal->riskFreeRate()->zeroRate(t, Compounding::Continuous);
		Real div = processToCal->dividendYield()->zeroRate(t, Compounding::Continuous);

		QL_REQUIRE(abs(dCdKdK) > QL_EPSILON, "local vol for cross asset delivers NaN (t=" << t << ", strike=" << strike << ")");

		return 2 * (dCdt + strike * dCdK*(rate-div) + div*negIndPrice) / dCdKdK;

	}
}


#include <cmath>
#include <ctime>
#include <boost/serialization/export.hpp>
#include "NeuralNetwork.hpp"
#include "layer/LayerModel.hpp"
#include "layer/LayerFactory.hpp"
#include "optimizer/NeuralNetworkOptimizerFactory.hpp"

using namespace std;
using namespace snn;
using namespace internal;

BOOST_CLASS_EXPORT(NeuralNetwork)

bool NeuralNetwork::isTheFirst = true;

void NeuralNetwork::initialize()
{
    srand(static_cast<unsigned>(time(nullptr)));
    rand();
    ActivationFunction::initialize();
    isTheFirst = false;
}

NeuralNetwork::NeuralNetwork(vector<LayerModel>& architecture, NeuralNetworkOptimizerModel optimizer)
{
    if (isTheFirst)
        this->initialize();
    this->optimizer = NeuralNetworkOptimizerFactory::build(optimizer);
    LayerFactory::build(this->layers, architecture, this->optimizer);
    this->StatisticAnalysis::initialize(this->layers.back()->getNumberOfNeurons());
}

NeuralNetwork::NeuralNetwork(const NeuralNetwork& neuralNetwork)
    : StatisticAnalysis(neuralNetwork),
      optimizer(neuralNetwork.optimizer->clone())
{
    this->layers.reserve(neuralNetwork.layers.size());
    for (const auto& layer : neuralNetwork.layers)
        this->layers.push_back(layer->clone(this->optimizer));
    this->StatisticAnalysis::initialize(this->layers.back()->getNumberOfNeurons());
}

void NeuralNetwork::evaluateOnceForRegression(
    const vector<float>& inputs, const vector<float>& desired, const float precision, bool temporalReset)
{
    const auto outputs = this->output(inputs, temporalReset);
    this->StatisticAnalysis::evaluateOnceForRegression(outputs, desired, precision);
}

void NeuralNetwork::evaluateOnceForMultipleClassification(
    const vector<float>& inputs, const vector<float>& desired, const float separator, bool temporalReset)
{
    const auto outputs = this->output(inputs, temporalReset);
    this->StatisticAnalysis::evaluateOnceForMultipleClassification(outputs, desired, separator);
}

void NeuralNetwork::evaluateOnceForClassification(const vector<float>& inputs, const int classNumber, const float separator,
                                                  bool temporalReset)
{
    const auto outputs = this->output(inputs, temporalReset);
    this->StatisticAnalysis::evaluateOnceForClassification(outputs, classNumber, separator);
}

void NeuralNetwork::trainOnce(const vector<float>& inputs, const vector<float>& desired, bool temporalReset)
{
    this->backpropagationAlgorithm(inputs, desired, temporalReset);
}

vector<float> NeuralNetwork::output(const vector<float>& inputs, bool temporalReset)
{
    auto outputs = layers[0]->output(inputs, temporalReset);

    for (size_t l = 1; l < this->layers.size(); ++l)
    {
        outputs = layers[l]->output(outputs, temporalReset);
    }

    if (ranges::any_of(outputs, [](const float& v) { return fpclassify(v) != FP_NORMAL && fpclassify(v) != FP_ZERO; }))
        this->outputNan = true;

    return outputs;
}

std::vector<float> NeuralNetwork::outputForTraining(const std::vector<float>& inputs, bool temporalReset)
{
    auto outputs = layers[0]->outputForTraining(inputs, temporalReset);

    for (size_t l = 1; l < this->layers.size(); ++l)
    {
        outputs = layers[l]->outputForTraining(outputs, temporalReset);
    }

    if (ranges::any_of(outputs, [](const float& v) { return fpclassify(v) != FP_NORMAL && fpclassify(v) != FP_ZERO; }))
        this->outputNan = true;

    return outputs;
}

void NeuralNetwork::backpropagationAlgorithm(const vector<float>& inputs, const vector<float>& desired,
                                             bool temporalReset)
{
    const auto outputs = this->outputForTraining(inputs, temporalReset);
    if (this->outputNan)
        return;
    auto errors = calculateError(outputs, desired);

    for (size_t l = this->layers.size() - 1; l > 0; --l)
    {
        errors = layers[l]->backOutput(errors);
    }
    layers[0]->train(errors);
}

inline
vector<float> NeuralNetwork::calculateError(const vector<float>& outputs, const vector<float>& desired) const
{
    vector<float> errors(this->layers.back()->getNumberOfNeurons(), 0);
    for (size_t n = 0; n < errors.size(); ++n)
    {
        if (isnan(desired[n]))
            errors[n] = 0;
        else
        {
            errors[n] = 2 * (desired[n] - outputs[n]);
        }
    }
    return errors;
}

vector<vector<float>> NeuralNetwork::getLayerOutputs(const vector<float>& inputs)
{
    vector<vector<float>> filterOutputs;
    auto outputs = layers[0]->output(inputs, true);
    filterOutputs.push_back(outputs);

    for (size_t l = 1; l < this->layers.size(); ++l)
    {
        outputs = layers[l]->output(outputs, true);
        filterOutputs.push_back(outputs);
    }
    return filterOutputs;
}

bool NeuralNetwork::hasNan() const
{
    return this->outputNan;
}

int NeuralNetwork::getNumberOfLayers() const
{
    return (int)this->layers.size();
}

int NeuralNetwork::getNumberOfInputs() const
{
    return this->layers[0]->getNumberOfInputs();
}

int NeuralNetwork::getNumberOfOutputs() const
{
    return this->layers.back()->getNumberOfNeurons();
}

int NeuralNetwork::getNumberOfNeurons() const
{
    int sum = 0;
    for (auto& layer : this->layers)
    {
        sum += layer->getNumberOfNeurons();
    }
    return sum;
}

int NeuralNetwork::getNumberOfParameters() const
{
    int sum = 0;
    for (const auto& layer : this->layers)
    {
        sum += layer->getNumberOfParameters();
    }
    return sum;
}

int NeuralNetwork::isValid() const
{
    //TODO: rework isValid
    if (this->getNumberOfInputs() < 1
    || this->getNumberOfInputs() > 2073600) // 1920 * 1080
        return 101;

    if (this->getNumberOfLayers() < 1
    || this->getNumberOfLayers() > 1000)
        return 102;

    int err = this->optimizer->isValid();
    if (err != 0)
        return err;

    for (const auto& layer : this->layers)
    {
        err = layer->isValid();
        if (err != 0)
            return err;
    }
    return 0;
}

bool NeuralNetwork::operator==(const NeuralNetwork& neuralNetwork) const
{
    return *this->optimizer == *neuralNetwork.optimizer
        && this->layers.size() == neuralNetwork.layers.size()
        && [this, &neuralNetwork]()
        {
            for (size_t l = 0; l < this->layers.size(); ++l)
            {
                if (*this->layers[l] != *neuralNetwork.layers[l])
                    return false;
            }
            return true;
        }();
}

bool NeuralNetwork::operator!=(const NeuralNetwork& neuralNetwork) const
{
    return !(*this == neuralNetwork);
}


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

  Copyright 2015 NumScale SAS
  Copyright 2015 J.T. Lapreste

  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/function/scalar/sincosd.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/function/sind.hpp>
#include <boost/simd/function/cosd.hpp>

STF_CASE_TPL (" sincosd",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  using bs::sincosd;
  T a[] = {bs::Zero<T>(), bs::One<T>(), T(120), T(180),
           T(90), bs::Inf<T>(), bs::Minf<T>(), bs::Nan<T>()};
  size_t N =  sizeof(a)/sizeof(T);

  STF_EXPR_IS( (sincosd(T()))
             , (std::pair<T,T>)
             );

  {
    T s, c;
    for(size_t i=0; i < N; ++i)
    {
      sincosd(a[i], s, c);
      STF_IEEE_EQUAL(s, bs::sind(a[i]));
      STF_IEEE_EQUAL(c, bs::cosd(a[i]));
    }
  }

  {
    T s, c;
    for(size_t i=0; i < N; ++i)
    {
      s = sincosd(a[i], c);
      STF_IEEE_EQUAL(s, bs::sind(a[i]));
      STF_IEEE_EQUAL(c, bs::cosd(a[i]));
    }
  }

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<T,T> p = sincosd(a[i]);
      STF_IEEE_EQUAL(p.first,  bs::sind(a[i]));
      STF_IEEE_EQUAL(p.second, bs::cosd(a[i]));
    }
  }

}


#pragma once

#include <metaSMT/frontend/Logic.hpp>

#include <boost/multi_array.hpp>

using namespace metaSMT::logic;

struct Level {

  std::vector<predicate> vars;

  Level(unsigned size)
  {
    for (unsigned i = 0; i < size; i++) {
      vars.push_back( new_variable() );
    }
  }

  predicate & operator[] (unsigned i) {
    return vars[i];
  }

  template <typename Context>
  typename Context::result_type
  is_sorted(Context & ctx) {
    //std::cout << "is_sorted called\n";
    typename Context::result_type ret = evaluate(ctx, True);
    for (unsigned i = 1; i < vars.size(); i++) {
      ret = evaluate(ctx, And(ret,
          implies(vars[i-1], vars[i])
      ));
    }
    return ret;
  }

  template <typename Context>
  typename Context::result_type
  equal_to(Context & ctx, std::string const & val) {
    typename Context::result_type ret = evaluate(ctx, True);
    for (unsigned i = 0; i < vars.size(); i++) {
      if(val[i] == '1') {
        ret = evaluate(ctx, And(ret,
              vars[i]
              ));
      } else {
        ret = evaluate(ctx, And(ret,
              Not(vars[i])
              ));
      }
    }
    return ret;
  }

  template <typename Context>
  void print_assignment(std::ostream& out, Context & ctx) {
    for (unsigned i = 0; i < vars.size(); i++) {
      bool b = read_value(ctx, vars[i]);
      out << b;
    }
    out << '\n';
  }

  template <typename Context>
  void print(std::ostream& out, Context & ctx) {
    print_assignment(out, ctx);
  }

  template <typename Context>
  std::string assignment(Context & ctx) {
    std::string ret(vars.size(), '-');
    for (unsigned i = 0; i < vars.size(); i++) {
      bool b = read_value(ctx, vars[i]);
      ret[i] = b ? '1' : '0';
    }
    return ret;
  }
};