/*
 * Copyright (c) 2012, Daniel Claes, Maastricht 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 Maastricht 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.
 */

#include "collvoid_local_planner/clearpath.h"
#include <ros/ros.h>
#include <float.h>
#include <boost/foreach.hpp>

namespace collvoid {

    // VO createObstacleVO(Vector2& position1, const std::vector<Vector2>& footprint1,Vector2& vel1,  Vector2& position2, const std::vector<Vector2>& footprint2){
    //   VO result;
    //   std::vector<Vector2> mink_sum = minkowskiSum(footprint1, footprint2);

    //   Vector2 rel_position = position2 - position1;

    //   Vector2 rel_position_normal = normal(rel_position);
    //   double min_dist = abs(rel_position);

    //   for (int i = 0; i< (int) mink_sum.size(); i++){
    //     Vector2 project_on_rel_position = intersectTwoLines(Vector2(0.0, 0.0), rel_position, rel_position + mink_sum[i], rel_position_normal);
    //     double dist = abs(project_on_rel_position);
    //     if (project_on_rel_position * rel_position < EPSILON){
    // 	//	ROS_ERROR("Collision?");
    // 	dist = -dist;

    //     }

    //     if (dist < min_dist) {
    // 	min_dist = dist;
    //     }
    //   }
    //   result.left_leg_dir = - normalize(rel_position_normal);
    //   result.right_leg_dir = - result.left_leg_dir;
    //   result.relative_position = rel_position;
    //   result.combined_radius = abs(rel_position) - min_dist;

    //   result.point = normalize(rel_position) * min_dist; // * std::max(1 - abs(vel1), 0.0);
    //   result.trunc_left = result.point;
    //   result.trunc_right = result.point;
    //   result.trunc_line_center = result.point;

    //   return result;

    // }

    VO createObstacleVO(Vector2 &position1, double radius1, const std::vector<Vector2> &footprint1, Vector2 &obst1,
                        Vector2 &obst2) {
        VO result;

        Vector2 position_obst = 0.5 * (obst1 + obst2);

        std::vector<Vector2> obst;
        obst.push_back(obst1 - position_obst);
        obst.push_back(obst2 - position_obst);

        std::vector<Vector2> mink_sum = minkowskiSum(footprint1, obst);

        Vector2 min_left, min_right;
        double min_ang = 0.0;
        double max_ang = 0.0;
        Vector2 rel_position = position_obst - position1;

        Vector2 rel_position_normal = normal(rel_position);
        double min_dist = abs(rel_position);

        for (int i = 0; i < (int) mink_sum.size(); i++) {
            double angle = angleBetween(rel_position, rel_position + mink_sum[i]);
            if (rightOf(Vector2(0.0, 0.0), rel_position, rel_position + mink_sum[i])) {
                if (-angle < min_ang) {
                    min_right = rel_position + mink_sum[i];
                    min_ang = -angle;
                }
            }
            else {
                if (angle > max_ang) {
                    min_left = rel_position + mink_sum[i];
                    max_ang = angle;
                }
            }
            Vector2 project_on_rel_position = intersectTwoLines(Vector2(0.0, 0.0), rel_position,
                                                                rel_position + mink_sum[i], rel_position_normal);
            double dist = abs(project_on_rel_position);
            if (project_on_rel_position * rel_position < -EPSILON) {
                //	ROS_ERROR("Collision?");
                dist = -dist;

            }

            if (dist < min_dist) {
                min_dist = dist;
            }
        }
        if (min_dist < 0) {
            result.left_leg_dir = -normalize(obst1 - obst2);
            result.right_leg_dir = -result.left_leg_dir;

            result.point = rel_position - 1.5 * radius1 * normal(result.left_leg_dir);
            result.trunc_left = result.point;
            result.trunc_right = result.point;
            return result;

        }

        double ang_rel = atan2(rel_position.y(), rel_position.x());
        result.left_leg_dir = Vector2(cos(ang_rel + max_ang), sin(ang_rel + max_ang));
        result.right_leg_dir = Vector2(cos(ang_rel + min_ang), sin(ang_rel + min_ang));

        result.left_leg_dir = rotateVectorByAngle(result.left_leg_dir, 0.15);
        result.right_leg_dir = rotateVectorByAngle(result.right_leg_dir, -0.05);

        //double ang_between = angleBetween(result.right_leg_dir, result.left_leg_dir);
        //double opening_ang = ang_rel + min_ang + (ang_between) / 2.0;

        // Vector2 dir_center = Vector2(cos(opening_ang), sin(opening_ang));
        // min_dist = abs(rel_position);
        // Vector2 min_point = rel_position;
        // for(int i = 0; i< (int)mink_sum.size(); i++) {
        //   Vector2 proj_on_center = intersectTwoLines(Vector2(0.0,0.0), dir_center, rel_position+mink_sum[i], normal(dir_center));
        //   if (abs(proj_on_center) < min_dist) {
        // 	min_dist = abs(proj_on_center);
        // 	min_point = rel_position+mink_sum[i];
        //   }

        // }

        //result.left_leg_dir = rotateVectorByAngle(normalize(min), 0.1);
        //result.right_leg_dir = rotateVectorByAngle(normalize(max), -0.1);

        result.relative_position = rel_position;
        result.combined_radius = abs(rel_position) - min_dist;
        result.point = Vector2(0, 0);

        result.trunc_left = intersectTwoLines(result.point, result.left_leg_dir,
                                              result.combined_radius / 2.0 * normalize(rel_position), obst2 - obst1);
        result.trunc_right = intersectTwoLines(result.point, result.right_leg_dir,
                                               result.combined_radius / 2.0 * normalize(rel_position), obst2 - obst1);

        //result = createTruncVO(result, 6.0);
        return result;

    }

    // VO createObstacleVO(Vector2& position1, double radius1,Vector2& vel1,  Vector2& position2, double radius2){
    //   VO result;

    //   Vector2 rel_position = position2 - position1;

    //   Vector2 rel_position_normal = normal(rel_position);

    //   result.left_leg_dir = - normalize(rel_position_normal);
    //   result.right_leg_dir = - result.left_leg_dir;
    //   result.relative_position = rel_position;
    //   result.combined_radius = radius1 + radius2;

    //   result.point = normalize(rel_position) * (abs(rel_position) - result.combined_radius); // * std::max(1 - abs(vel1), 0.0);
    //   result.trunc_left = result.point;
    //   result.trunc_right = result.point;
    //   result.trunc_line_center = result.point;

    //   return result;

    // }

    VO createVO(Vector2 &position1, const std::vector<Vector2> &footprint1, Vector2 &position2,
                const std::vector<Vector2> &footprint2, Vector2 &vel2) {
        VO result;
        std::vector<Vector2> mink_sum = minkowskiSum(footprint1, footprint2);

        Vector2 min_left, min_right;
        double min_ang = 0.0;
        double max_ang = 0.0;
        Vector2 rel_position = position2 - position1;

        Vector2 rel_position_normal = normal(rel_position);
        double min_dist = abs(rel_position);

        for (int i = 0; i < (int) mink_sum.size(); i++) {
            double angle = angleBetween(rel_position, rel_position + mink_sum[i]);
            if (rightOf(Vector2(0.0, 0.0), rel_position, rel_position + mink_sum[i])) {
                if (-angle < min_ang) {
                    min_right = rel_position + mink_sum[i];
                    min_ang = -angle;
                }
            }
            else {
                if (angle > max_ang) {
                    min_left = rel_position + mink_sum[i];
                    max_ang = angle;
                }
            }
            Vector2 project_on_rel_position = intersectTwoLines(Vector2(0.0, 0.0), rel_position,
                                                                rel_position + mink_sum[i], rel_position_normal);
            double dist = abs(project_on_rel_position);
            if (project_on_rel_position * rel_position < -EPSILON) {
                //	ROS_ERROR("Collision?");
                dist = -dist;

            }

            if (dist < min_dist) {
                min_dist = dist;
            }
        }
        if (min_dist < 0) {
            result.left_leg_dir = -normalize(rel_position_normal);
            result.right_leg_dir = -result.left_leg_dir;
            result.relative_position = rel_position;
            result.combined_radius = abs(rel_position) - min_dist;
            result.point = vel2;
            return result;
        }

        double ang_rel = atan2(rel_position.y(), rel_position.x());
        result.left_leg_dir = Vector2(cos(ang_rel + max_ang), sin(ang_rel + max_ang));
        result.right_leg_dir = Vector2(cos(ang_rel + min_ang), sin(ang_rel + min_ang));

        result.left_leg_dir = rotateVectorByAngle(result.left_leg_dir, 0.15);
        result.right_leg_dir = rotateVectorByAngle(result.right_leg_dir, -0.05);

        double ang_between = angleBetween(result.right_leg_dir, result.left_leg_dir);
        double opening_ang = ang_rel + min_ang + (ang_between) / 2.0;

        Vector2 dir_center = Vector2(cos(opening_ang), sin(opening_ang));
        min_dist = abs(rel_position);
        Vector2 min_point = rel_position;
        for (int i = 0; i < (int) mink_sum.size(); i++) {
            Vector2 proj_on_center = intersectTwoLines(Vector2(0.0, 0.0), dir_center, rel_position + mink_sum[i],
                                                       normal(dir_center));
            if (abs(proj_on_center) < min_dist) {
                min_dist = abs(proj_on_center);
                min_point = rel_position + mink_sum[i];
            }

        }

        //ROS_ERROR("min_right %f, %f, min_left %f,%f,", min_right.x(), min_right.y(), min_left.x(), min_left.y());
        double center_p, radius;
        Vector2 center_r;
        //test left/right
        if (abs(min_left) < abs(min_right)) {
            center_p = abs(min_left) / cos(ang_between / 2.0);
            radius = tan(ang_between / 2.0) * abs(min_left);
            center_r = center_p * dir_center;
        }
        else {
            center_p = abs(min_right) / cos(ang_between / 2.0);
            center_r = center_p * dir_center;
            radius = tan(ang_between / 2.0) * abs(min_right);
        }
        //check min_point, if failed stupid calc for new radius and point;
        if (abs(min_point - center_r) > radius) {
            double gamma = min_point.x() * dir_center.x() + min_point.y() * dir_center.y();
            double sqrt_exp = absSqr(min_point) / (sqr(sin(ang_between / 2.0)) - 1) +
                              sqr(gamma / (sqr(sin(ang_between / 2.0)) - 1));
            if (fabs(sqrt_exp) < EPSILON) {
                sqrt_exp = 0;
            }
            if (sqrt_exp >= 0) {
                center_p = -gamma / (sqr(sin(ang_between / 2.0)) - 1) + std::sqrt(sqrt_exp);
                center_r = center_p * dir_center;
                radius = abs(min_point - center_r);
            }
            else {
                ROS_ERROR("ang = %f, sqrt_ext = %f", ang_between, sqrt_exp);
                ROS_ERROR("rel position %f, %f, radius %f, center_p %f", rel_position.x(), rel_position.y(), radius,
                          center_p);
            }
        }
        //result.relative_position = rel_position;
        //result.combined_radius = abs(rel_position) - min_dist;

        result.relative_position = center_r;
        result.combined_radius = radius;
        result.point = vel2;
        return result;

    }

    VO createRVO(Vector2 &position1, const std::vector<Vector2> &footprint1, Vector2 &vel1, Vector2 &position2,
                 const std::vector<Vector2> &footprint2, Vector2 &vel2) {
        VO result = createVO(position1, footprint1, position2, footprint2, vel2);
        result.point = 0.5 * (vel1 + vel2); //TODO add uncertainty
        return result;

    }

    VO createHRVO(Vector2 &position1, const std::vector<Vector2> &footprint1, Vector2 &vel1, Vector2 &position2,
                  const std::vector<Vector2> &footprint2, Vector2 &vel2) {
        //std::vector<Vector2> mink_sum = minkowskiSum(footprint1, footprint2);
        //std::vector<Vector2> empty;
        VO result = createRVO(position1, footprint1, vel1, position2, footprint2, vel2);

        Vector2 rel_velocity = vel1 - vel2;
        // Vector2 rel_position = position2 - position1;

        // //Test if origin is inside mink_sum (Then we are in collision)!!!)
        // bool inside = true;
        // for (int i = 0; i<(int)mink_sum.size(); i++){
        //   int j = i+1;
        //   if (j == (int) mink_sum.size())
        // 	j = 0;
        //   if (leftOf(rel_position + mink_sum[i], mink_sum[j]-mink_sum[i], Vector2(0.0,0.0))) {
        // 	inside = false;

        // 	break;
        //   }
        // }
        // Vector2 new_position = position1;
        // int max_tries = 0;
        // while (result.combined_radius > abs(result.relative_position) && max_tries > 0) {
        //   //      ROS_ERROR("inside minkowskiSum!!");
        //   new_position = new_position - normalize(result.relative_position) * 0.1;
        //   //result = createRVO(new_position, footprint1,vel1, position2, footprint2, vel2);
        //   max_tries--;
        // }
        if (result.combined_radius > abs(result.relative_position)) {
            //ROS_ERROR("comb.rad. %f, relPos %f, %f (abs = %f)", result.combined_radius, result.relative_position.x(), result.relative_position.y(), abs(result.relative_position));
            //result.point = 0.5 * (vel2 + vel1) -  ((result.combined_radius - abs(result.relative_position)) / (2.0f * 0.1)) * normalize(result.relative_position);
            return result;

        }

        if (leftOf(Vector2(0.0, 0.0), result.relative_position, rel_velocity)) { //left of centerline
            result.point = intersectTwoLines(result.point, result.left_leg_dir, vel2,
                                             result.right_leg_dir); // TODO add uncertainty

        }
        else { //right of centerline
            result.point = intersectTwoLines(vel2, result.left_leg_dir, result.point,
                                             result.right_leg_dir); // TODO add uncertainty
        }

        return result;

    }

    VO createVO(Vector2 &position1, const std::vector<Vector2> &footprint1, Vector2 &vel1, Vector2 &position2,
                const std::vector<Vector2> &footprint2, Vector2 &vel2, int TYPE) {
        if (TYPE == HRVOS) {
            return createHRVO(position1, footprint1, vel1, position2, footprint2, vel2);
        }
        else if (TYPE == RVOS) {
            return createRVO(position1, footprint1, vel1, position2, footprint2, vel2);
        }
        else {
            return createVO(position1, footprint1, position2, footprint2, vel2);
        }
    }

    VO createVO(Vector2 &position1, double radius1, Vector2 &vel1, Vector2 &position2, double radius2, Vector2 &vel2,
                int TYPE) {
        if (TYPE == HRVOS) {
            return createHRVO(position1, radius1, vel1, position2, radius2, vel2);
        }
        else if (TYPE == RVOS) {
            return createRVO(position1, radius1, vel1, position2, radius2, vel2);
        }
        else {
            return createVO(position1, radius1, position2, radius2, vel2);
        }
    }

    VO createVO(Vector2 &position1, double radius1, Vector2 &position2, double radius2, Vector2 &vel2) {
        VO result;
        Vector2 rel_position = position2 - position1;
        double ang_to_other = atan(rel_position);
        double combined_radius = radius2 + radius1;
        double angle_of_opening;
        if (abs(rel_position) < combined_radius) {
            // angle_of_opening = M_PI/2.0-EPSILON;
            result.left_leg_dir = -normalize(normal(rel_position));
            result.right_leg_dir = -result.left_leg_dir;
            // result.right_leg_dir = Vector2(std::cos(ang_to_other - angle_of_opening), std::sin(ang_to_other - angle_of_opening));
            // result.left_leg_dir = Vector2(std::cos(ang_to_other + angle_of_opening), std::sin(ang_to_other + angle_of_opening));
        }
        else {
            angle_of_opening = std::asin(combined_radius / abs(rel_position));
            result.right_leg_dir = Vector2(std::cos(ang_to_other - angle_of_opening),
                                           std::sin(ang_to_other - angle_of_opening));
            result.left_leg_dir = Vector2(std::cos(ang_to_other + angle_of_opening),
                                          std::sin(ang_to_other + angle_of_opening));
        }
        //    ROS_ERROR("angle_of_opening %f, combined_radius %f, rel_position %f", angle_of_opening, combined_radius, abs(rel_position));
        result.point = vel2;
        result.relative_position = rel_position;
        result.combined_radius = radius1 + radius2;

        return result;
    }

    VO createRVO(Vector2 &position1, double radius1, Vector2 &vel1, Vector2 &position2, double radius2, Vector2 &vel2) {
        VO result = createVO(position1, radius1, position2, radius2, vel2);
        result.point = 0.5 * (vel1 + vel2); //TODO add uncertainty
        return result;
    }

    VO createHRVO(Vector2 &position1, double radius1, Vector2 &vel1, Vector2 &position2, double radius2,
                  Vector2 &vel2) {

        VO result = createRVO(position1, radius1, vel1, position2, radius2, vel2);

        Vector2 rel_velocity = vel1 - vel2;
        Vector2 rel_position = position2 - position1;
        if (abs(rel_position) < radius1 + radius2) {
            result.point = 0.5 * (vel2 + vel1);
            return result;
        }

        if (leftOf(Vector2(0.0, 0.0), rel_position, rel_velocity)) { //left of centerline
            result.point = intersectTwoLines(result.point, result.left_leg_dir, vel2,
                                             result.right_leg_dir); // TODO add uncertainty
        }
        else { //right of centerline
            result.point = intersectTwoLines(vel2, result.left_leg_dir, result.point,
                                             result.right_leg_dir); // TODO add uncertainty
        }
        return result;

    }

    VO createTruncVO(VO &vo, double time) {
        VO result;
        result.point = vo.point;
        result.left_leg_dir = vo.left_leg_dir;
        result.right_leg_dir = vo.right_leg_dir;
        result.relative_position = vo.relative_position;
        result.combined_radius = vo.combined_radius;
        double trunc_radius = vo.combined_radius / time;
        double angle_of_opening;

        if (abs(vo.relative_position) < vo.combined_radius) {
            result.trunc_left = result.point;
            result.trunc_right = result.point;
            result.trunc_line_center = result.point;
            return result;
        }
        else {
            angle_of_opening = std::asin(vo.combined_radius / abs(vo.relative_position));
            double trunc_dist = trunc_radius / std::sin(angle_of_opening) - trunc_radius;
            result.trunc_line_center = normalize(vo.relative_position) * trunc_dist;
            Vector2 intersectLeft = intersectTwoLines(result.point + result.trunc_line_center,
                                                      Vector2(result.trunc_line_center.y(),
                                                              -result.trunc_line_center.x()), result.point,
                                                      result.left_leg_dir);
            result.trunc_left = intersectLeft;
            result.trunc_right = intersectTwoLines(result.point + result.trunc_line_center,
                                                   Vector2(result.trunc_line_center.y(), -result.trunc_line_center.x()),
                                                   result.point, result.right_leg_dir);

            return result;
        }
    }

    bool isInsideVO(VO vo, Vector2 point, bool use_truncation) {
        bool trunc = leftOf(vo.trunc_left, vo.trunc_right - vo.trunc_left, point);
        if (abs(vo.trunc_left - vo.trunc_right) < EPSILON)
            trunc = true;
        return rightOf(vo.point, vo.left_leg_dir, point) && leftOf(vo.point, vo.right_leg_dir, point) &&
               (!use_truncation || trunc);
    }

    bool isWithinAdditionalConstraints(const std::vector<Line> &additional_constraints, const Vector2 &point) {
        BOOST_FOREACH(Line line, additional_constraints) {
                        if (rightOf(line.point, line.dir, point)) {
                            return false;
                        }
                    }
        return true;
    }

    void addCircleLineIntersections(std::vector<VelocitySample> &samples, const Vector2 &pref_vel, double maxSpeed,
                                    bool use_truncation, const Vector2 &point, const Vector2 &dir) {

        double discriminant = sqr(maxSpeed) - sqr(det(point,
                                                      dir)); // http://stackoverflow.com/questions/1073336/circle-line-collision-detection
        if (discriminant > 0.0f) //intersection with line
        {
            double t1 = -(point * dir) + std::sqrt(discriminant); //first solution
            double t2 = -(point * dir) - std::sqrt(discriminant); //second solution
            //ROS_ERROR("Adding circle line dist %f, %f", t1, t2);
            Vector2 point1 = point + t1 * dir;
            Vector2 point2 = point + t2 * dir;

            if (t1 >= 0.0f) {
                VelocitySample intersection_point;
                intersection_point.velocity = point1;
                intersection_point.dist_to_pref_vel = absSqr(pref_vel - intersection_point.velocity);
                samples.push_back(intersection_point);
            }
            if (t2 >= 0.0f) {
                VelocitySample intersection_point;
                intersection_point.velocity = point2;
                intersection_point.dist_to_pref_vel = absSqr(pref_vel - intersection_point.velocity);
                samples.push_back(intersection_point);
            }
        }
    }

    void addRayVelocitySamples(std::vector<VelocitySample> &samples, const Vector2 &pref_vel, Vector2 point1,
                               Vector2 dir1, Vector2 point2, Vector2 dir2, double max_speed, int TYPE) {
        double r, s;

        double x1, x2, x3, x4, y1, y2, y3, y4;
        x1 = point1.x();
        y1 = point1.y();
        x2 = x1 + dir1.x();
        y2 = y1 + dir1.y();
        x3 = point2.x();
        y3 = point2.y();
        x4 = x3 + dir2.x();
        y4 = y3 + dir2.y();

        double det = (((x2 - x1) * (y4 - y3)) - (y2 - y1) * (x4 - x3));

        if (det == 0.0) {
            //ROS_WARN("No Intersection found");
            return;
        }
        if (det != 0) {
            r = (((y1 - y3) * (x4 - x3)) - (x1 - x3) * (y4 - y3)) / det;
            s = (((y1 - y3) * (x2 - x1)) - (x1 - x3) * (y2 - y1)) / det;

            if ((TYPE == LINELINE) || (TYPE == RAYLINE && r >= 0) || (TYPE == SEGMENTLINE && r >= 0 && r <= 1) ||
                (TYPE == RAYRAY && r >= 0 && s >= 0) ||
                (TYPE == RAYSEGMENT && r >= 0 && s >= 0 && s <= 1) ||
                (TYPE == SEGMENTSEGMENT && r >= 0 && s >= 0 && r <= 1 && s <= 1)) {

                VelocitySample intersection_point;
                intersection_point.velocity = Vector2(x1 + r * (x2 - x1), y1 + r * (y2 - y1));
                intersection_point.dist_to_pref_vel = absSqr(pref_vel - intersection_point.velocity);
                if (absSqr(intersection_point.velocity) < sqr(1.2 * max_speed)) {
                    //ROS_ERROR("adding VelocitySample");
                    samples.push_back(intersection_point);
                    //ROS_ERROR("size of VelocitySamples %d", (int) samples->size());

                }
            }
        }
    }

    void createSamplesWithinMovementConstraints(std::vector<VelocitySample> &samples, double cur_vel_x,
                                                double cur_vel_y, double cur_vel_theta, double acc_lim_x,
                                                double acc_lim_y, double acc_lim_theta, double min_vel_x,
                                                double max_vel_x, double min_vel_y, double max_vel_y,
                                                double min_vel_theta, double max_vel_theta, double heading,
                                                Vector2 pref_vel, double sim_period, int num_samples, bool holo_robot) {

        if (holo_robot) {//holonomic drive
            double min_x, max_x, min_y, max_y;

            min_x = std::max(-max_vel_x, cur_vel_x - acc_lim_x * sim_period);
            max_x = std::min(max_vel_x, cur_vel_x + acc_lim_x * sim_period);

            min_y = std::max(-max_vel_y, cur_vel_y - acc_lim_y * sim_period);
            max_y = std::min(max_vel_y, cur_vel_y + acc_lim_y * sim_period);

            double step_x, step_y;

            int num_samples_per_dir = (int) std::sqrt(num_samples);

            step_x = (max_x - min_x) / num_samples_per_dir;
            step_y = (max_y - min_y) / num_samples_per_dir;

            for (int i = 0; i < num_samples_per_dir; i++) {
                for (int j = 0; j < num_samples_per_dir; j++) {
                    VelocitySample p;
                    Vector2 vel = Vector2(min_x + i * step_x, min_y + j * step_y);
                    p.dist_to_pref_vel = absSqr(vel - Vector2(cur_vel_x, cur_vel_y));
                    p.velocity = rotateVectorByAngle(Vector2(min_x + i * step_x, min_y + j * step_y), heading);
                    samples.push_back(p);
                }

            }

        }
        else {
            double min_x, max_x, min_theta, max_theta;

            min_x = -0.3;//std::max(-0.2, cur_vel_x - acc_lim_x * sim_period);
            max_x = std::min(max_vel_x, cur_vel_x + acc_lim_x * sim_period);

            //cur_vel_theta = 0.0; //HACK

            min_theta = -2 * max_vel_theta;//std::max(-max_vel_theta, cur_vel_theta - acc_lim_theta * sim_period);
            max_theta = 2 * max_vel_theta;//std::min(max_vel_theta, cur_vel_theta + acc_lim_theta * sim_period);

            double step_x, step_theta;

            int num_samples_per_dir = (int) std::sqrt(num_samples);

            step_x = (max_x - min_x) / num_samples_per_dir;
            step_theta = (max_theta - min_theta) / num_samples_per_dir;

            // ROS_ERROR("heading %f, min_theta %f, %f cur_vel_theta %f", heading, min_theta, max_theta, cur_vel_theta);

            for (int i = 0; i < num_samples_per_dir; i++) {
                for (int j = 0; j < num_samples_per_dir; j++) {
                    VelocitySample p;
                    double th_dif = min_theta + j * step_theta;
                    double x_dif = (min_x + i * step_x) * cos(heading + th_dif / 2.0);
                    double y_dif = (min_x + i * step_x) * sin(heading + th_dif / 2.0);
                    p.dist_to_pref_vel = absSqr(Vector2(cur_vel_x, cur_vel_y));

                    p.velocity = Vector2(x_dif, y_dif);
                    //p.velocity = rotateVectorByAngle(Vector2(min_x + i* step_x, 0), heading + min_theta + j * step_theta);

                    samples.push_back(p);
                }

            }
        }

    }

    Vector2 calculateNewVelocitySampled(std::vector<VelocitySample> &samples, const std::vector<VO> &truncated_vos,
                                        const Vector2 &pref_vel, double max_speed, bool use_truncation) {

        // VelocitySample pref_vel_sample;
        // pref_vel_sample.velocity = pref_vel;
        // samples.push_back(pref_vel_sample);

        // VelocitySample null_vel;
        // null_vel.velocity = Vector2(0,0);
        // null_vel.dist_to_pref_vel = absSqr(pref_vel);
        // samples.push_back(null_vel);

        double min_cost = DBL_MAX;
        Vector2 best_vel;

        for (int i = 0; i < (int) samples.size(); i++) {
            VelocitySample cur = samples[i];
            double cost = calculateVelCosts(cur.velocity, truncated_vos, pref_vel, max_speed, use_truncation);
            cost += cur.dist_to_pref_vel;
            if (cost < min_cost) {
                min_cost = cost;
                best_vel = cur.velocity;
            }
        }
        //ROS_ERROR("min_cost %f", min_cost);
        return best_vel;
    }

    double calculateVelCosts(const Vector2 &test_vel, const std::vector<VO> &truncated_vos, const Vector2 &pref_vel,
                             double max_speed, bool use_truncation) {
        double cost = 0.0;
        double COST_IN_VO = 1000.0;
        for (int j = 0; j < (int) truncated_vos.size(); j++) {
            if (isInsideVO(truncated_vos[j], test_vel, use_truncation)) {
                cost += (truncated_vos.size() - j) * COST_IN_VO;
            }
        }
        cost += absSqr(pref_vel - test_vel);
        return cost;
    }

    Vector2 calculateClearpathVelocity(std::vector<VelocitySample> &samples, const std::vector<VO> &truncated_vos,
                                       const std::vector<Line> &additional_constraints, const Vector2 &pref_vel,
                                       double max_speed, bool use_truncation) {

        if (!isWithinAdditionalConstraints(additional_constraints, pref_vel)) {
            BOOST_FOREACH (Line line, additional_constraints) {
                            VelocitySample pref_vel_sample;
                            pref_vel_sample.velocity = intersectTwoLines(line.point, line.dir, pref_vel,
                                                                         Vector2(line.dir.y(), -line.dir.x()));
                            pref_vel_sample.dist_to_pref_vel = absSqr(pref_vel - pref_vel_sample.velocity);
                            samples.push_back(pref_vel_sample);
                        }
        }
        else {
            VelocitySample pref_vel_sample;
            pref_vel_sample.velocity = pref_vel;
            pref_vel_sample.dist_to_pref_vel = 0;
            samples.push_back(pref_vel_sample);
        }
        VelocitySample null_vel_sample;
        null_vel_sample.velocity = Vector2(0, 0);
        null_vel_sample.dist_to_pref_vel = absSqr(pref_vel);
        samples.push_back(null_vel_sample);

        BOOST_FOREACH(Line line, additional_constraints) {
                        BOOST_FOREACH(Line line2, additional_constraints) {
                                        addRayVelocitySamples(samples, pref_vel, line.point, line.dir, line2.point,
                                                              line2.dir, max_speed, LINELINE);
                                    }
                    }

        for (int i = 0; i < (int) truncated_vos.size(); i++) {
            if (isInsideVO(truncated_vos[i], pref_vel, use_truncation)) {

                VelocitySample leg_projection;
                if (leftOf(truncated_vos[i].point, truncated_vos[i].relative_position,
                           pref_vel)) { //left of centerline, project on left leg
                    leg_projection.velocity = intersectTwoLines(truncated_vos[i].point, truncated_vos[i].left_leg_dir,
                                                                pref_vel, Vector2(truncated_vos[i].left_leg_dir.y(),
                                                                                -truncated_vos[i].left_leg_dir.x()));
                }
                else { //project on right leg
                    leg_projection.velocity = intersectTwoLines(truncated_vos[i].point, truncated_vos[i].right_leg_dir,
                                                                pref_vel, Vector2(truncated_vos[i].right_leg_dir.y(),
                                                                                -truncated_vos[i].right_leg_dir.x()));
                }

                //if(absSqr(leg_projection.velocity) < max_speed) { //only add if below max_speed
                leg_projection.dist_to_pref_vel = absSqr(pref_vel - leg_projection.velocity);
                samples.push_back(leg_projection);
                //}

                if (use_truncation) {
                    addRayVelocitySamples(samples, pref_vel, pref_vel, -truncated_vos[i].relative_position,
                                          truncated_vos[i].trunc_left,
                                          truncated_vos[i].trunc_right - truncated_vos[i].trunc_left, max_speed,
                                          RAYSEGMENT);
                }
            }
        }

        // for (int i= 0 ; i< (int) truncated_vos.size(); i++){ //intersect with max_speed circle
        //   if (!use_truncation) {
        // 	addCircleLineIntersections(samples, pref_vel, max_speed, use_truncation, truncated_vos[i].point, truncated_vos[i].left_leg_dir); //intersect with left leg_dir
        // 	addCircleLineIntersections(samples, pref_vel, max_speed, use_truncation, truncated_vos[i].point, truncated_vos[i].right_leg_dir); //intersect with right_leg_dir
        //   }
        //   else {
        // 	addCircleLineIntersections(samples, pref_vel, max_speed, use_truncation, truncated_vos[i].trunc_left, truncated_vos[i].left_leg_dir); //intersect with left leg_dir
        // 	addCircleLineIntersections(samples, pref_vel, max_speed, use_truncation, truncated_vos[i].trunc_right, truncated_vos[i].right_leg_dir); //intersect with right_leg_dir

        //  	//addCircleLineIntersections(samples, pref_vel, max_speed, use_truncation, truncated_vos[i].trunc_left, truncated_vos[i].trunc_right - truncated_vos[i].trunc_left, i, truncated_vos[i]); //intersect with truncation line
        //   }
        // }

        for (int i = 0; i < (int) truncated_vos.size(); i++) {
            for (int j = 0; j < (int) additional_constraints.size(); j++) {
                if (!use_truncation) {
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].left_leg_dir,
                                          additional_constraints[j].point, additional_constraints[j].dir, max_speed,
                                          RAYLINE);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].right_leg_dir,
                                          additional_constraints[j].point, additional_constraints[j].dir, max_speed,
                                          RAYLINE);
                }
                else {
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left, truncated_vos[i].left_leg_dir,
                                          additional_constraints[j].point, additional_constraints[j].dir, max_speed,
                                          RAYLINE);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_right,
                                          truncated_vos[i].right_leg_dir, additional_constraints[j].point,
                                          additional_constraints[j].dir, max_speed, RAYLINE);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left,
                                          truncated_vos[i].trunc_right - truncated_vos[i].trunc_left,
                                          additional_constraints[j].point, additional_constraints[j].dir, max_speed,
                                          SEGMENTLINE);
                }
            }

            for (int j = i + 1; j < (int) truncated_vos.size(); j++) {

                if (!use_truncation) {
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].left_leg_dir,
                                          truncated_vos[j].point, truncated_vos[j].left_leg_dir, max_speed, RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].left_leg_dir,
                                          truncated_vos[j].point, truncated_vos[j].right_leg_dir, max_speed, RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].right_leg_dir,
                                          truncated_vos[j].point, truncated_vos[j].left_leg_dir, max_speed, RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].point, truncated_vos[i].right_leg_dir,
                                          truncated_vos[j].point, truncated_vos[j].right_leg_dir, max_speed, RAYRAY);

                }
                else {
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left, truncated_vos[i].left_leg_dir,
                                          truncated_vos[j].trunc_left, truncated_vos[j].left_leg_dir, max_speed,
                                          RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left, truncated_vos[i].left_leg_dir,
                                          truncated_vos[j].trunc_right, truncated_vos[j].right_leg_dir, max_speed,
                                          RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_right,
                                          truncated_vos[i].right_leg_dir, truncated_vos[j].trunc_left,
                                          truncated_vos[j].left_leg_dir, max_speed, RAYRAY);
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_right,
                                          truncated_vos[i].right_leg_dir, truncated_vos[j].trunc_left,
                                          truncated_vos[j].right_leg_dir, max_speed, RAYRAY);

                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left, truncated_vos[i].left_leg_dir,
                                          truncated_vos[j].trunc_left,
                                          truncated_vos[j].trunc_right - truncated_vos[j].trunc_left, max_speed,
                                          RAYSEGMENT); //left trunc
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[j].trunc_left, truncated_vos[j].left_leg_dir,
                                          truncated_vos[i].trunc_left,
                                          truncated_vos[i].trunc_right - truncated_vos[i].trunc_left, max_speed,
                                          RAYSEGMENT); //trunc left

                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_right,
                                          truncated_vos[i].right_leg_dir, truncated_vos[j].trunc_left,
                                          truncated_vos[j].trunc_right - truncated_vos[j].trunc_left, max_speed,
                                          RAYSEGMENT); //right trunc
                    addRayVelocitySamples(samples, pref_vel, truncated_vos[j].trunc_right,
                                          truncated_vos[j].right_leg_dir, truncated_vos[i].trunc_left,
                                          truncated_vos[i].trunc_right - truncated_vos[i].trunc_left, max_speed,
                                          RAYSEGMENT); //trunc right

                    addRayVelocitySamples(samples, pref_vel, truncated_vos[i].trunc_left,
                                          truncated_vos[i].trunc_right - truncated_vos[i].trunc_left,
                                          truncated_vos[j].trunc_left,
                                          truncated_vos[j].trunc_right - truncated_vos[j].trunc_left, max_speed,
                                          SEGMENTSEGMENT); //trunc trunc

                }

            }
        }

        //    ROS_ERROR("projection list length  = %d", samples.size());

        std::sort(samples.begin(), samples.end(), compareVelocitySamples);

        Vector2 new_vel; // = pref_vel;

        bool valid = false;
        bool foundOutside = false;
        bool outside = true;
        int optimal = -1;

        for (int i = 0; i < (int) samples.size(); i++) {
            outside = true;
            valid = true;
            if (!isWithinAdditionalConstraints(additional_constraints, samples[i].velocity)) {
                outside = false;
            }

            for (int j = 0; j < (int) truncated_vos.size(); j++) {
                if (isInsideVO(truncated_vos[j], samples[i].velocity, use_truncation)) {
                    valid = false;
                    if (j > optimal) {
                        optimal = j;
                        new_vel = samples[i].velocity;
                    }
                    break;
                }
            }
            if (valid && outside) {
                return samples[i].velocity;
            }
            if (valid && !outside && !foundOutside) {
                optimal = truncated_vos.size();
                new_vel = samples[i].velocity;
                foundOutside = true;
            }

        }
        //    ROS_INFO("selected j %d, of size %d", optimal, (int) truncated_vos.size());

        return new_vel;
    }

    std::vector<Vector2> minkowskiSum(const std::vector<Vector2> polygon1, const std::vector<Vector2> polygon2) {
        std::vector<Vector2> result;
        std::vector<ConvexHullPoint> convex_hull;

        for (int i = 0; i < (int) polygon1.size(); i++) {
            for (int j = 0; j < (int) polygon2.size(); j++) {
                ConvexHullPoint p;
                p.point = polygon1[i] + polygon2[j];
                convex_hull.push_back(p);
            }

        }
        convex_hull = convexHull(convex_hull, false);
        for (int i = 0; i < (int) convex_hull.size(); i++) {
            result.push_back(convex_hull[i].point);
        }
        return result;

    }

    bool compareVectorsLexigraphically(const ConvexHullPoint &v1, const ConvexHullPoint &v2) {
        return v1.point.x() < v2.point.x() || (v1.point.x() == v2.point.x() && v1.point.y() < v2.point.y());
    }

    double cross(const ConvexHullPoint &O, const ConvexHullPoint &A, const ConvexHullPoint &B) {
        return (A.point.x() - O.point.x()) * (B.point.y() - O.point.y()) -
               (A.point.y() - O.point.y()) * (B.point.x() - O.point.x());
    }

    // Returns a list of points on the convex hull in counter-clockwise order.
    // Note: the last point in the returned list is the same as the first one.
    //Wikipedia Monotone chain...
    std::vector<ConvexHullPoint> convexHull(std::vector<ConvexHullPoint> P, bool sorted) {
        int n = P.size(), k = 0;
        std::vector<ConvexHullPoint> result(2 * n);

        // Sort points lexicographically
        if (!sorted)
            sort(P.begin(), P.end(), compareVectorsLexigraphically);

        //    ROS_WARN("points length %d", (int)P.size());

        // Build lower hull
        for (int i = 0; i < n; i++) {
            while (k >= 2 && cross(result[k - 2], result[k - 1], P[i]) <= 0) k--;
            result[k++] = P[i];
        }

        // Build upper hull
        for (int i = n - 2, t = k + 1; i >= 0; i--) {
            while (k >= t && cross(result[k - 2], result[k - 1], P[i]) <= 0) k--;
            result[k++] = P[i];
        }
        result.resize(k);

        return result;
    }

    bool compareVelocitySamples(const VelocitySample &p1, const VelocitySample &p2) {
        return p1.dist_to_pref_vel < p2.dist_to_pref_vel;
    }

    Vector2 intersectTwoLines(Vector2 point1, Vector2 dir1, Vector2 point2, Vector2 dir2) {
        double x1, x2, x3, x4, y1, y2, y3, y4;
        x1 = point1.x();
        y1 = point1.y();
        x2 = x1 + dir1.x();
        y2 = y1 + dir1.y();
        x3 = point2.x();
        y3 = point2.y();
        x4 = x3 + dir2.x();
        y4 = y3 + dir2.y();

        double det = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);

        if (det == 0) {
            return Vector2(0, 0); //TODO fix return NULL

        }
        double x_i = ((x3 - x4) * (x1 * y2 - y1 * x2) - (x1 - x2) * (x3 * y4 - y3 * x4)) / det;
        double y_i = ((y3 - y4) * (x1 * y2 - y1 * x2) - (y1 - y2) * (x3 * y4 - y3 * x4)) / det;

        return Vector2(x_i, y_i);
    }

    Vector2 intersectTwoLines(Line line1, Line line2) {
        return intersectTwoLines(line1.point, line1.dir, line2.point, line2.dir);
    }

}


#pragma once

#include <polyfem/Common.hpp>
#include <polyfem/ElasticityUtils.hpp>

#include <polyfem/ElementAssemblyValues.hpp>
#include <polyfem/ElementBases.hpp>
#include <polyfem/AutodiffTypes.hpp>
#include <polyfem/Types.hpp>

#include <Eigen/Dense>
#include <array>

namespace polyfem
{
	// Similar to HookeLinear but with non-linear stress strain: C:½(F+Fᵀ+FᵀF)
	class SaintVenantElasticity
	{
	public:
		SaintVenantElasticity();

		Eigen::MatrixXd assemble_hessian(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;
		Eigen::VectorXd assemble_grad(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;
		double compute_energy(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		Eigen::Matrix<double, Eigen::Dynamic, 1, 0, 3, 1>
		compute_rhs(const AutodiffHessianPt &pt) const;

		inline int size() const { return size_; }
		void set_size(const int size);

		void set_stiffness_tensor(int i, int j, const double val);
		double stifness_tensor(int i, int j) const;

		void compute_von_mises_stresses(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &stresses) const;
		void compute_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, Eigen::MatrixXd &tensor) const;

		void set_parameters(const json &params);

	private:
		int size_ = 2;

		ElasticityTensor elasticity_tensor_;

		template <typename T, unsigned long N>
		T stress(const std::array<T, N> &strain, const int j) const;

		template <typename T>
		T compute_energy_aux(const ElementAssemblyValues &vals, const Eigen::MatrixXd &displacement, const QuadratureVector &da) const;

		void assign_stress_tensor(const int el_id, const ElementBases &bs, const ElementBases &gbs, const Eigen::MatrixXd &local_pts, const Eigen::MatrixXd &displacement, const int all_size, Eigen::MatrixXd &all, const std::function<Eigen::MatrixXd(const Eigen::MatrixXd &)> &fun) const;
	};
} // namespace polyfem


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          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 __OPTIMISATION2_BA_CONTAINER_HPP__
#define __OPTIMISATION2_BA_CONTAINER_HPP__

#include "../container/container.hpp"

#include <libv/lma/color/console.hpp>
#include <cassert>
#include <libv/core/logger.hpp>

#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/ttt/traits/typed_indice.hpp>
#include <libv/lma/lm/bundle/indice_container.hpp>
#include <libv/lma/lm/trait/size.hpp>

#include <libv/lma/string/display.hpp>
#include <boost/fusion/include/make_vector.hpp>//! pour la liste computeS
#include "mat.hpp"

#include <type_traits>

namespace lma {

  template<class Id, class Tag_, bool column> class Vector
  {
    public:
      const static std::size_t I = Size<Id>::value;
      const static std::size_t J = (column?1:I);
      typedef Tag_ MatrixTag;
      typedef ContainerOption<MatrixTag,I,J> MatrixMaker;
      typedef typename MatrixMaker::Matrix Matrix;
      typedef ttt::Indice<Id> Indice;
      typedef Id Key;
      typedef typename MatrixMaker::Float Float;

      typedef Vector<Id,MatrixTag,column> self;

      Vector(){/* std::cout << name() << std::endl;*/ }

      static std::string name()
      { return color.cyan() + "Vector<" + color.reset() + ttt::name<Id>() + "," + ttt::name<Matrix>() + color.cyan() + ">" +color.reset(); }

      bool empty() const { return v.empty(); }
      void resize(const Indice& indice) { v.resize(indice(),MatrixMaker::Zero()); }
      void assign(const Indice& indice) { v.assign(indice(),MatrixMaker::Zero()); }

      Matrix& operator()(const Indice& indice) {
        assert(indice<size());return v[indice()]; }
      const Matrix& operator()(const Indice& indice) const {
        assert(indice<size());return v[indice()]; }

      Indice first() const { return Indice(0); }
      Indice size() const { return Indice(v.size()); }
      size_t full_size() const { return v.size() * I; }

      void set_zero() { for(auto& x : v) x = MatrixMaker::Zero();}

      bool is_invalid() const
      {
        for(auto& x : v)
          for(size_t k = 0 ; k < I ; ++k)
          {
            if (lma::is_invalid(x[k]))
            {
              std::cout << " invalid value : " << x[k] << std::endl;
              return true;
            }
          }
        return false;
      }

      typedef std::vector<Matrix,Eigen::aligned_allocator<Matrix>> TVector;

      void disp() const
      {
        for(auto& x : v)
          std::cout << x.transpose() << " ";
        std::cout << std::endl;
      }

      void copy_mat(const self& vector)
      {
        v = vector.v;
      }

      TVector v;

    public:

      typedef typename TVector::iterator iterator;
      typedef typename TVector::const_iterator citerator;
      iterator begin() { return v.begin(); }
      iterator end() { return v.end(); }
      citerator begin() const { return v.cbegin(); }
      citerator end() const { return v.cend(); }
  };

  template<class Id, class Tag_> class VectorColumn : public Vector<Id,Tag_,true>
  {
    public:
      typedef Tag_ Tag;
      typedef Vector<Id,Tag,true> parent;
      using parent::I;
      typedef typename parent::Matrix Matrix;
      typedef typename parent::Float Float;
      using parent::first;
      using parent::size;
      using parent::empty;
//       Vector(){/* std::cout << name() << std::endl;*/ }

      static std::string name()
      { return color.cyan() + "VectorColumn<" + color.reset() + ttt::name<Id>() + "," + ttt::name<Matrix>() + color.cyan() + ">" +color.reset(); }

      void minus() { for(auto& x : *this) x = -x; }

      Float squaredNorm() const
      {
        Float n = 0;
        for(auto& x : *this)
          n += squared_norm(x);
        return n;
      }

//       Float norm() const
//       {
//         Float n = 0;
//         for(auto& x : *this)
//           n += std::sqrt(squared_norm(x));
//         return n;
//       }

      Float compute_dot(const VectorColumn<Id,Tag>& b) const
      {
        Float res = 0;
        assert(b.size()==size());
        for(auto i = first() ; i < size() ; ++i)
          res += dot((*this)(i),b(i));
        return res;
      }
  };

  template<class Id, class T>
  std::ostream& operator<<(std::ostream& o, const VectorColumn<Id,T>& residu)
  {
    o << color.green() << residu.name() << color.reset() << std::endl;

    for(auto i = residu.first() ; i < residu.size() ; ++i)
    {
      if (i==0) std::cout << color.cyan() << " ==== " << i << " <" << ttt::name<decltype(i)>() << "> ==== " << color.reset();
      else
        std::cout << color.cyan() << " ==== " << i << color.reset() ;
      o << std::endl << residu(i) << std::endl;
    }
    return o;
  }

  /// ########### Table #############

  template<class Id1_, class Id2_, class Tag, class Spec = void> struct Table
  {
      typedef Id1_ Id1;
      typedef Id2_ Id2;
      typedef Table<Id1,Id2,Tag,Spec> self;
      typedef Id1 P1;
      typedef Id2 P2;
      static const size_t I = Size<Id1>::value;
      static const size_t J = Size<Id2>::value;
      typedef ContainerOption<Tag,I,J> MatrixMaker;
      typedef typename MatrixMaker::Matrix Matrix;
//       typedef typename MatrixMaker::MatrixDD MatrixDD;
      typedef typename MatrixMaker::Float Float;
      typedef Id1 Key1;
      typedef Id2 Key2;
      typedef SIC<Id1,Id2> IndiceContainer;

      typedef typename IndiceContainer::IndiceA Indice1;
      typedef typename IndiceContainer::SparseAB tSparse;

      typedef std::vector< Matrix, Eigen::aligned_allocator<Matrix>> TVector;
      TVector v;
      std::vector<int> voffset;
      IndiceContainer indice;

      static std::size_t H() { return I; }
      static std::size_t W() { return J; }
      static std::string name() { return color.cyan() + "Table<" + color.reset() + ttt::name<Id1>() + "," + ttt::name<Id2>() + " : " + ttt::name<Matrix>() + color.cyan() + ">" + color.reset(); }

      Table() {}

      void set(const SIC<Id1,Id2>& cont_) { resize(cont_); }

      void resize(const SIC<Id1,Id2>& cont_)
      {
        indice = cont_;
        /*
        if (indice.size()==0)
        {
          std::string msg = "Warning : indice.size()==0" + name();
          V_DUMP(msg);
        }
        */
        voffset.clear();
        std::size_t total = 0;
        for(auto i = indice.first() ; i < indice.size() ; ++i)
        {
          voffset.push_back(total);
          total += indice.size(i)();
        }
        v.assign(total, MatrixMaker::Zero());
      }

      //! réserver begin et end pour iterator -> for( auto x : table ) {...}
      auto first() const ->decltype(indice.first()) { return indice.first(); }
      auto size() const ->decltype(indice.size()) { return indice.size(); }

      auto first(const Indice1& indice1) const ->decltype(indice.first(indice1)) { return indice.first(indice1); }
      auto size(const Indice1& indice1) const ->decltype(indice.size(indice1)) { return indice.size(indice1); }

      Matrix& operator()(const Indice1& indice1, const tSparse& indice2)
      {
        assert(indice1 < voffset.size());
        assert(size_t(voffset[indice1()] + indice2()) < v.size());
        return v[voffset[indice1()] + indice2()];
      }

      const Matrix& operator()(const Indice1& indice1, const tSparse& indice2) const
      {
        assert(indice1 < voffset.size());
        if (!(size_t(voffset[indice1()] + indice2()) < v.size()))
        {
          std::cout << " voffset[indice1()] + indice2()) < v.size() " << std::endl;
          std::cout << " voffset["<<indice1() << "]=" << voffset[indice1()] << " + " << indice2() << "  )" << " <   " << v.size() << std::endl;
        }
        assert(size_t(voffset[indice1()] + indice2()) < v.size());
        return v[voffset[indice1()] + indice2()];
      }

      void disp() const { for(auto&x : v) std::cout << x << std::endl << std::endl; }

      void copy_mat(const Table<Id1,Id2,Tag>& table)
      {
        v = table.v;voffset = table.voffset;
      }

      bool is_invalid() const
      {
        for(auto& x : v)
          for(size_t k = 0 ; k < I ; ++k)
            if (is_invalid(x(k)))
            {
              std::cout << " Invalid value " << x(k) << std::endl;
              return true;
            }
        return false;
      }

      void set_zero() { for(auto& x : v) x = MatrixMaker::Zero();}

      typedef typename TVector::iterator iterator;
      typedef typename TVector::const_iterator citerator;
      iterator begin() { return v.begin(); }
      iterator end() { return v.end(); }
      citerator begin() const { return v.cbegin(); }
      citerator end() const { return v.cend(); }
  };

  struct Diagonal{};
  struct Symetric{};

  template<class P, class T_> struct Table<P,P,T_,Symetric> : public Table<P,P,T_>
  {
    typedef Table<P,P,T_> parent;
    typedef typename parent::MatrixMaker MatrixMaker;
    typedef T_ Tag;
    typedef P Id1;
    typedef P Id2;
    typedef typename parent::Float Float;
//     typedef typename parent::MatrixDD MatrixDD;
    typedef typename parent::Matrix Matrix;
    static const size_t I = parent::I;
    static const size_t J = parent::J;
    using parent::indice;
    using parent::first;
    using parent::size;
    using parent::v;
    using parent::voffset;

//     static std::string name() { return color.cyan() + "SymetricTable<" + color.reset() + ttt::name<Id1>() + "," + ttt::name<Id2>() + " : " + ttt::name<Matrix>() + color.cyan() + ">" + color.reset(); }
    static std::string name() { return std::string("SymetricTable::[") + parent::name() + "]"; }

//     SparseTable(){std::cout << name()  << std::endl;}

    void resize(const SIC<Id1,Id1>& cont_)
    {
      indice = cont_;
      /*
      if (indice.size()==0)
      {
        std::string msg = "Warning : indice.size()==0" + name();
        V_DUMP(msg);
      }
      */
      voffset.clear();
      std::size_t total = 0;
      for(auto i = indice.first() ; i < indice.size() ; ++i)
      {
        voffset.push_back(total);
        total += indice.size(i)();
      }
      v.assign(total, MatrixMaker::Zero());
    }

    void inv()
    {

      for(auto i = first() ; i < size() ; ++i)
      {
        inverse_in_place(this->operator()(i,0));
//         assert( x != Matrix::Zero() );
//         this->operator()(i,0) = this->operator()(i,0).inverse().eval();

//         if (std::isinf(x(0))||std::isnan(x(0)))
//           throw ttt::Indice<Id1>(&x - &v[0]);
//         assert( !std::isinf(x(0)) );
      }

    }

    Float max_lambda() const
    {
      Float lambda = 0;
      for(auto i = first() ; i < size() ; ++i)
        for(size_t k = 0 ; k < I; ++k)
          lambda = std::max(std::abs(at((*this)(i,0),k,k)),lambda);
      return lambda;
    }

    void augment_diag(double lambda)
    {
      for(auto i = first() ; i < size() ; ++i)
//         damp_diag(this->operator()(i,0),lambda);
	for(size_t k = 0 ; k < I ; ++k)
	  at((*this)(i,0),k,k) +=lambda;
    }

    void set_diag_inv(const Table<Id1,Id1,Tag,Symetric>& table)
    {
      voffset.clear();
      v.clear();
      std::size_t total = 0;
      indice.set_max(table.size(),table.size());
      for(auto i = table.first() ; i < table.size() ; ++i)
      {
        indice.add(i,i);
        voffset.push_back(total);
        total++;
      }
      v.resize(total);//, Matrix::Zero());
      for(auto i = table.first() ; i < table.size() ; ++i)
        v[i()] = inverse(table(i,0));
    }

    void set_diag_inv(const Table<Id1,Id1,Tag,Diagonal>& table)
    {
      voffset.clear();
      v.clear();
      std::size_t total = 0;
      indice.set_max(table.size(),table.size());
      for(auto i = table.first() ; i < table.size() ; ++i)
      {
        indice.add(i,i);
        voffset.push_back(total);
        total++;
      }
      v.resize(total);//, MatrixMaker::Zero());
      for(auto i = table.first() ; i < table.size() ; ++i)
        v[i()] = inverse(table(i));
    }
  };

  template<class P, class Q, class T> struct Table<P,Q,T,Diagonal> : public Vector<P,T,false>
  {
    static_assert(boost::is_same<P,Q>::value,"Table<P,Q>: P!=Q");
    typedef P Id1;
    typedef P Id2;
    typedef Vector<P,T,false> parent;
    typedef Table<P,P,T,Diagonal> self;
    typedef typename parent::Float Float;
    static std::string name() { return std::string("DiagonalTable::[") + parent::name() + "]"; }

    using parent::I;

    void augment_diag(double lambda)
    {
      for(auto& block : *this)
        for(size_t k = 0 ; k < I ; ++k)
          block(k,k) += lambda;
    }

    void resize(const SIC<P,P>& sic)
    {
      parent::resize(sic.size());
    }

    Float max_lambda() const
    {
      Float lambda = 0;
      for(auto& x : *this)
        for(size_t k = 0 ; k < I; ++k)
          lambda = std::max(std::abs(x(k,k)),lambda);
      return lambda;
    }

    void inv()
    {
      for(auto& x : *this)
      {
        inverse_in_place(x);
        if (std::isinf(x(0,0)))
          throw ttt::Indice<P>( &x - &parent::v[0] );
        assert( !std::isinf(x(0,0)) );
      }
    }
  };

  template<class> struct IsDiagonal { static const bool value = false; };
  template<class P, class T> struct IsDiagonal<Table<P,P,T,Diagonal>> { static const bool value = true; };

  template<class M, class N, class T, class _>
  std::ostream& disp_container(std::ostream& o, const Table<M,N,T,_>& inter, typename boost::disable_if<boost::is_same<_,Diagonal>>::type* =0)
  {
    o << color.green() << inter.name() << color.reset() << std::endl;
    for(auto i = inter.first(); i < inter.size(); ++i)
    {
      std::cout << color.cyan() << " ==== " << i << " <" << ttt::name<decltype(i)>() << "> ==== " << color.reset();
      for(auto j = inter.first(i) ; j < inter.size(i) ; ++j)
        if (j()==0) o << std::endl << color.magenta() << "   -- " << j << " <" << ttt::name<decltype(j)>() << "> -- \n" << color.reset() << inter(i,j) << std::endl;
        else
          o << std::endl << color.magenta() << "   -- " << j << std::endl << color.reset() << inter(i,j) << std::endl;
      if(inter.size(i)==0) std::cout << std::endl;
    }
    return o;
  }

  template<class M, class T>
  std::ostream& disp_container(std::ostream& o, const Table<M,M,T,Diagonal>& inter)
  {
    o << color.green() << inter.name() << color.reset() << std::endl;
    for(auto i = inter.first(); i < inter.size(); ++i)
    {
      std::cout << color.cyan() << " ==== " << i << " <" << ttt::name<decltype(i)>() << "> ==== " << color.reset();
      o << std::endl << color.magenta() << " -- \n" << color.reset() << inter(i) << std::endl;
      if(inter.size()==0) std::cout << std::endl;
    }
    return o;
  }

  template<class M, class N, class T, class _> std::ostream& operator<<(std::ostream& o, const Table<M,N,T,_>& table)
  {
    return disp_container(o,table);
  }

  template<class P, class T> using DiagonalTable = Table<P,P,T,Diagonal>;
  template<class P, class T> using SymetricTable = Table<P,P,T,Symetric>;

}// eon

namespace ttt
{
  template<class Id,class T, bool col> struct Name< lma::Vector<Id,T,col> > { static std::string name(){ return lma::Vector<Id,T,col>::name();} };
  template<class Id,class T> struct Name< lma::VectorColumn<Id,T> > { static std::string name(){ return lma::VectorColumn<Id,T>::name();} };

  template<class A, class B, class T> struct Name< lma::Table<A,B,T> > { static std::string name(){ return lma::Table<A,B,T>::name();} };
  template<class A, class T> struct Name< lma::SymetricTable<A,T> > { static std::string name(){ return lma::SymetricTable<A,T>::name();} };
  template<class A,class T> struct Name< lma::DiagonalTable<A,T> > { static std::string name(){ return lma::DiagonalTable<A,T>::name();} };
}

#include <libv/lma/lm/ba/tuple_to_mat.hpp>

#endif


// parameters
#include "rjmcmc-wx/wx_parameter.hpp"
typedef parameters< wx_parameter > param;
#include "core/building_footprint_extraction_parameters_inc.hpp"

// images
#include "rjmcmc/image/image_types.hpp"

// optimization
#include "core/building_footprint_extraction.hpp"
#include "rjmcmc/simulated_annealing/simulated_annealing.hpp"

// visitors
#include "rjmcmc/simulated_annealing/visitor/all.hpp"
#include "rjmcmc/simulated_annealing/visitor/wx/all.hpp"

// template instanciations
#include "core/global_reconstruction_unary_energy_inc.hpp"
#include "rjmcmc/image/oriented_inc.hpp"
#include "rjmcmc/image/gradient_functor_inc.hpp"

// threading and wx
#include <boost/thread.hpp>
#include <wx/app.h>
#include "rjmcmc/gui/resources/IGN.xpm"

class building_footprint_rectangle_gui : public wxApp, public Controler
{
    typedef simulated_annealing::composite_visitor<
            simulated_annealing::wx::log_visitor*,
            simulated_annealing::wx::configuration_visitor*,
            simulated_annealing::wx::parameters_visitor*,
            simulated_annealing::wx::chart_visitor*
            > visitor;

private:
    bool OnInit()
    {
        char **my_argv = new char*[argc];
        for (int i=0;i<argc;++i)
        {
            wxString mystr(argv[i]);
            my_argv[i] = new char[mystr.size()+1];
            strcpy(my_argv[i], mystr.To8BitData());
        }
        param *p = param::instance();
        initialize_parameters(p);
        if(!p->parse(argc,my_argv)) return false;
        m_confg_visitor = new simulated_annealing::wx::configuration_visitor((wxFrame *)NULL, wxID_ANY, _("librjmcmc: rectangular building footprint extraction"), wxDefaultPosition, wxSize(600,600) );
        m_confg_visitor->SetIcon(wxICON(IGN));
        m_param_visitor = new simulated_annealing::wx::parameters_visitor(m_confg_visitor);
        m_chart_visitor = new simulated_annealing::wx::chart_visitor(m_confg_visitor);
        m_visitor = new visitor(&m_log_visitor,m_confg_visitor,m_param_visitor,m_chart_visitor);
        m_confg_visitor->controler(this);
        return true;
    }

public:
    virtual void go() {
        release();
        param *p = param::instance();
        update_values(p);

        Iso_rectangle_2 bbox = get_bbox(p);
        std::string ndvi_file = p->get<boost::filesystem::path>("ndvi").string();
        std::string  dsm_file = p->get<boost::filesystem::path>("dsm" ).string();
        clip_bbox(bbox,ndvi_file);
        clip_bbox(bbox,dsm_file );

        gradient_functor gf(p->get<double>("sigmaD"));
        oriented_gradient_view grad_view(dsm_file,  bbox, gf);
        oriented_ndvi_view     ndvi_view(ndvi_file, bbox);

        m_confg_visitor->add_layer(ndvi_file);
        m_confg_visitor->add_layer(dsm_file);
        set_bbox(p,bbox);
        wxPoint p0(wxCoord(bbox.min().x()),wxCoord(bbox.min().y()));
        wxPoint p1(wxCoord(bbox.max().x()),wxCoord(bbox.max().y()));
        m_confg_visitor->set_bbox(wxRect(p0,p1));

        init_visitor        (p,*m_visitor);
        create_configuration(p,grad_view,ndvi_view,m_config);
        create_sampler      (p,m_sampler);
        create_schedule     (p,m_schedule);
        create_end_test     (p,m_end_test);

        m_thread = new boost::thread(
                simulated_annealing::optimize<configuration,sampler,schedule,end_test,visitor>,
                boost::ref(*m_config), boost::ref(*m_sampler),
                boost::ref(*m_schedule), boost::ref(*m_end_test),
                boost::ref(*m_visitor) );
    }

    virtual void stop() { m_end_test->stop(); }

    virtual void param_visibility(bool b) { m_param_visitor->Show(b); }
    virtual void chart_visibility(bool b) { m_chart_visitor->Show(b); }
    virtual bool param_visibility() const { return m_param_visitor->IsShown(); }
    virtual bool chart_visibility() const { return m_chart_visitor->IsShown(); }

    building_footprint_rectangle_gui() :
            m_config(NULL), m_sampler(NULL), m_schedule(NULL), m_end_test(NULL),
            m_visitor(NULL), m_thread(NULL),
            m_confg_visitor(NULL), m_param_visitor(NULL), m_chart_visitor(NULL)
    {
    }

    virtual ~building_footprint_rectangle_gui() {
        release();
        /*
        if(m_confg_visitor) { m_confg_visitor->Close(); }
        if(m_param_visitor) { m_param_visitor->Close(); }
        if(m_chart_visitor) { m_chart_visitor->Close(); }
        */
        if(m_visitor    ) { delete m_visitor; }
    }
private:
    void release()
    {
        if(m_end_test){ m_end_test->stop(); }
        if(m_thread)  { m_thread->join(); delete m_thread; m_thread=NULL; }
        if(m_end_test){ delete m_end_test;m_end_test=NULL; }
        if(m_config)  { delete m_config;  m_config  =NULL; }
        if(m_schedule){ delete m_schedule;m_schedule=NULL; }
        if(m_sampler) { delete m_sampler; m_sampler =NULL; }
    }

    configuration *m_config;
    sampler *m_sampler;
    schedule *m_schedule;
    end_test *m_end_test;
    visitor *m_visitor;
    boost::thread *m_thread;

    simulated_annealing::wx::configuration_visitor *m_confg_visitor;
    simulated_annealing::wx::parameters_visitor    *m_param_visitor;
    simulated_annealing::wx::chart_visitor         *m_chart_visitor;
    simulated_annealing::wx::log_visitor            m_log_visitor;
};

IMPLEMENT_APP(building_footprint_rectangle_gui);


#ifndef STAN_MATH_PRIM_SCAL_PROB_PARETO_TYPE_2_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_PARETO_TYPE_2_RNG_HPP

#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/scal/fun/log1m.hpp>
#include <stan/math/prim/scal/prob/uniform_rng.hpp>

namespace stan {
  namespace math {

    template <class RNG>
    inline double
    pareto_type_2_rng(double mu,
                      double lambda,
                      double alpha,
                      RNG& rng) {
      static const char* function("pareto_type_2_rng");

      check_positive(function, "scale parameter", lambda);
      double uniform_01 = uniform_rng(0.0, 1.0, rng);
      return (std::pow(1.0 - uniform_01, -1.0 / alpha) - 1.0) * lambda + mu;
    }

  }
}
#endif


#ifndef RAND_IK_HPP
#define RAND_IK_HPP

#include "config.hpp"
#include <boost/date_time.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>

namespace RAND_IK {

    enum BasicJointType { RotJoint, TransJoint, Continuous };

    struct DebugInfo
    {
        double parallel_time_random;
        double parallel_time_search;
        double ratio_a;
        double ratio_d;
    };

    class RAND_IK
    {
    public:
        RAND_IK(const KDL::Chain& _chain, int _steps=100, double _eps=1e-5);
        ~RAND_IK(){};

        int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, DebugInfo &info);

    private:
        int steps;
        double eps;
        KDL::Chain chain;

        std::unique_ptr<KDL::ChainJntToJacSolver> jacsolver;
        std::unique_ptr<KDL::ChainFkSolverPos_recursive> fksolver;

        std::vector<BasicJointType> types;

        inline static double squaredNorm(KDL::Twist &t) {
            double sum = 0;
            for (int i = 0; i < 3; i++) {
                sum += t.vel.data[i] * t.vel.data[i];
                sum += t.rot.data[i] * t.rot.data[i];
            }
            return sum;
        }
    };

}

#endif


#ifndef rv_hpp
#define rv_hpp

#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>

class RV
{
public:
    boost::mt19937 rng;
    boost::random::uniform_01<> uniform_real;
    boost::variate_generator<boost::mt19937 &, boost::random::uniform_01<> > generate_uniform_real;

    RV();
};

#endif


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

#include <boost/simd/bitwise/functions/genmaskc.hpp>
#include <boost/simd/include/functions/scalar/unary_minus.hpp>
#include <boost/simd/include/functions/scalar/bitwise_cast.hpp>
#include <boost/simd/sdk/meta/as_arithmetic.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( genmaskc_, tag::cpu_, (A0)
                                   , (scalar_< fundamental_<A0> >)
                                   )
  {
    typedef typename meta::as_arithmetic<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<result_type>::type itype;
      return bitwise_cast<result_type>(unary_minus(itype(!a0)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( genmaskc_, tag::cpu_, (A0)
                                   , (scalar_< arithmetic_<A0> >)
                                   )
  {
    typedef typename meta::as_arithmetic<A0>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<result_type>::type itype;
      return bitwise_cast<result_type>(unary_minus(itype(a0 == 0)));
    }
  };
} } }

#endif


// Copyright (c) 2014-2017 The Bitcoin Core developers
// Copyright (c) 2018 Equibit Group AG
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chainparams.h>
#include <validation.h>
#include <net.h>

#include <test/test_bitcoin.h>

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

#ifdef BUILD_BTC
static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 64;
    CAmount nInitialSubsidy = 50 * COIN;

    CAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        BOOST_CHECK(nSubsidy <= nInitialSubsidy);
        BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
        nPreviousSubsidy = nSubsidy;
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    Consensus::Params consensusParams;
    consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    TestBlockSubsidyHalvings(150); // As in regtest
    TestBlockSubsidyHalvings(1000); // Just another interval
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(CBaseChainParams::MAIN);
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 14000000; nHeight += 1000) {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, chainParams->GetConsensus());
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy * 1000;
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, 2099999997690000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
#else  // BUILD_TESR

BOOST_AUTO_TEST_CASE(calc_subsidy_total_test)
{
    for (int nInterval = 10; nInterval <= 1000000; nInterval *= 10) {
        CAmount nSum = 0;
        int buffer = nInterval/1000;

        for (int nHeight = -buffer; nHeight < nInterval + buffer; nHeight++)
        {
            CAmount nSubsidy = CalcBlockSubsidy(nHeight, nInterval);
            nSum += nSubsidy;

            //if (buffer > 0 && nHeight % buffer == 0) {
            //    std::cout << "\nnHeight = " << nHeight << "\tnSubsidy = " << nSubsidy << "\tnSum= " << nSum;
            //}

            //BOOST_CHECK(MoneyRange(nSum));
        }
        BOOST_CHECK_EQUAL(nSum, MAX_MONEY);
        //std::cout << "nInterval: " << nInterval << " nSum: " << nSum << std::endl;
    }
}

void SubsidyTest(const Consensus::Params& consensusParams)
{
    CAmount nSum = 0;
    int nInterval = consensusParams.nSubsidyInterval;
    int buffer = nInterval / 1000;

    // Test a buffer of heights on either side of the full interval
    for (int nHeight = -buffer; nHeight < nInterval + buffer; nHeight++)
    {
        CAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
        nSum += nSubsidy;

        // if (nHeight % 100000 == 0) {
        //    std::cout << "\nnHeight = " << nHeight << "\tnSubsidy = " << nSubsidy << "\tnSum= " << nSum << "\tMoneyRange=" << MoneyRange(nSum);
        // }

        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, MAX_MONEY);
}

BOOST_AUTO_TEST_CASE(get_subsidy_total_test)
{
    const auto mainParams = CreateChainParams(CBaseChainParams::MAIN);
    SubsidyTest(mainParams->GetConsensus());

    const auto testParams = CreateChainParams(CBaseChainParams::TESTNET);
    SubsidyTest(testParams->GetConsensus());

    // TESR_TODO: revisit?
    // const auto regtParams = CreateChainParams(CBaseChainParams::REGTEST);
    // SubsidyTest(regtParams->GetConsensus());
}

#endif // END_BUILD

BOOST_AUTO_TEST_SUITE_END()


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

#ifndef BOOST_MATH_CCMATH_FLOOR_HPP
#define BOOST_MATH_CCMATH_FLOOR_HPP

#include <cmath>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>
#include <boost/math/ccmath/isinf.hpp>
#include <boost/math/ccmath/isnan.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T floor_pos_impl(T arg) noexcept
{
    T result = 1;

    if(result < arg)
    {
        while(result < arg)
        {
            result *= 2;
        }
        while(result > arg)
        {
            --result;
        }

        return result;
    }
    else
    {
        return T(0);
    }
}

template <typename T>
inline constexpr T floor_neg_impl(T arg) noexcept
{
    T result = -1;

    if(result > arg)
    {
        while(result > arg)
        {
            result *= 2;
        }
        while(result < arg)
        {
            ++result;
        }
        if(result != arg)
        {
            --result;
        }
    }

    return result;
}

template <typename T>
inline constexpr T floor_impl(T arg) noexcept
{
    if(arg > 0)
    {
        return floor_pos_impl(arg);
    }
    else
    {
        return floor_neg_impl(arg);
    }
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real floor(Real arg) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(arg))
    {
        return boost::math::ccmath::abs(arg) == Real(0) ? arg :
               boost::math::ccmath::isinf(arg) ? arg :
               boost::math::ccmath::isnan(arg) ? arg :
               boost::math::ccmath::detail::floor_impl(arg);
    }
    else
    {
        using std::floor;
        return floor(arg);
    }
}

template <typename Z, std::enable_if_t<std::is_integral_v<Z>, bool> = true>
inline constexpr double floor(Z arg) noexcept
{
    return boost::math::ccmath::floor(static_cast<double>(arg));
}

inline constexpr float floorf(float arg) noexcept
{
    return boost::math::ccmath::floor(arg);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double floorl(long double arg) noexcept
{
    return boost::math::ccmath::floor(arg);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_FLOOR_HPP


/*
LICENSE: see isogeometric_application/LICENSE.txt
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: hbui $
//   Date:                $Date: Nov 11, 2017 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes
#include <string>

// External includes
#include <boost/foreach.hpp>
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <boost/python/operators.hpp>

// Project includes
#include "includes/define.h"
#include "python/pointer_vector_set_python_interface.h"
#include "custom_utilities/patch.h"
#include "custom_utilities/multipatch.h"
#include "custom_utilities/patch_interface.h"
#include "custom_utilities/import_export/multi_nurbs_patch_geo_exporter.h"
#include "custom_utilities/import_export/multi_nurbs_patch_geo_importer.h"
#include "custom_utilities/import_export/multi_nurbs_patch_matlab_exporter.h"
#include "custom_utilities/import_export/multi_nurbs_patch_glvis_exporter.h"
#include "custom_python/iga_define_python.h"
#include "custom_python/add_import_export_to_python.h"
#include "custom_python/add_patches_to_python.h"

namespace Kratos
{

namespace Python
{

using namespace boost::python;

////////////////////////////////////////

template<class TPatchType>
std::size_t Patch_GetId(TPatchType& rDummy)
{
    return rDummy.Id();
}

template<class TPatchType>
void Patch_SetId(TPatchType& rDummy, const std::size_t& Id)
{
    rDummy.SetId(Id);
}

template<class TPatchType>
std::string Patch_GetPrefix(TPatchType& rDummy)
{
    return rDummy.Prefix();
}

template<class TPatchType>
void Patch_SetPrefix(TPatchType& rDummy, const std::string& Prefix)
{
    rDummy.SetPrefix(Prefix);
}

template<class TPatchType>
int Patch_GetLayerIndex(TPatchType& rDummy)
{
    return rDummy.LayerIndex();
}

template<class TPatchType>
void Patch_SetLayerIndex(TPatchType& rDummy, const int& Index)
{
    rDummy.SetLayerIndex(Index);
}

template<class TPatchType>
typename TPatchType::FESpaceType::Pointer Patch_pFESpace(TPatchType& rDummy)
{
    return rDummy.pFESpace();
}

template<int TDim, typename TDataType>
typename GridFunction<TDim, TDataType>::Pointer Patch_CreateGridFunction(Patch<TDim>& rDummy, typename ControlGrid<TDataType>::Pointer pControlGrid)
{
    return rDummy.template CreateGridFunction<TDataType>(pControlGrid);
}

template<int TDim, class TVariableType>
typename GridFunction<TDim, typename TVariableType::Type>::Pointer Patch_GridFunction(Patch<TDim>& rDummy, const TVariableType& rVariable)
{
    return rDummy.template pGetGridFunction<TVariableType>(rVariable);
}

template<class TPatchType>
boost::python::list Patch_LocalCoordinates(TPatchType& rDummy, boost::python::list& P, boost::python::list& xi0)
{
    typedef boost::python::stl_input_iterator<double> iterator_value_type;

    std::vector<double> xi0_vec;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(xi0), iterator_value_type() ) )
    {
        xi0_vec.push_back(v);
    }

    std::vector<double> P_vec;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(P), iterator_value_type() ) )
    {
        P_vec.push_back(v);
    }

    array_1d<double, 3> point, xi;
    noalias(point) = ZeroVector(3);
    noalias(xi) = ZeroVector(3);

    for (std::size_t i = 0; i < std::min(static_cast<std::size_t>(3), P_vec.size()); ++i)
        point[i] = P_vec[i];

    for (std::size_t i = 0; i < std::min(static_cast<std::size_t>(3), xi0_vec.size()); ++i)
        xi[i] = xi0_vec[i];

    int stat = rDummy.LocalCoordinates(point, xi);

    boost::python::list out_point;
    out_point.append(xi[0]);
    out_point.append(xi[1]);
    out_point.append(xi[2]);

    boost::python::list output;
    output.append(stat);
    output.append(out_point);
    return output;
}

template<class TPatchType>
bool Patch_IsInside(TPatchType& rDummy, boost::python::list& P, boost::python::list& xi0)
{
    typedef boost::python::stl_input_iterator<double> iterator_value_type;

    std::vector<double> xi0_vec;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(xi0), iterator_value_type() ) )
    {
        xi0_vec.push_back(v);
    }

    std::vector<double> P_vec;
    BOOST_FOREACH(const iterator_value_type::value_type& v, std::make_pair(iterator_value_type(P), iterator_value_type() ) )
    {
        P_vec.push_back(v);
    }

    array_1d<double, 3> point, xi;
    noalias(point) = ZeroVector(3);
    noalias(xi) = ZeroVector(3);

    for (std::size_t i = 0; i < std::min(static_cast<std::size_t>(3), P_vec.size()); ++i)
        point[i] = P_vec[i];

    for (std::size_t i = 0; i < std::min(static_cast<std::size_t>(3), xi0_vec.size()); ++i)
        xi[i] = xi0_vec[i];

    return rDummy.IsInside(point, xi);
}

template<int TDim>
typename PatchInterface<TDim>::Pointer Patch_GetInterface(Patch<TDim>& rDummy, const std::size_t& i)
{
    return rDummy.pInterface(i);
}

template<int TDim>
typename Patch<TDim-1>::Pointer Patch_ConstructBoundaryPatch(Patch<TDim>& rDummy, const std::size_t& iside)
{
    BoundarySide side = static_cast<BoundarySide>(iside);
    return rDummy.ConstructBoundaryPatch(side);
}

template<class TMultiPatchType>
typename TMultiPatchType::PatchContainerType MultiPatch_GetPatches(TMultiPatchType& rDummy)
{
    return rDummy.Patches();
}

template<class TPatchType, class TMultiPatchType>
typename TPatchType::Pointer MultiPatch_GetItem(TMultiPatchType& rDummy, std::size_t index)
{
    return rDummy.pGetPatch(index);
}

template<class TMultiPatchType>
std::size_t MultiPatch_Len(TMultiPatchType& rDummy)
{
    return rDummy.size();
}

template<int TDim>
std::size_t MultiPatch_Enumerate1(MultiPatch<TDim>& rDummy)
{
    std::size_t system_size;

    system_size = rDummy.Enumerate();
//    KRATOS_WATCH(system_size)

    return system_size;
}

template<int TDim>
std::size_t MultiPatch_Enumerate2(MultiPatch<TDim>& rDummy, const std::size_t& start)
{
    std::size_t system_size;

    system_size = rDummy.Enumerate(start);
//    KRATOS_WATCH(system_size)

    return system_size;
}

template<int TDim>
typename Patch<TDim>::Pointer PatchInterface_pPatch1(PatchInterface<TDim>& rDummy)
{
    return rDummy.pPatch1();
}

template<class TPatchInterfaceType>
BoundarySide PatchInterface_Side1(TPatchInterfaceType& rDummy)
{
    return rDummy.Side1();
}

template<int TDim>
typename Patch<TDim>::Pointer PatchInterface_pPatch2(PatchInterface<TDim>& rDummy)
{
    return rDummy.pPatch2();
}

template<class TPatchInterfaceType>
BoundarySide PatchInterface_Side2(TPatchInterfaceType& rDummy)
{
    return rDummy.Side2();
}

////////////////////////////////////////

template<int TDim>
void IsogeometricApplication_AddPatchesToPython_Helper()
{
    std::stringstream ss;

    ss.str(std::string());
    ss << "Patch" << TDim << "D";
    class_<Patch<TDim>, bases<Flags> >
    // class_<Patch<TDim>, typename Patch<TDim>::Pointer > // do not use this to export Patch pointer
    (ss.str().c_str(), init<const std::size_t&, typename FESpace<TDim>::Pointer>())
    .add_property("Id", &Patch_GetId<Patch<TDim> >, &Patch_SetId<Patch<TDim> >)
    .add_property("Prefix", &Patch_GetPrefix<Patch<TDim> >, &Patch_SetPrefix<Patch<TDim> >)
    .add_property("LayerIndex", &Patch_GetLayerIndex<Patch<TDim> >, &Patch_SetLayerIndex<Patch<TDim> >)
    .def("WorkingSpaceDimension", &Patch<TDim>::WorkingSpaceDimension)
    .def("Name", &Patch<TDim>::Name)
    .def("CreateControlPointGridFunction", &Patch<TDim>::CreateControlPointGridFunction)
    .def("CreateGridFunction", &Patch_CreateGridFunction<TDim, double>)
    .def("CreateGridFunction", &Patch_CreateGridFunction<TDim, array_1d<double, 3> >)
    .def("CreateGridFunction", &Patch_CreateGridFunction<TDim, Vector>)
    .def("GridFunction", &Patch_GridFunction<TDim, Variable<ControlPoint<double> > >)
    .def("GridFunction", &Patch_GridFunction<TDim, Variable<double> >)
    .def("GridFunction", &Patch_GridFunction<TDim, Variable<array_1d<double, 3> > >)
    .def("GridFunction", &Patch_GridFunction<TDim, Variable<Vector> >)
    .def("ApplyTransformation", &Patch<TDim>::ApplyTransformation)
    .def("Order", &Patch<TDim>::Order)
    .def("TotalNumber", &Patch<TDim>::TotalNumber)
    .def("FESpace", &Patch_pFESpace<Patch<TDim> >)
    .def("LocalCoordinates", &Patch_LocalCoordinates<Patch<TDim> >)
    .def("IsInside", &Patch_IsInside<Patch<TDim> >)
    .def("NumberOfInterfaces", &Patch<TDim>::NumberOfInterfaces)
    .def("AddInterface", &Patch<TDim>::AddInterface)
    .def("RemoveInterface", &Patch<TDim>::RemoveInterface)
    .def("GetInterface", &Patch_GetInterface<TDim>)
    .def("ConstructBoundaryPatch", &Patch_ConstructBoundaryPatch<TDim>)
    .def("FindBoundarySide", &Patch<TDim>::FindBoundarySide)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "Patch" << TDim << "DPointer";
    class_<typename Patch<TDim>::Pointer>
    (ss.str().c_str(), init<typename Patch<TDim>::Pointer>())
    .def("GetReference", GetReference<Patch<TDim> >, return_value_policy<reference_existing_object>())
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "Patch" << TDim << "DContainer";
    PointerVectorSetPythonInterface<typename MultiPatch<TDim>::PatchContainerType>::CreateInterface(ss.str());

    ss.str(std::string());
    ss << "PatchInterface" << TDim << "D";
    // class_<PatchInterface<TDim> >
    class_<PatchInterface<TDim>, typename PatchInterface<TDim>::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def(init<typename Patch<TDim>::Pointer, const BoundarySide&, typename Patch<TDim>::Pointer, const BoundarySide&>())
    .def("Patch1", &PatchInterface_pPatch1<TDim>)
    .def("Patch2", &PatchInterface_pPatch2<TDim>)
    .def("Side1", &PatchInterface_Side1<PatchInterface<TDim> >)
    .def("Side2", &PatchInterface_Side2<PatchInterface<TDim> >)
    .def(self_ns::str(self))
    ;

    ss.str(std::string());
    ss << "MultiPatch" << TDim << "D";
    class_<MultiPatch<TDim>, typename MultiPatch<TDim>::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<>())
    // .def("ResetId", &MultiPatch<TDim>::ResetId) // this function is not really useful. One shall keep control over the id of the patch.
    .def("AddPatch", &MultiPatch<TDim>::AddPatch)
    .def("RemovePatch", &MultiPatch<TDim>::RemovePatch)
    .def("Patches", &MultiPatch_GetPatches<MultiPatch<TDim> >)
    .def("__getitem__", &MultiPatch_GetItem<Patch<TDim>, MultiPatch<TDim> >)
    .def("__len__", &MultiPatch_Len<MultiPatch<TDim> >)
    .def("EquationSystemSize", &MultiPatch<TDim>::EquationSystemSize)
    .def("ResetFunctionIndices", &MultiPatch<TDim>::ResetFunctionIndices)
    .def("Enumerate", &MultiPatch_Enumerate1<TDim>)
    .def("Enumerate", &MultiPatch_Enumerate2<TDim>)
    .def("IsEnumerated", &MultiPatch<TDim>::IsEnumerated)
    .def(self_ns::str(self))
    ;
}

void IsogeometricApplication_AddExportToPython()
{
    class_<MultiNURBSPatchGeoExporter, MultiNURBSPatchGeoExporter::Pointer, boost::noncopyable>
    ("MultiNURBSPatchGeoExporter", init<>())
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchGeoExporter, Patch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchGeoExporter, Patch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchGeoExporter, Patch<3> >)
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchGeoExporter, MultiPatch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchGeoExporter, MultiPatch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchGeoExporter, MultiPatch<3> >)
    .def(self_ns::str(self))
    ;

    class_<MultiNURBSPatchMatlabExporter, MultiNURBSPatchMatlabExporter::Pointer, boost::noncopyable>
    ("MultiNURBSPatchMatlabExporter", init<>())
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchMatlabExporter, Patch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchMatlabExporter, Patch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchMatlabExporter, Patch<3> >)
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchMatlabExporter, MultiPatch<1> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchMatlabExporter, MultiPatch<2> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchMatlabExporter, MultiPatch<3> >)
    .def(self_ns::str(self))
    ;

    class_<MultiNURBSPatchGLVisExporter, MultiNURBSPatchGLVisExporter::Pointer, boost::noncopyable>
    ("MultiNURBSPatchGLVisExporter", init<>())
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchGLVisExporter, Patch<1> >)
    .def("Export", &MultiPatchExporter_Export_Variable<1, MultiNURBSPatchGLVisExporter, Patch<1>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<1, MultiNURBSPatchGLVisExporter, Patch<1>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<1, MultiNURBSPatchGLVisExporter, Patch<1>, Variable<Vector> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchGLVisExporter, Patch<2> >)
    .def("Export", &MultiPatchExporter_Export_Variable<2, MultiNURBSPatchGLVisExporter, Patch<2>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<2, MultiNURBSPatchGLVisExporter, Patch<2>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<2, MultiNURBSPatchGLVisExporter, Patch<2>, Variable<Vector> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchGLVisExporter, Patch<3> >)
    .def("Export", &MultiPatchExporter_Export_Variable<3, MultiNURBSPatchGLVisExporter, Patch<3>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<3, MultiNURBSPatchGLVisExporter, Patch<3>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<3, MultiNURBSPatchGLVisExporter, Patch<3>, Variable<Vector> >)
    .def("Export", &MultiPatchExporter_Export<1, MultiNURBSPatchGLVisExporter, MultiPatch<1> >)
    .def("Export", &MultiPatchExporter_Export_Variable<1, MultiNURBSPatchGLVisExporter, MultiPatch<1>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<1, MultiNURBSPatchGLVisExporter, MultiPatch<1>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<1, MultiNURBSPatchGLVisExporter, MultiPatch<1>, Variable<Vector> >)
    .def("Export", &MultiPatchExporter_Export<2, MultiNURBSPatchGLVisExporter, MultiPatch<2> >)
    .def("Export", &MultiPatchExporter_Export_Variable<2, MultiNURBSPatchGLVisExporter, MultiPatch<2>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<2, MultiNURBSPatchGLVisExporter, MultiPatch<2>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<2, MultiNURBSPatchGLVisExporter, MultiPatch<2>, Variable<Vector> >)
    .def("Export", &MultiPatchExporter_Export<3, MultiNURBSPatchGLVisExporter, MultiPatch<3> >)
    .def("Export", &MultiPatchExporter_Export_Variable<3, MultiNURBSPatchGLVisExporter, MultiPatch<3>, Variable<double> >)
    .def("Export", &MultiPatchExporter_Export_Variable<3, MultiNURBSPatchGLVisExporter, MultiPatch<3>, Variable<array_1d<double, 3> > >)
    .def("Export", &MultiPatchExporter_Export_Variable_WithComponents<3, MultiNURBSPatchGLVisExporter, MultiPatch<3>, Variable<Vector> >)
    .def(self_ns::str(self))
    ;

}

template<int TDim>
void IsogeometricApplication_AddImportToPython()
{
    std::stringstream ss;

    ss.str(std::string());
    ss << "MultiNURBSPatchGeoImporter" << TDim << "D";
    class_<MultiNURBSPatchGeoImporter<TDim>, typename MultiNURBSPatchGeoImporter<TDim>::Pointer, boost::noncopyable>
    (ss.str().c_str(), init<>())
    .def("ImportSingle", &MultiNURBSPatchGeoImporter<TDim>::ImportSingle)
    .def("Import", &MultiNURBSPatchGeoImporter<TDim>::Import)
    .def(self_ns::str(self))
    ;

}

void IsogeometricApplication_AddPatchesToPython()
{

    /////////////////////////////////////////////////////////////////
    ///////////////////////PATCHES///////////////////////////////////
    /////////////////////////////////////////////////////////////////

    enum_<ParametricAxis>("ParametricAxis")
    .value("U", _PA_U_)
    .value("V", _PA_V_)
    .value("W", _PA_W_)
    ;

    enum_<BoundarySide>("BoundarySide")
    .value("Left", _BLEFT_)
    .value("Right", _BRIGHT_)
    .value("Top", _BTOP_)
    .value("Bottom", _BBOTTOM_)
    .value("Front", _BFRONT_)
    .value("Back", _BBACK_)
    ;

    enum_<BoundarySide2D>("BoundarySide2D")
    .value("U0", _B2LEFT_)
    .value("U1", _B2RIGHT_)
    .value("V0", _B2BOTTOM_)
    .value("V1", _B2TOP_)
    ;

    enum_<BoundarySide3D>("BoundarySide3D")
    .value("U0", _B3LEFT_)
    .value("U1", _B3RIGHT_)
    .value("V0", _B3FRONT_)
    .value("V1", _B3BACK_)
    .value("W0", _B3BOTTOM_)
    .value("W1", _B3TOP_)
    ;

    enum_<BoundaryDirection>("BoundaryDirection")
    .value("Forward", _FORWARD_)
    .value("Reversed", _REVERSED_)
    ;

    enum_<BoundaryFlag>("BoundaryFlag")
    .value("Left", _FLEFT_)
    .value("Right", _FRIGHT_)
    .value("Top", _FTOP_)
    .value("Bottom", _FBOTTOM_)
    .value("Front", _FFRONT_)
    .value("Back", _FBACK_)
    ;

    enum_<IsogeometricEchoFlags>("IsogeometricEchoFlags")
    .value("ECHO_REFINEMENT", ECHO_REFINEMENT)
    .value("ECHO_REFINEMENT_DETAIL", ECHO_REFINEMENT_DETAIL)
    ;

    IsogeometricApplication_AddPatchesToPython_Helper<1>();
    IsogeometricApplication_AddPatchesToPython_Helper<2>();
    IsogeometricApplication_AddPatchesToPython_Helper<3>();

    /////////////////////////////////////////////////////////////////
    ///////////////////////IMPORT/EXPORT/////////////////////////////
    /////////////////////////////////////////////////////////////////

    IsogeometricApplication_AddExportToPython();
    IsogeometricApplication_AddImportToPython<1>();
    IsogeometricApplication_AddImportToPython<2>();
    IsogeometricApplication_AddImportToPython<3>();

}

}  // namespace Python.

} // Namespace Kratos