#include <Configuration.h>
#include <Eigen/Eigenvalues>
#include <PnC/PlannerSet/PIPM_FootPlacementPlanner/Reversal_LIPM_Planner.hpp>
#include <Utils/IO/IOUtilities.hpp>

Reversal_LIPM_Planner::Reversal_LIPM_Planner()
    : FootStepPlanner(),
      com_vel_limit_(2),
      b_set_omega_(false),
      planner_save_data_(11) {
    t_prime_.resize(2);
    kappa_.resize(2);
    x_step_length_limit_.resize(2);
    y_step_length_limit_.resize(2);
    com_vel_limit_.resize(2);
    R_w_t_ = Eigen::MatrixXd::Identity(2, 2);
}

Reversal_LIPM_Planner::~Reversal_LIPM_Planner() {}

void Reversal_LIPM_Planner::_computeSwitchingState(
    double swing_time, const Eigen::Vector3d& com_pos,
    const Eigen::Vector3d& com_vel, const Eigen::Vector3d& stance_foot_loc,
    std::vector<Eigen::Vector2d>& switching_state) {
    double A, B;
    for (int i(0); i < 2; ++i) {
        A = ((com_pos[i] - stance_foot_loc[i]) + com_vel[i] / omega_) / 2.;
        B = ((com_pos[i] - stance_foot_loc[i]) - com_vel[i] / omega_) / 2.;
        switching_state[i][0] = A * exp(omega_ * swing_time) +
                                B * exp(-omega_ * swing_time) +
                                stance_foot_loc[i];
        switching_state[i][1] = omega_ * (A * exp(omega_ * swing_time) -
                                          B * exp(-omega_ * swing_time));
    }
}

// global CoM pos
void Reversal_LIPM_Planner::getNextFootLocation(const Eigen::Vector3d& com_pos,
                                                const Eigen::Vector3d& com_vel,
                                                Eigen::Vector3d& target_loc,
                                                const void* additional_input,
                                                void* additional_output) {
    if (!b_set_omega_) {
        printf("[Reversal Planner] Omega is not set\n");
        exit(0);
    }
    ParamReversalPL* _input = ((ParamReversalPL*)additional_input);
    OutputReversalPL* _output = ((OutputReversalPL*)additional_output);
    _UpdateRotation(_input->yaw_angle);

    std::vector<Eigen::Vector2d> switch_state(2);
    _computeSwitchingState(_input->swing_time, com_pos, com_vel,
                           _input->stance_foot_loc, switch_state);

    // (x, y, xdot, ydot)
    _output->switching_state[0] = switch_state[0][0];
    _output->switching_state[1] = switch_state[1][0];
    _output->switching_state[2] = switch_state[0][1];
    _output->switching_state[3] = switch_state[1][1];
    // printf("switching position: %f, %f\n", switch_state[0][0],
    // switch_state[1][0]);
    // printf("switching velocity: %f, %f\n", switch_state[0][1],
    // switch_state[1][1]);

    // !! TEST !!
     int check_switch(_check_switch_velocity(switch_state));
    //int check_switch(_check_switch_velocity_considering_rotation(switch_state));
    // !! TEST !!
    double new_swing_time(_input->swing_time);
    int count(0);
    while (check_switch != 0) {
        if (check_switch > 0) {  // Too small velocity increase time
            new_swing_time *= 1.1;
            myUtils::color_print(myColor::BoldRed,
                                 "Too small velocity.. increase swing time: " +
                                     std::to_string(new_swing_time),
                                 true);
        } else {  // Too larget velocity decrease time
            new_swing_time *= 0.9;
            myUtils::color_print(myColor::BoldRed,
                                 "Too small velocity.. decrease swing time: " +
                                     std::to_string(new_swing_time),
                                 true);
        }
        _computeSwitchingState(new_swing_time, com_pos, com_vel,
                               _input->stance_foot_loc, switch_state);

        // !! TEST !!
         check_switch = _check_switch_velocity(switch_state);
        //check_switch = _check_switch_velocity_considering_rotation(switch_state);
        // !! TEST !!
        ++count;
        if (count > 0) break;
    }
    _output->time_modification = new_swing_time - _input->swing_time;

    for (int i(0); i < 2; ++i) {
        double exp_weight =
            (exp(omega_ * t_prime_[i]) + exp(-omega_ * t_prime_[i])) /
            (exp(omega_ * t_prime_[i]) - exp(-omega_ * t_prime_[i]));

        target_loc[i] = switch_state[i][0] +
                        (switch_state[i][1] / omega_) * exp_weight +
                        kappa_[i] * (_input->des_loc[i] - switch_state[i][0]);
    }
    target_loc[2] = 0.;

    // _StepLengthCheck(target_loc, switch_state);
    // !! TEST !!
    _StepLengthCheck(target_loc, _input->b_positive_sidestep, _input->stance_foot_loc);
    //_StepLengthCheckConsideringRotation(target_loc, _input->b_positive_sidestep,
                                        //_input->stance_foot_loc);
    // !! TEST !!

    // save data
    for (int i(0); i < 2; ++i) {
        planner_save_data_[i] = com_pos[i];
        planner_save_data_[2 + i] = com_vel[i];
        planner_save_data_[4 + i] = switch_state[i][0];
        planner_save_data_[6 + i] = switch_state[i][1];
        planner_save_data_[8 + i] = target_loc[i];
    }
    planner_save_data_[10] = new_swing_time;
    myUtils::saveVector(planner_save_data_, "planner_data");
}

int Reversal_LIPM_Planner::_check_switch_velocity(
    const std::vector<Eigen::Vector2d>& switch_state) {
    int ret(0);
    double x_vel(switch_state[0][1]);
    double y_vel(switch_state[1][1]);

    // X
    if (x_vel > 0.) {
        if (x_vel < com_vel_limit_[0]) ret = 1;
        if (x_vel > com_vel_limit_[1]) ret = -1;
    } else {
        if (x_vel > -com_vel_limit_[0]) ret = 1;
        if (x_vel < -com_vel_limit_[1]) ret = -1;
    }

    // Y
    if (y_vel > 0.) {
        if (y_vel < com_vel_limit_[0]) ret = 1;
        if (y_vel > com_vel_limit_[1]) ret = -1;
    } else {
        if (y_vel > -com_vel_limit_[0]) ret = 1;
        if (y_vel < -com_vel_limit_[1]) ret = -1;
    }

    return ret;
}

int Reversal_LIPM_Planner::_check_switch_velocity_considering_rotation(
    const std::vector<Eigen::Vector2d>& switch_state) {
    int ret(0);
    Eigen::VectorXd global_com_vel = Eigen::VectorXd::Zero(2);
    global_com_vel << switch_state[0][1], switch_state[1][1];
    Eigen::VectorXd local_com_vel = Eigen::VectorXd::Zero(2);
    local_com_vel = R_w_t_.transpose() * global_com_vel;

    // X
    double x_vel(local_com_vel[0]);
    double y_vel(local_com_vel[1]);
    if (x_vel > 0.) {
        if (x_vel < com_vel_limit_[0]) ret = 1;
        if (x_vel > com_vel_limit_[1]) ret = -1;
    } else {
        if (x_vel > -com_vel_limit_[0]) ret = 1;
        if (x_vel < -com_vel_limit_[1]) ret = -1;
    }

    // Y
    if (y_vel > 0.) {
        if (y_vel < com_vel_limit_[0]) ret = 1;
        if (y_vel > com_vel_limit_[1]) ret = -1;
    } else {
        if (y_vel > -com_vel_limit_[0]) ret = 1;
        if (y_vel < -com_vel_limit_[1]) ret = -1;
    }
    return ret;
}

void Reversal_LIPM_Planner::_StepLengthCheck(
    Eigen::Vector3d& target_loc, bool b_positive_sidestep,
    const Eigen::Vector3d& stance_foot) {
    // X limit check
    double x_step_length(target_loc[0] - stance_foot[0]);
    if (x_step_length < x_step_length_limit_[0]) {
        target_loc[0] = stance_foot[0] + x_step_length_limit_[0];
        myUtils::color_print(
            myColor::BoldRed,
            "x step length hit min: " + std::to_string(x_step_length), true);
        myUtils::color_print(myColor::BoldRed,
                             "new x step: (" + std::to_string(target_loc[0]) +
                                 ", " + std::to_string(stance_foot[0]) + ")",
                             true);
    }
    if (x_step_length > x_step_length_limit_[1]) {
        target_loc[0] = stance_foot[0] + x_step_length_limit_[1];
        myUtils::color_print(
            myColor::BoldRed,
            "x step length hit max: " + std::to_string(x_step_length), true);
        myUtils::color_print(myColor::BoldRed,
                             "new x step: (" + std::to_string(target_loc[0]) +
                                 ", " + std::to_string(stance_foot[0]) + ")",
                             true);
    }

    // Y limit check
    double y_step_length(target_loc[1] - stance_foot[1]);
    if (b_positive_sidestep) {  // move to left
        if (y_step_length < y_step_length_limit_[0]) {
            target_loc[1] = stance_foot[1] + y_step_length_limit_[0];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit min: " + std::to_string(y_step_length),
                true);
            myUtils::color_print(myColor::BoldRed,
                                 "new y step: (" +
                                     std::to_string(target_loc[1]) + ", " +
                                     std::to_string(stance_foot[1]) + ")",
                                 true);
        }

        if (y_step_length > y_step_length_limit_[1]) {
            target_loc[1] = stance_foot[1] + y_step_length_limit_[1];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit max: " + std::to_string(y_step_length),
                true);
            myUtils::color_print(myColor::BoldRed,
                                 "new y step: (" +
                                     std::to_string(target_loc[1]) + ", " +
                                     std::to_string(stance_foot[1]) + ")",
                                 true);
        }

    } else {  // move to right
        if (-y_step_length < y_step_length_limit_[0]) {
            target_loc[1] = stance_foot[1] - y_step_length_limit_[0];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit min: " + std::to_string(y_step_length),
                true);
            myUtils::color_print(myColor::BoldRed,
                                 "new y step: (" +
                                     std::to_string(target_loc[1]) + ", " +
                                     std::to_string(stance_foot[1]) + ")",
                                 true);
        }

        if (-y_step_length > y_step_length_limit_[1]) {
            target_loc[1] = stance_foot[1] - y_step_length_limit_[1];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit max: " + std::to_string(y_step_length),
                true);
            myUtils::color_print(myColor::BoldRed,
                                 "new y step: (" +
                                     std::to_string(target_loc[1]) + ", " +
                                     std::to_string(stance_foot[1]) + ")",
                                 true);
        }
    }
}

void Reversal_LIPM_Planner::_StepLengthCheck(
    Eigen::Vector3d& target_loc,
    const std::vector<Eigen::Vector2d>& switch_state) {
    // X limit check
    double x_step_length(target_loc[0] - switch_state[0][0]);
    if (x_step_length < x_step_length_limit_[0]) {
        target_loc[0] = switch_state[0][0] + x_step_length_limit_[0];
        printf("x step length hit min: %f\n", x_step_length);
        printf("new x step: %f, %f \n", target_loc[0], switch_state[0][0]);
    }
    if (x_step_length > x_step_length_limit_[1]) {
        target_loc[0] = switch_state[0][0] + x_step_length_limit_[1];
        printf("x step length hit max: %f\n", x_step_length);
        printf("new x step: %f, %f \n", target_loc[0], switch_state[0][0]);
    }

    // Y limit check
    double y_step_length(target_loc[1] - switch_state[1][0]);
    if (switch_state[1][1] > 0) {  // move to left
        if (y_step_length < y_step_length_limit_[0]) {
            target_loc[1] = switch_state[1][0] + y_step_length_limit_[0];
        }

        if (y_step_length > y_step_length_limit_[1]) {
            target_loc[1] = switch_state[1][0] + y_step_length_limit_[1];
        }

    } else {  // move to right
        if (-y_step_length < y_step_length_limit_[0])
            target_loc[1] = switch_state[1][0] - y_step_length_limit_[0];
        if (-y_step_length > y_step_length_limit_[1])
            target_loc[1] = switch_state[1][0] - y_step_length_limit_[1];
    }
}

void Reversal_LIPM_Planner::PlannerInitialization(const YAML::Node& node) {
    try {
        Eigen::VectorXd tmp;
        myUtils::readParameter(node, "t_prime", tmp);
        for (int i = 0; i < 2; ++i) {
            t_prime_[i] = tmp[i];
        }
        myUtils::readParameter(node, "kappa", tmp);
        for (int i = 0; i < 2; ++i) {
            kappa_[i] = tmp[i];
        }
        myUtils::readParameter(node, "x_step_length_limit", tmp);
        for (int i = 0; i < 2; ++i) {
            x_step_length_limit_[i] = tmp[i];
        }
        myUtils::readParameter(node, "y_step_length_limit", tmp);
        for (int i = 0; i < 2; ++i) {
            y_step_length_limit_[i] = tmp[i];
        }
        myUtils::readParameter(node, "com_velocity_limit", tmp);
        for (int i = 0; i < 2; ++i) {
            com_vel_limit_[i] = tmp[i];
        }
    } catch (std::runtime_error& e) {
        std::cout << "Error reading parameter [" << e.what() << "] at file: ["
                  << __FILE__ << "]" << std::endl
                  << std::endl;
    }
}

void Reversal_LIPM_Planner::CheckEigenValues(double swing_time) {
    Eigen::MatrixXd A(2, 2);
    printf("omega, swing_time: %f, %f\n", omega_, swing_time);

    for (int i(0); i < 2; ++i) {
        double coth = cosh(omega_ * t_prime_[i]) / sinh(omega_ * t_prime_[i]);

        A(0, 0) = 1 - kappa_[i] + kappa_[i] * cosh(omega_ * swing_time);
        A(0, 1) = (sinh(omega_ * swing_time) +
                   (1. - cosh(omega_ * swing_time)) * coth) /
                  omega_;
        A(1, 0) = kappa_[i] * omega_ * sinh(omega_ * swing_time);
        A(1, 1) = cosh(omega_ * swing_time) - sinh(omega_ * swing_time) * coth;

        Eigen::VectorXcd eivals = A.eigenvalues();
        printf("%d - axis eigen value:\n", i);
        std::cout << eivals << std::endl;
    }
}

void Reversal_LIPM_Planner::_UpdateRotation(double yaw_angle) {
    R_w_t_(0, 0) = cos(yaw_angle);
    R_w_t_(1, 0) = sin(yaw_angle);
    R_w_t_(1, 0) = -sin(yaw_angle);
    R_w_t_(1, 1) = cos(yaw_angle);
}

void Reversal_LIPM_Planner::_StepLengthCheckConsideringRotation(
    Eigen::Vector3d& target_loc, bool b_positive_sidestep,
    const Eigen::Vector3d& stance_foot) {
    Eigen::Vector2d stance_foot_in_torso, target_loc_in_torso,
        v_stance_target_in_torso;
    stance_foot_in_torso << stance_foot[0], stance_foot[1];
    stance_foot_in_torso = R_w_t_.transpose() * stance_foot_in_torso;
    target_loc_in_torso << target_loc[0], target_loc[1];
    target_loc_in_torso = R_w_t_.transpose() * target_loc_in_torso;
    v_stance_target_in_torso = target_loc_in_torso - stance_foot_in_torso;

    // X limit check
    if (v_stance_target_in_torso[0] < x_step_length_limit_[0]) {
        target_loc_in_torso[0] =
            stance_foot_in_torso[0] + x_step_length_limit_[0];
        myUtils::color_print(myColor::BoldRed,
                             "x step length hit min: " +
                                 std::to_string(v_stance_target_in_torso[0]),
                             true);
        // myUtils::color_print(myColor::BoldRed, "new x step: (" +
        // std::to_string(target_loc[0]) + ", " + std::to_string(stance_foot[0])
        // + ")", true);
    }
    if (v_stance_target_in_torso[0] > x_step_length_limit_[1]) {
        target_loc_in_torso[0] =
            stance_foot_in_torso[0] + x_step_length_limit_[1];
        myUtils::color_print(myColor::BoldRed,
                             "x step length hit max: " +
                                 std::to_string(v_stance_target_in_torso[0]),
                             true);
        // myUtils::color_print(myColor::BoldRed, "new x step: ("+
        // std::to_string(target_loc[0]) + ", " + std::to_string(stance_foot[0])
        // + ")", true);
    }

    // Y limit check
    if (b_positive_sidestep) {  // move to left
        if (v_stance_target_in_torso[1] < y_step_length_limit_[0]) {
            target_loc_in_torso[1] =
                stance_foot_in_torso[1] + y_step_length_limit_[0];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit min: " +
                    std::to_string(v_stance_target_in_torso[1]),
                true);
            // myUtils::color_print(myColor::BoldRed, "new y step: ("+
            // std::to_string(target_loc[1]) + ", " +
            // std::to_string(stance_foot[1]) + ")", true);
        }

        if (v_stance_target_in_torso[1] > y_step_length_limit_[1]) {
            target_loc_in_torso[1] =
                stance_foot_in_torso[1] + y_step_length_limit_[1];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit max: " +
                    std::to_string(v_stance_target_in_torso[1]),
                true);
            // myUtils::color_print(myColor::BoldRed, "new y step: ("+
            // std::to_string(target_loc[1]) + ", " +
            // std::to_string(stance_foot[1]) + ")", true);
        }

    } else {  // move to right
        if (-v_stance_target_in_torso[1] < y_step_length_limit_[0]) {
            target_loc_in_torso[1] =
                stance_foot_in_torso[1] - y_step_length_limit_[0];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit min: " +
                    std::to_string(v_stance_target_in_torso[1]),
                true);
            // myUtils::color_print(myColor::BoldRed, "new y step: ("+
            // std::to_string(target_loc[1]) + ", " +
            // std::to_string(stance_foot[1]) + ")", true);
        }

        if (-v_stance_target_in_torso[1] > y_step_length_limit_[1]) {
            target_loc_in_torso[1] =
                stance_foot_in_torso[1] - y_step_length_limit_[1];
            myUtils::color_print(
                myColor::BoldRed,
                "y step length hit max: " +
                    std::to_string(v_stance_target_in_torso[1]),
                true);
            // myUtils::color_print(myColor::BoldRed, "new y step: ("+
            // std::to_string(target_loc[1]) + ", " +
            // std::to_string(stance_foot[1]) + ")", true);
        }
    }
    target_loc[0] = (R_w_t_ * target_loc_in_torso)[0];
    target_loc[1] = (R_w_t_ * target_loc_in_torso)[1];
}


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2013-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#include <vector>

#include <boost/range/numeric.hpp>

#include "mc/classic/reachability_eval.hh"
#include "support/pn/types.hh"

namespace pnmc { namespace mc { namespace classic {

/*------------------------------------------------------------------------------------------------*/

using boost::accumulate;
using boost::apply_visitor;

namespace /* anonymous */ {

/*------------------------------------------------------------------------------------------------*/

struct integer_eval
{
  using result_type = int;
  const std::vector<pn::valuation_type>& values;

  result_type
  operator()(const integer_constant& e)
  const noexcept
  {
    return e.value;
  }

  result_type
  operator()(const integer_sum& e)
  const noexcept
  {
    return accumulate( e.expressions, 0
                     , [this](auto v, const auto& sub_e){return apply_visitor(*this, sub_e) + v;});
  }

  result_type
  operator()(const integer_product& e)
  const noexcept
  {
    return accumulate( e.expressions, 1
                     , [this](auto v, const auto& sub_e){return apply_visitor(*this, sub_e) * v;});
  }

  result_type
  operator()(const integer_difference& e)
  const noexcept
  {
    return apply_visitor(*this, e.lhs_expression) - apply_visitor(*this, e.rhs_expression);
  }

  result_type
  operator()(const integer_division& e)
  const noexcept
  {
    return apply_visitor(*this, e.lhs_expression) / apply_visitor(*this, e.rhs_expression);
  }

  result_type
  operator()(const integer_tokens&)
  const
  {
//    return values[e.pos];
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }
};

/*------------------------------------------------------------------------------------------------*/

struct boolean_eval
{
  using result_type = bool;
  const integer_eval& integer;

  result_type
  operator()(const invariant&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const possibility&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const impossibility&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const true_&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const false_&)
  const
  {
    throw std::runtime_error(__PRETTY_FUNCTION__);
  }

  result_type
  operator()(const negation& e)
  const
  {
    return not apply_visitor(*this, e.expression);
  }

  result_type
  operator()(const conjunction& e)
  const
  {
    for (const auto& sub_e : e.expressions)
    {
      if (not apply_visitor(*this, sub_e))
      {
        return false;
      }
    }
    return true;
  }

  result_type
  operator()(const disjunction& e)
  const
  {
    for (const auto& sub_e : e.expressions)
    {
      if (apply_visitor(*this, sub_e))
      {
        return true;
      }
    }
    return false;
  }

  result_type
  operator()(const exclusive_disjonction& e)
  const
  {
    bool has_true = false;
    for (const auto& sub_e : e.expressions)
    {
      const auto tmp = apply_visitor(*this, sub_e);
      if      (tmp and not has_true) {has_true = true;}
      else if (tmp and has_true)     {return false;}
    }
    return has_true;
  }

  result_type
  operator()(const implication& e)
  const
  {
    return apply_visitor(*this, e.lhs_expression)
         ? apply_visitor(*this, e.rhs_expression)
         : true;
  }

  result_type
  operator()(const equivalence& e)
  const
  {
    return apply_visitor(*this, e.lhs_expression) == apply_visitor(*this, e.rhs_expression);
  }

  result_type
  operator()(const integer_eq& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) == apply_visitor(integer, e.rhs_expression);
  }

  result_type
  operator()(const integer_ne& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) != apply_visitor(integer, e.rhs_expression);
  }

  result_type
  operator()(const integer_lt& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) < apply_visitor(integer, e.rhs_expression);
  }

  result_type
  operator()(const integer_le& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) <= apply_visitor(integer, e.rhs_expression);
  }

  result_type
  operator()(const integer_gt& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) > apply_visitor(integer, e.rhs_expression);
  }

  result_type
  operator()(const integer_ge& e)
  const noexcept
  {
    return apply_visitor(integer, e.lhs_expression) >= apply_visitor(integer, e.rhs_expression);
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace anonymous

/*------------------------------------------------------------------------------------------------*/

int
eval(const integer_ast& a, const SDD&)
{
  if (const auto* ptr = boost::get<integer_constant>(&a))
  {
    return ptr->value;
  }
  std::vector<pn::valuation_type> values;
  return apply_visitor(integer_eval{values}, a);
}

/*------------------------------------------------------------------------------------------------*/

bool
eval(const boolean_ast& a, const SDD&)
{
  if (boost::get<true_>(&a))
  {
    return true;
  }
  else if (boost::get<false_>(&a))
  {
    return false;
  }
  std::vector<pn::valuation_type> values;
  integer_eval integer{values};
  return apply_visitor(boolean_eval{integer}, a);
}

/*------------------------------------------------------------------------------------------------*/

}}} // namespace pnmc::mc::classic


#define BOOST_TEST_MODULE Gpufit

#include "Gpufit/gpufit.h"

#include <boost/test/included/unit_test.hpp>

#include <array>
#include <cmath>

template<std::size_t n_points, std::size_t n_parameters>
void generate_gauss_1d(
    std::array< REAL, n_points >& values,
    std::array< REAL, n_points >& x_data,
    std::array< REAL, n_parameters > const & parameters )
{
    REAL const a = parameters[ 0 ];
    REAL const x0 = parameters[ 1 ];
    REAL const s = parameters[ 2 ];
    REAL const b = parameters[ 3 ];

    for ( int point_index = 0; point_index < n_points; point_index++ )
    {
        REAL const x = x_data[point_index];
        REAL const argx = ( ( x - x0 )*( x - x0 ) ) / ( 2 * s * s );
        REAL const ex = exp( -argx );
        values[ point_index ] = a * ex + b;
    }
}

void gauss_fit_1d()
{
    /*
    Performs a single fit using the GAUSS_1D model.
    - Doesn't use user_info or weights.
    - No noise is added.
    - Checks fitted parameters equalling the true parameters.
    */

    std::size_t const n_fits{ 1 };
    std::size_t const n_points{ 5 };
    std::size_t const n_parameters{ 4 };

    std::array< REAL, n_parameters > const true_parameters{ { 4, 2, .5f, 1 } };

    std::array< REAL, n_points > x_data{ { 0, 1, 2, 3, 4} };
    std::array< REAL, n_points > data{};
    generate_gauss_1d(data, x_data, true_parameters);

    std::array< REAL, n_parameters > initial_parameters{ { 2, 1.5f, 0.3f, 0 } };

    REAL tolerance{ 0.001f };

    int max_n_iterations{ 10 };

    std::array< int, n_parameters > parameters_to_fit{ { 1, 1, 1, 1 } };

    std::array< REAL, n_parameters > output_parameters;
    int output_states;
    REAL output_chi_square;
    int output_n_iterations;

    int const status
        = gpufit
        (
            n_fits,
            n_points,
            data.data(),
            0,
            GAUSS_1D,
            initial_parameters.data(),
            tolerance,
            max_n_iterations,
            parameters_to_fit.data(),
            LSE,
            0,
            0,
            output_parameters.data(),
            &output_states,
            &output_chi_square,
            &output_n_iterations
        );

    BOOST_CHECK(status == 0);
    BOOST_CHECK(output_states == 0);
    BOOST_CHECK(output_chi_square < 1e-6f);
    BOOST_CHECK(output_n_iterations <= max_n_iterations);

    BOOST_CHECK(std::abs(output_parameters[0] - true_parameters[0]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[1] - true_parameters[1]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[2] - true_parameters[2]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[3] - true_parameters[3]) < 1e-6f);
}

void gauss_fit_1d_custom_x()
{
    /*
    Performs two fits using the GAUSS_1D model.
    - Doesn't use or weights.
    - Uses user_info for custom x coordinate values, unique for each fit.
    - No noise is added.
    - Checks fitted parameters equalling the true parameters.
    */

    std::size_t const n_fits{ 2 };
    std::size_t const n_points{ 5 };
    std::size_t const n_parameters{ 4 };

    std::array< REAL, n_parameters > const true_parameters_1{ { 4, 0, .25f, 1 } };
    std::array< REAL, n_parameters > const true_parameters_2{ { 6, .5f, .15f, 2 } };

    std::array< REAL, n_parameters > initial_parameters_1{ { 2, .25f, .15f, 0 } };
    std::array< REAL, n_parameters > initial_parameters_2{ { 8, .75f, .2f, 3 } };

    std::array< REAL, n_points > x_data_1 = { { -1, -.5f, 0, .5f, 1 } };
    std::array< REAL, n_points > x_data_2 = { { 0, .25f, .5f, .75f, 1 } };

    std::array< REAL, n_points > fit_data_1{};
    std::array< REAL, n_points > fit_data_2{};

    generate_gauss_1d(fit_data_1, x_data_1, true_parameters_1);
    generate_gauss_1d(fit_data_2, x_data_2, true_parameters_2);

    std::array< REAL, n_points * n_fits> data{};
    std::array< REAL, n_points * n_fits> x_data{};

    for (int i = 0; i < n_points; i++)
    {
        data[i] = fit_data_1[i];
        data[n_points + i] = fit_data_2[i];

        x_data[i] = x_data_1[i];
        x_data[n_points + i] = x_data_2[i];
    }

    std::array< REAL, n_parameters * n_fits> initial_parameters{};

    for (int i = 0; i < n_parameters; i++)
    {
        initial_parameters[i] = initial_parameters_1[i];
        initial_parameters[n_parameters + i] = initial_parameters_2[i];
    }

    REAL tolerance{ 1e-6f };

    int max_n_iterations{ 20 };

    std::array< int, n_parameters > parameters_to_fit{ { 1, 1, 1, 1 } };

    std::array< REAL, n_parameters * n_fits > output_parameters;
    std::array< int, n_fits > output_states;
    std::array< REAL, n_fits > output_chi_square;
    std::array< int, n_fits > output_n_iterations;

    int const status
        = gpufit
        (
            n_fits,
            n_points,
            data.data(),
            0,
            GAUSS_1D,
            initial_parameters.data(),
            tolerance,
            max_n_iterations,
            parameters_to_fit.data(),
            LSE,
            n_points * n_fits * sizeof(REAL),
            reinterpret_cast< char * >(x_data.data()),
            output_parameters.data(),
            output_states.data(),
            output_chi_square.data(),
            output_n_iterations.data()
        );
    // check gpufit status
    BOOST_CHECK(status == 0);

    // check first fit
    BOOST_CHECK(output_states[0] == 0);
    BOOST_CHECK(output_chi_square[0] < 1e-6f);
    BOOST_CHECK(output_n_iterations[0] <= max_n_iterations);

    BOOST_CHECK(std::abs(output_parameters[0] - true_parameters_1[0]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[1] - true_parameters_1[1]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[2] - true_parameters_1[2]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[3] - true_parameters_1[3]) < 1e-6f);

    // check second fit
    BOOST_CHECK(output_states[1] == 0);
    BOOST_CHECK(output_chi_square[1] < 1e-6f);
    BOOST_CHECK(output_n_iterations[1] <= max_n_iterations);

    BOOST_CHECK(std::abs(output_parameters[4] - true_parameters_2[0]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[5] - true_parameters_2[1]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[6] - true_parameters_2[2]) < 1e-6f);
    BOOST_CHECK(std::abs(output_parameters[7] - true_parameters_2[3]) < 1e-6f);
}

BOOST_AUTO_TEST_CASE( Gauss_Fit_1D )
{
    // single 1d gauss fit
    gauss_fit_1d();

    // two gauss fits with custom x coordinate values
    gauss_fit_1d_custom_x();
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::TensorMap;

static void test_additions()
{
  Tensor<std::complex<float>, 1> data1(3);
  Tensor<std::complex<float>, 1> data2(3);
  for (int i = 0; i < 3; ++i) {
    data1(i) = std::complex<float>(i, -i);
    data2(i) = std::complex<float>(i, 7 * i);
  }

  Tensor<std::complex<float>, 1> sum = data1 + data2;
  for (int i = 0; i < 3; ++i) {
    VERIFY_IS_EQUAL(sum(i),  std::complex<float>(2*i, 6*i));
  }
}

static void test_abs()
{
  Tensor<std::complex<float>, 1> data1(3);
  Tensor<std::complex<double>, 1> data2(3);
  data1.setRandom();
  data2.setRandom();

  Tensor<float, 1> abs1 = data1.abs();
  Tensor<double, 1> abs2 = data2.abs();
  for (int i = 0; i < 3; ++i) {
    VERIFY_IS_APPROX(abs1(i), std::abs(data1(i)));
    VERIFY_IS_APPROX(abs2(i), std::abs(data2(i)));
  }
}

static void test_conjugate()
{
  Tensor<std::complex<float>, 1> data1(3);
  Tensor<std::complex<double>, 1> data2(3);
  Tensor<int, 1> data3(3);
  data1.setRandom();
  data2.setRandom();
  data3.setRandom();

  Tensor<std::complex<float>, 1> conj1 = data1.conjugate();
  Tensor<std::complex<double>, 1> conj2 = data2.conjugate();
  Tensor<int, 1> conj3 = data3.conjugate();
  for (int i = 0; i < 3; ++i) {
    VERIFY_IS_APPROX(conj1(i), std::conj(data1(i)));
    VERIFY_IS_APPROX(conj2(i), std::conj(data2(i)));
    VERIFY_IS_APPROX(conj3(i), data3(i));
  }
}

static void test_contractions()
{
  Tensor<std::complex<float>, 4> t_left(30, 50, 8, 31);
  Tensor<std::complex<float>, 5> t_right(8, 31, 7, 20, 10);
  Tensor<std::complex<float>, 5> t_result(30, 50, 7, 20, 10);

  t_left.setRandom();
  t_right.setRandom();

  typedef Map<Matrix<std::complex<float>, Dynamic, Dynamic>> MapXcf;
  MapXcf m_left(t_left.data(), 1500, 248);
  MapXcf m_right(t_right.data(), 248, 1400);
  Matrix<std::complex<float>, Dynamic, Dynamic> m_result(1500, 1400);

  // This contraction should be equivalent to a regular matrix multiplication
  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 2> dims;
  dims[0] = DimPair(2, 0);
  dims[1] = DimPair(3, 1);
  t_result = t_left.contract(t_right, dims);
  m_result = m_left * m_right;
  for (int i = 0; i < t_result.dimensions().TotalSize(); i++) {
    VERIFY_IS_APPROX(t_result.data()[i], m_result.data()[i]);
  }
}

EIGEN_DECLARE_TEST(cxx11_tensor_of_complex)
{
  CALL_SUBTEST(test_additions());
  CALL_SUBTEST(test_abs());
  CALL_SUBTEST(test_conjugate());
  CALL_SUBTEST(test_contractions());
}


#pragma once

#include <vector>

#include <boost/variant/recursive_variant.hpp>

struct Constant
{
    std::wstring name;
};

bool operator==(const Constant& c1, const Constant& c2);

struct Value
{
    double value;
};

bool operator==(const Value& v1, const Value& v2);

struct Variable
{
    std::wstring name;
};

bool operator==(const Variable& v1, const Variable& v2);

// struct Monomial
// {
//     double multiplier;
//     Variable variable;
//     int exponant = 1;
// };

struct Sum;
struct Product;
struct Cos;
struct Sin;

using Expression
    = boost::variant<Constant, Value, Variable, boost::recursive_wrapper<Sum>, boost::recursive_wrapper<Product>,
                     boost::recursive_wrapper<Cos>, boost::recursive_wrapper<Sin>>;

struct Sum
{
    std::vector<Expression> operands;
};

bool operator==(const Sum& s1, const Sum& s2);

struct Product
{
    std::vector<Expression> operands;
};

bool operator==(const Product& s1, const Product& s2);

struct Cos
{
    Expression expr;
};

bool operator==(const Cos& c1, const Cos& c2);

struct Sin
{
    Expression expr;
};

bool operator==(const Sin& s1, const Sin& s2);


// Adapted from: http://people.sc.fsu.edu/~jburkardt/m_src/shallow_water_2d/
// Saved images may be converted into an animated gif with:
// convert   -delay 20   -loop 0   swater*.png   swater.gif
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <bp_util.h>

#include <boost/multi_array.hpp>

int main (int argc, char **argv)
{
    bp_util_type bp = bp_util_create(argc, argv, 3);
    if (bp.args.has_error) {
        return 1;
    }
    const int n         = bp.args.sizes[0];
    const int other_n   = bp.args.sizes[1];
    const int T         = bp.args.sizes[2];

    if (n != other_n) {
        fprintf(stderr, "Implementation only supports quadratic grid.\n");
        exit(0);
    }

    const double g   = 9.8;       // gravitational constant
    const double dt  = 0.02;      // hardwired timestep
    const double dx  = 1.0;
    const double dy  = 1.0;
    const int droploc = n/4;

    typedef boost::multi_array<double, 2> array_type;

    array_type H(boost::extents[n+2][n+2]);
    array_type U(boost::extents[n+2][n+2]);
    array_type V(boost::extents[n+2][n+2]);

    array_type Hx(boost::extents[n+1][n+1]);
    array_type Ux(boost::extents[n+1][n+1]);
    array_type Vx(boost::extents[n+1][n+1]);
    array_type Hy(boost::extents[n+1][n+1]);
    array_type Uy(boost::extents[n+1][n+1]);
    array_type Vy(boost::extents[n+1][n+1]);

    for(int i=0; i<n+2; i++)
        for(int j=0; j<n+2; j++)
        {
          H[i][j]=0.1;
          U[i][j]=0.1;
          V[i][j]=0.1;
        }

    for(int i=0; i<n+1; i++)
        for(int j=0; j<n+1; j++)
        {
          Hx[i][j]=0.1;
          Ux[i][j]=0.1;
          Vx[i][j]=0.1;

          Hy[i][j]=0.1;
          Uy[i][j]=0.1;
          Vy[i][j]=0.1;
        }

    H[droploc][droploc] += 5.0;

    bp.timer_start();

    for(int iter=0; iter < T; iter++)
    {
        // Reflecting boundary conditions
        for(int i=0; i<n+2; i++)
        {
            H[i][0] = H[i][1]   ; U[i][0] = U[i][1]     ; V[i][0] = -V[i][1];
            H[i][n+1] = H[i][n] ; U[i][n+1] = U[i][n]   ; V[i][n+1] = -V[i][n];
            H[0][i] = H[1][i]   ; U[0][i] = -U[1][i]    ; V[0][i] = V[1][i];
            H[n+1][i] = H[n][i] ; U[n+1][i] = -U[n][i]  ; V[n+1][i] = V[n][i];
        }
        //
        // First half step
        //
        for(int i=0; i<n+1; i++)
        {
            for(int j=0; j<n; j++)
            {

                // height
                Hx[i][j] = (H[i+1][j+1]+H[i][j+1])/2 - dt/(2*dx)*(U[i+1][j+1]-U[i][j+1]);

                // x momentum
                Ux[i][j] = (U[i+1][j+1]+U[i][j+1])/2 -          \
                dt/(2*dx)*((pow(U[i+1][j+1],2)/H[i+1][j+1] +	\
                          g/2*pow(H[i+1][j+1],2)) -		\
                         (pow(U[i][j+1],2)/H[i][j+1] +	        \
                          g/2*pow(H[i][j+1],2)));

                // y momentum
                Vx[i][j] = (V[i+1][j+1]+V[i][j+1])/2 -          \
                      dt/(2*dx)*((U[i+1][j+1] *                 \
                                  V[i+1][j+1]/H[i+1][j+1]) -    \
                                 (U[i][j+1] *                   \
                                  V[i][j+1]/H[i][j+1]));
            }
        }

        for(int i=0; i<n; i++)
        {
            for(int j=0; j<n+1; j++)
            {
                //height
                Hy[i][j] = (H[i+1][j+1]+H[i+1][j])/2 - dt/(2*dy)*(V[i+1][j+1]-V[i+1][j]);

                //x momentum
                Uy[i][j] = (U[i+1][j+1]+U[i+1][j])/2 -	   \
                dt/(2*dy)*((V[i+1][j+1] *                  \
                           U[i+1][j+1]/H[i+1][j+1]) -	   \
                                (V[i+1][j] *               \
                                 U[i+1][j]/H[i+1][j]));

                //y momentum
                Vy[i][j] = (V[i+1][j+1]+V[i+1][j])/2 -	\
                dt/(2*dy)*((pow(V[i+1][j+1],2)/H[i+1][j+1] +	\
                           g/2*pow(H[i+1][j+1],2)) -	\
                          (pow(V[i+1][j],2)/H[i+1][j] +	\
                           g/2*pow(H[i+1][j],2)));
            }
        }
        //
        // Second half step
        //

        for(int i=1; i<n+1; i++)
        {
            for(int j=1; j<n+1; j++)
            {
                //height
                H[i][j] -= (dt/dx)*(Ux[i][j-1]-Ux[i-1][j-1]) - (dt/dy)*(Vy[i-1][j]-Vy[i-1][j-1]);

                // x momentum
                U[i][j] -= (dt/dx)*((pow(Ux[i][j-1],2)/Hx[i][j-1] + g/2*pow(Hx[i][j-1],2)) -        \
                                    (pow(Ux[i-1][j-1],2)/Hx[i-1][j-1] + g/2*pow(Hx[i-1][j-1],2))) - \
                           (dt/dy)*((Vy[i-1][j] * Uy[i-1][j]/Hy[i-1][j]) -
                                    (Vy[i-1][j-1] * Uy[i-1][j-1]/Hy[i-1][j-1]));

                // y momentum    - score
                V[i][j] -= (dt/dx)*((Ux[i][j-1] * Vx[i][j-1]/Hx[i][j-1]) -                         \
                                    (Ux[i-1][j-1]*Vx[i-1][j-1]/Hx[i-1][j-1])) -                    \
                           (dt/dy)*((pow(Vy[i-1][j],2)/Hy[i-1][j] + g/2*pow(Hy[i-1][j],2)) -       \
                                    (pow(Vy[i-1][j-1],2)/Hy[i-1][j-1] + g/2*pow(Hy[i-1][j-1],2)));

            }
        }
    }

    // res = numpy.add.reduce(numpy.add.reduce(H / n))
    double res = 0.0;
    for(int i=0;i<n+2;i++)
        for(int j=0;j<n+2;j++)
            res+=H[i][j]/n;

    bp.timer_stop();
    bp.print("shallow_water(cpp11_boost)");
}


//  (C) Copyright Eric Niebler 2005.
//  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)

// Test case for pot_quantile.hpp

#define BOOST_NUMERIC_FUNCTIONAL_STD_COMPLEX_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VALARRAY_SUPPORT
#define BOOST_NUMERIC_FUNCTIONAL_STD_VECTOR_SUPPORT

#include <boost/random.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/peaks_over_threshold.hpp>

using namespace boost;
using namespace unit_test;
using namespace boost::accumulators;

///////////////////////////////////////////////////////////////////////////////
// test_stat
//
void test_stat()
{
    // tolerance in %
    double epsilon = 1.;

    // two random number generators
    boost::lagged_fibonacci607 rng;
    boost::normal_distribution<> mean_sigma(0,1);
    boost::exponential_distribution<> lambda(1);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::normal_distribution<> > normal(rng, mean_sigma);
    boost::variate_generator<boost::lagged_fibonacci607&, boost::exponential_distribution<> > exponential(rng, lambda);

    accumulator_set<double, stats<tag::pot_quantile<right>(with_threshold_value)> > acc1(
        pot_threshold_value = 3.
    );
    accumulator_set<double, stats<tag::pot_quantile<right>(with_threshold_probability)> > acc2(
        right_tail_cache_size = 2000
      , pot_threshold_probability = 0.99
    );
    accumulator_set<double, stats<tag::pot_quantile<left>(with_threshold_value)> > acc3(
        pot_threshold_value = -3.
    );
    accumulator_set<double, stats<tag::pot_quantile<left>(with_threshold_probability)> > acc4(
        left_tail_cache_size = 2000
      , pot_threshold_probability = 0.01
    );

    accumulator_set<double, stats<tag::pot_quantile<right>(with_threshold_value)> > acc5(
        pot_threshold_value = 5.
    );
    accumulator_set<double, stats<tag::pot_quantile<right>(with_threshold_probability)> > acc6(
        right_tail_cache_size = 2000
      , pot_threshold_probability = 0.995
    );

    for (std::size_t i = 0; i < 100000; ++i)
    {
        double sample = normal();
        acc1(sample);
        acc2(sample);
        acc3(sample);
        acc4(sample);
    }

    for (std::size_t i = 0; i < 100000; ++i)
    {
        double sample = exponential();
        acc5(sample);
        acc6(sample);
    }

    BOOST_CHECK_CLOSE( quantile(acc1, quantile_probability = 0.999), 3.090232, 3*epsilon );
    BOOST_CHECK_CLOSE( quantile(acc2, quantile_probability = 0.999), 3.090232, 2*epsilon );
    BOOST_CHECK_CLOSE( quantile(acc3, quantile_probability = 0.001), -3.090232, 2*epsilon );
    BOOST_CHECK_CLOSE( quantile(acc4, quantile_probability = 0.001), -3.090232, 2*epsilon );

    BOOST_CHECK_CLOSE( quantile(acc5, quantile_probability = 0.999), 6.908, 3*epsilon );
    BOOST_CHECK_CLOSE( quantile(acc6, quantile_probability = 0.999), 6.908, 3*epsilon );
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("pot_quantile test");

    test->add(BOOST_TEST_CASE(&test_stat));

    return test;
}


#include <CGAL/Cartesian.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/determinant.h>

#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>               Traits;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>                Triangulation;
typedef Triangulation::Face_iterator                                                Face_iterator;
typedef Triangulation::Vertex_handle                                                Vertex_handle;
typedef Triangulation::Point                                                        Point;
typedef Traits::Side_of_original_octagon                                            Side_of_original_octagon;
typedef CGAL::Cartesian<double>::Point_2                                            Point_double;
typedef CGAL::Creator_uniform_2<double, Point_double >                              Creator;

int main(int argc, char** argv)
{
  int iter;
  if(argc < 2)
  {
    std::cout << "usage: " << argv[0] << " [number_of_iterations]" << std::endl;
    std::cout << "defaulting to 10 iterations..." << std::endl;
    iter = 10;
  } else {
    iter = atoi(argv[1]);
  }

  Side_of_original_octagon pred;

  int N = 500;
  int min = 2*N;
  int max = -1;
  double mean = 0.0;
  for(int j=0; j<iter; ++j)
  {
    std::vector<Point_double> v;
    CGAL::Random_points_in_disc_2<Point_double, Creator> g(0.85);

    Triangulation tr;
    assert(tr.is_valid(true));

    int cnt = 0;
    int idx = 0;
    do
    {
      Point_double pt = *(++g);
      if(pred(pt) != CGAL::ON_UNBOUNDED_SIDE)
      {
        tr.insert(Point(pt.x(), pt.y()));
        cnt++;
      }
    }
    while(tr.number_of_dummy_points() > 0 && idx < N-1);

    if(tr.number_of_dummy_points() > 0)
    {
      std::cout << "!!! FAILED to remove all dummy points after the insertion of " << N << " random points!" << std::endl;
      continue;
    }

    assert(tr.is_valid());
    std::cout << cnt << std::endl;

    if(cnt > max)
      max = cnt;

    if(cnt < min)
      min = cnt;

    mean += cnt;
  }
  mean /= double(iter);

  std::cout << "Finished " << iter << " iterations!" << std::endl;
  std::cout << "Minimum number of points inserted: " << min << std::endl;
  std::cout << "Maximum number of points inserted: " << max << std::endl;
  std::cout << "Average number of points inserted: " << mean << std::endl;

  return EXIT_SUCCESS;
}


/* boost random/gamma_distribution.hpp header file
 *
 * Copyright Jens Maurer 2002
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: gamma_distribution.hpp,v 1.7 2002/12/22 22:03:10 jmaurer Exp $
 *
 */

#ifndef BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP
#define BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP

#include <cmath>
#include <cassert>
#include <boost/random/uniform_01.hpp>

namespace boost {

// Knuth
// deterministic polar method, uses trigonometric functions
template<class UniformRandomNumberGenerator, class RealType = double,
         class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class gamma_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  explicit gamma_distribution(base_type & rng,
                              const result_type& alpha = result_type(1))
    : _rng(rng), _exp(rng, result_type(1)), _alpha(alpha)
  {
    assert(alpha > result_type(0));
    init();
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng; }
  base_type& base() const { return _rng.base(); }
  RealType alpha() const { return _alpha; }

  void reset() { _rng.reset(); _exp.reset(); }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::tan; using std::sqrt; using std::exp; using std::log;
    using std::pow;
#endif
    if(_alpha == result_type(1)) {
      return _exp();
    } else if(_alpha > result_type(1)) {
      // Can we have a boost::mathconst please?
      const result_type pi = result_type(3.14159265358979323846);
      for(;;) {
        result_type y = tan(pi * _rng());
        result_type x = sqrt(result_type(2)*_alpha-result_type(1))*y
          + _alpha-result_type(1);
        if(x <= result_type(0))
          continue;
        if(_rng() >
           (result_type(1)+y*y) * exp((_alpha-result_type(1))
                                        *log(x/(_alpha-result_type(1)))
                                        - sqrt(result_type(2)*_alpha
                                               -result_type(1))*y))
          continue;
        return x;
      }
    } else /* alpha < 1.0 */ {
      for(;;) {
        result_type u = _rng();
        result_type y = _exp();
        result_type x, q;
        if(u < _p) {
          x = exp(-y/_alpha);
          q = _p*exp(-x);
        } else {
          x = result_type(1)+y;
          q = _p + (result_type(1)-_p) * pow(x, _alpha-result_type(1));
        }
        if(u >= q)
          continue;
        return x;
      }
    }
  }
#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const gamma_distribution& x,
                         const gamma_distribution& y)
  {
    return x._alpha == y._alpha && x._rng == y._rng;
  }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const gamma_distribution& gd)
  {
    os << gd._alpha;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, gamma_distribution& gd)
  {
    is >> std::ws >> gd._alpha;
    gd.init();
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const gamma_distribution& rhs) const
  {
    return _alpha == rhs._alpha && _rng == rhs._rng;
  }
#endif

private:
  void init()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp;
#endif
    _p = exp(result_type(1)) / (_alpha + exp(result_type(1)));
  }

  adaptor_type _rng;
  exponential_distribution<base_type, RealType, Adaptor> _exp;
  result_type _alpha;
  // some data precomputed from the parameters
  result_type _p;
};

} // namespace boost

#endif // BOOST_RANDOM_GAMMA_DISTRIBUTION_HPP


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-exponential
    This function object returns the exponential of its argument minus one:\f$e^{x}-1\f$

    @par Header <boost/simd/function/expm1.hpp>

    @par Notes

    - result is accurate even for @c x of small modulus

    @par Decorators

     - std_ for floating entries calls @c std::expm1

    @see exp

    @par Example:

      @snippet expm1.cpp expm1

    @par Possible output:

      @snippet expm1.txt expm1

  **/
  IEEEValue expm1(IEEEValue const& x);
} }
#endif

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

#endif


#define BOOST_TEST_MODULE TestrocFFT
#include "libraries/rocfft/rocfft_helper.hpp"

#include <hip/hip_runtime_api.h>
#include <hip/hip_vector_types.h>
#include <rocfft.h>

#include <boost/test/included/unit_test.hpp> // Single-header usage variant

#include <cstdint>

#include <iostream>
#include <vector>
#include <complex>

BOOST_AUTO_TEST_CASE( FFT1DSmall, * boost::unit_test::tolerance(0.0001f) )
{
  // rocFFT gpu compute
        // ========================================

        CHECK_HIP(rocfft_setup());

        std::size_t N = 2 << 10; //should be small enough to not need a work_buffer
        std::size_t Nbytes = N * sizeof(float2);

        // Create HIP device buffer
        float2 *x;
        CHECK_HIP(hipMalloc(&x, Nbytes));
        // Initialize data
        std::vector<float2> cx(N);
        for (std::size_t i = 0; i < N; i++)
        {
                cx[i].x = 1;
                cx[i].y = -1;
        }

        //  Copy data to device
        CHECK_HIP(hipMemcpy(x, cx.data(), Nbytes, hipMemcpyHostToDevice));
        // Create fwd rocFFT fwd
        rocfft_plan fwd = nullptr;
        std::size_t length = N;
        CHECK_HIP(
          rocfft_plan_create(&fwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_forward,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Create bwd rocFFT bwd
        rocfft_plan bwd = nullptr;
        CHECK_HIP(
          rocfft_plan_create(&bwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_inverse,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Execute fwd
        CHECK_HIP(rocfft_execute(fwd, (void**) &x, nullptr, nullptr));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy fwd
        CHECK_HIP(rocfft_plan_destroy(fwd));

        // Execute bwd
        CHECK_HIP(rocfft_execute(bwd, (void**) &x, nullptr, nullptr));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy bwd
        CHECK_HIP(rocfft_plan_destroy(bwd));

        // Copy result back to host
        std::vector<float2> y(N);
        CHECK_HIP(hipMemcpy(y.data(), x, Nbytes, hipMemcpyDeviceToHost));

        // Print results
        for (std::size_t i = 0; i < N; i++)
        {
          y[i].x *= 1.f/N;
          y[i].y *= 1.f/N;

          BOOST_TEST( cx[i].x == y[i].x );
          BOOST_TEST( cx[i].y == y[i].y );
        }

        // Free device buffer
        CHECK_HIP(hipFree(x));

        CHECK_HIP(rocfft_cleanup());

}

BOOST_AUTO_TEST_CASE( FFT1D, * boost::unit_test::tolerance(0.0001f) )
{
        // rocFFT gpu compute
        // ========================================

        CHECK_HIP(rocfft_setup());

        std::size_t N = 128 << 10; //needs a work_buffer
        std::size_t Nbytes = N * sizeof(float2);

        // Create HIP device buffer
        float2 *x;
        CHECK_HIP(hipMalloc(&x, Nbytes));
        // Initialize data
        std::vector<float2> cx(N);
        for (std::size_t i = 0; i < N; i++)
        {
                cx[i].x = 1;
                cx[i].y = -1;
        }

        //  Copy data to device
        CHECK_HIP(hipMemcpy(x, cx.data(), Nbytes, hipMemcpyHostToDevice));
        // Create fwd rocFFT fwd
        rocfft_plan fwd = nullptr;
        std::size_t length = N;
        CHECK_HIP(
          rocfft_plan_create(&fwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_forward,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Create bwd rocFFT bwd
        rocfft_plan bwd = nullptr;
        CHECK_HIP(
          rocfft_plan_create(&bwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_inverse,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Setup work buffer
        void *workBuffer = nullptr;
        size_t workBufferSize_fwd = 0;
        CHECK_HIP(rocfft_plan_get_work_buffer_size(fwd, &workBufferSize_fwd));
        size_t workBufferSize_bwd = 0;
        CHECK_HIP(rocfft_plan_get_work_buffer_size(bwd, &workBufferSize_bwd));

        BOOST_REQUIRE_EQUAL(workBufferSize_bwd,workBufferSize_fwd);

        // Setup exec info to pass work buffer to the library
        rocfft_execution_info info = nullptr;
        CHECK_HIP(rocfft_execution_info_create(&info));

        if(workBufferSize_fwd > 0)
        {
                CHECK_HIP(hipMalloc(&workBuffer, workBufferSize_fwd));
                CHECK_HIP(rocfft_execution_info_set_work_buffer(info, workBuffer, workBufferSize_fwd));
        }

        // Execute fwd
        CHECK_HIP(rocfft_execute(fwd, (void**) &x, nullptr, info));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy fwd
        CHECK_HIP(rocfft_plan_destroy(fwd));

        // Execute bwd
        CHECK_HIP(rocfft_execute(bwd, (void**) &x, nullptr, info));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy bwd
        CHECK_HIP(rocfft_plan_destroy(bwd));

        if(workBuffer)
  	     	CHECK_HIP(hipFree(workBuffer));

        CHECK_HIP(rocfft_execution_info_destroy(info));

        // Copy result back to host
        std::vector<float2> y(N);
        CHECK_HIP(hipMemcpy(y.data(), x, Nbytes, hipMemcpyDeviceToHost));

        // Print results
        for (std::size_t i = 0; i < N; i++)
        {
                y[i].x *= 1.f/N;
                y[i].y *= 1.f/N;

                BOOST_TEST( cx[i].x == y[i].x );
                BOOST_TEST( cx[i].y == y[i].y );
        }

        // Free device buffer
        CHECK_HIP(hipFree(x));

        CHECK_HIP(rocfft_cleanup());

}

BOOST_AUTO_TEST_CASE( FFT1DSmall_stdcomplex, * boost::unit_test::tolerance(0.0001f) )
{
  // rocFFT gpu compute
        // ========================================

        CHECK_HIP(rocfft_setup());

        std::size_t N = 2 << 10; //should be small enough to not need a work_buffer
        std::size_t Nbytes = N * sizeof(std::complex<float>);

        // Create HIP device buffer
        std::complex<float> *x;
        CHECK_HIP(hipMalloc(&x, Nbytes));
        // Initialize data
        std::vector<std::complex<float>> cx(N);
        for (std::size_t i = 0; i < N; i++)
        {
                cx[i].real( 1 );
                cx[i].imag( -1);
        }

        //  Copy data to device
        CHECK_HIP(hipMemcpy(x, cx.data(), Nbytes, hipMemcpyHostToDevice));
        // Create fwd rocFFT fwd
        rocfft_plan fwd = nullptr;
        std::size_t length = N;
        CHECK_HIP(
          rocfft_plan_create(&fwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_forward,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Create bwd rocFFT bwd
        rocfft_plan bwd = nullptr;
        CHECK_HIP(
          rocfft_plan_create(&bwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_complex_inverse,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Execute fwd
        CHECK_HIP(rocfft_execute(fwd, (void**) &x, nullptr, nullptr));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy fwd
        CHECK_HIP(rocfft_plan_destroy(fwd));

        // Execute bwd
        CHECK_HIP(rocfft_execute(bwd, (void**) &x, nullptr, nullptr));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy bwd
        CHECK_HIP(rocfft_plan_destroy(bwd));

        // Copy result back to host
        std::vector<std::complex<float>> y(N);
        CHECK_HIP(hipMemcpy(y.data(), x, Nbytes, hipMemcpyDeviceToHost));

        // Print results
        for (std::size_t i = 0; i < N; i++)
        {
          y[i].real ( y[i].real() * 1.f/N);
          y[i].imag ( y[i].imag() * 1.f/N);

          BOOST_TEST( cx[i].real() == y[i].real() );
          BOOST_TEST( cx[i].imag() == y[i].imag() );
        }

        // Free device buffer
        CHECK_HIP(hipFree(x));

        CHECK_HIP(rocfft_cleanup());

}

BOOST_AUTO_TEST_CASE( failing_float_265841_Inplace_Real, * boost::unit_test::tolerance(0.0001f) )
{
        // rocFFT gpu compute
        // ========================================

        CHECK_HIP(rocfft_setup());

        std::size_t N = 265841;
        std::size_t Nbytes = 2*(N/2 + 1) * sizeof(float);

        BOOST_REQUIRE_GT(Nbytes, N*sizeof(float));
        // Create HIP device buffer
        float *x;
        CHECK_HIP(hipMalloc(&x, Nbytes));
        // Initialize data
        std::vector<float> h_x(N,1.);

        //  Copy data to device
        CHECK_HIP(hipMemcpy(x, h_x.data(), Nbytes, hipMemcpyHostToDevice));
        // Create fwd rocFFT fwd
        rocfft_plan fwd = nullptr;
        std::size_t length = N;
        CHECK_HIP(
          rocfft_plan_create(&fwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_real_forward,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Create bwd rocFFT bwd
        rocfft_plan bwd = nullptr;
        CHECK_HIP(
          rocfft_plan_create(&bwd,
                             rocfft_placement_inplace,
                             rocfft_transform_type_real_inverse,
                             rocfft_precision_single,
                             1, &length, 1, nullptr));

        // Setup work buffer
        void *workBuffer = nullptr;
        size_t workBufferSize_fwd = 0;
        CHECK_HIP(rocfft_plan_get_work_buffer_size(fwd, &workBufferSize_fwd));
        size_t workBufferSize_bwd = 0;
        CHECK_HIP(rocfft_plan_get_work_buffer_size(bwd, &workBufferSize_bwd));

        BOOST_REQUIRE_EQUAL(workBufferSize_bwd,workBufferSize_fwd);

        // Setup exec info to pass work buffer to the library
        rocfft_execution_info info = nullptr;
        CHECK_HIP(rocfft_execution_info_create(&info));

        if(workBufferSize_fwd > 0)
        {
                CHECK_HIP(hipMalloc(&workBuffer, workBufferSize_fwd));
                CHECK_HIP(rocfft_execution_info_set_work_buffer(info, workBuffer, workBufferSize_fwd));
        }

        // Execute fwd
        CHECK_HIP(rocfft_execute(fwd, (void**) &x, nullptr, info));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy fwd
        CHECK_HIP(rocfft_plan_destroy(fwd));

        // Execute bwd
        CHECK_HIP(rocfft_execute(bwd, (void**) &x, nullptr, info));

        // Wait for execution to finish
        CHECK_HIP(hipDeviceSynchronize());

        // Destroy bwd
        CHECK_HIP(rocfft_plan_destroy(bwd));

        if(workBuffer)
  	     	CHECK_HIP(hipFree(workBuffer));

        CHECK_HIP(rocfft_execution_info_destroy(info));

        // Copy result back to host
        std::vector<float> h_y(N);
        CHECK_HIP(hipMemcpy(h_y.data(), x, Nbytes, hipMemcpyDeviceToHost));

        // Print results
        for (std::size_t i = 0; i < N; i++)
        {
                h_y[i]*= 1.f/N;

                BOOST_TEST( h_x[i] == h_y[i] );

        }

        // Free device buffer
        CHECK_HIP(hipFree(x));

        CHECK_HIP(rocfft_cleanup());

}


#define CATCH_CONFIG_MAIN
#include <Eigen/Dense>
#include <catch.hpp>
#include <random>

#include "EDP/ConstructSparseMat.hpp"
#include "EDP/LocalHamiltonian.hpp"

#include "yavque/Operators/SumLocalHamEvol.hpp"
#include "yavque/utils.hpp"

#include "common.hpp"

TEST_CASE("test sum of local hamiltonian", "[sum-local]")
{
	using namespace yavque;
	using namespace Eigen;

	constexpr unsigned int N = 8;
	constexpr cx_double I(0., 1.);

	std::random_device rd;
	std::default_random_engine re{rd()};
	std::normal_distribution<> nd;

	SparseMatrix<cx_double> m = pauli_x().cast<cx_double>();
	SumLocalHam ham(N, m);
	SumLocalHamEvol ham_evol(ham);
	auto var = ham_evol.get_variable();

	for(uint32_t k = 0; k < 100; ++k) // instance for loop
	{
		Eigen::VectorXcd ini = Eigen::VectorXcd::Random(1 << N);
		ini.normalize();

		double t = nd(re);
		var = t;
		VectorXcd out_test = ham_evol * ini;

		MatrixXcd mevol = (cos(t) * MatrixXcd::Identity(2, 2) - I * sin(t) * m);
		VectorXcd out = apply_kronecker(N, mevol, ini);

		REQUIRE((out - out_test).norm() < 1e-6);
	}

	ham_evol.dagger_in_place();
	for(uint32_t k = 0; k < 100; ++k) // instance for loop
	{
		Eigen::VectorXcd ini = Eigen::VectorXcd::Random(1 << N);
		ini.normalize();

		double t = nd(re);
		var = t;
		VectorXcd out_test = ham_evol * ini;

		MatrixXcd mevol = (cos(t) * MatrixXcd::Identity(2, 2) + I * sin(t) * m);
		VectorXcd out = apply_kronecker(N, mevol, ini);

		REQUIRE((out - out_test).norm() < 1e-6);
	}
}


#pragma once

#include <vector>
#include <tuple>
#include <array>
#include <Eigen/Dense>
#include <iod/symbols.hh>
#include <vpp/core/image2d.hh>
#include <vpp/core/vector.hh>
#include <vpp/core/make_array.hh>

namespace vpp
{

  template <typename T, typename U>
  void euclide_distance_transform(image2d<T>& input, image2d<U>& sedt)
  {
    image2d<vshort2> R(input.domain(), _border = 1);
    fill_with_border(R, vshort2{0,0});

    auto forward4 = [] () { return make_array(vint2{-1, -1}, vint2{-1, 0}, vint2{-1, 1}, vint2{0, -1}); };
    auto backward4 = [] () { return make_array(vint2{1, 1}, vint2{1, 0}, vint2{1, -1}, vint2{0, 1}); };

    fill_with_border(sedt, input.nrows() + input.ncols());
    pixel_wise(input, sedt) | [] (auto& i, auto& s) { if (i == 0) s = 0; };

    auto run = [&] (auto neighborhood, auto col_direction,
                    auto row_direction1, auto row_direction2, auto spn) {

      row_wise(sedt, R)(col_direction, _no_threads) | [&] (auto sedt_row, auto R_row)
      {
        // Forward pass
        pixel_wise(relative_access(sedt_row), relative_access(R_row))(row_direction1, _no_threads)
        | [&] (auto sedt_nbh, auto R_nbh)
        {
          vint2 min_rel_coord = neighborhood()[0];
          int min_dist = INT_MAX;
          for (vint2 nc : neighborhood())
          {
            int d = sedt_nbh(nc) + 2 * (std::abs(R_nbh(nc)[0] * nc[0]) +
                                        std::abs(R_nbh(nc)[1] * nc[1]))
              + nc.cwiseAbs().sum();

            if (d < min_dist)
            {
              min_dist = d;
              min_rel_coord = nc;
            }
          }

          if (min_dist < sedt_nbh(0, 0))
          {
            R_nbh(0, 0) = (R_nbh(min_rel_coord) + min_rel_coord.cast<short>()).template cast<short>();
            sedt_nbh(0, 0) = min_dist;
          }
        };

        // Backward pass
        pixel_wise(relative_access(sedt_row), relative_access(R_row))
        (row_direction2, _no_threads) | [&] (auto sedt_nbh, auto R_nbh)
        {
          int d = sedt_nbh(spn()) + 2 * std::abs(R_nbh(spn())[1]) + 1;
          if (d < sedt_nbh(0, 0))
          {
            sedt_nbh(0, 0) = d;
            R_nbh(0, 0) = (R_nbh(spn()) + spn().template cast<short>()).template cast<short>();
          }
        };

      };

    };

    run(forward4, _top_to_bottom, _left_to_right, _right_to_left, [] () { return vint2{0, 1}; });
    run(backward4, _bottom_to_top, _right_to_left, _left_to_right, [] () { return vint2{0, -1}; });

    // pixel_wise(sedt) | [] (auto& p) { p/=100; };
  }

  template <unsigned N, typename F>
  void loop_unroll(F f, std::enable_if_t<N == 0>* = 0) { f(N); }

  template <unsigned N, typename F>
  void loop_unroll(F f, std::enable_if_t<N != 0>* = 0) { f(N); loop_unroll<N-1>(f); }

  template <typename T, typename U, typename F, typename FW, typename B, typename BW, int WS = 3>
  void generic_incremental_distance_transform(image2d<T>& input, image2d<U>& sedt,
                                              F forward,
                                              FW forward_ws,
                                              B backward,
                                              BW backward_ws,
                                              std::integral_constant<int, WS> = std::integral_constant<int, WS>())
  {
    fill_with_border(sedt, input.nrows() + input.ncols());
    pixel_wise(input, sedt) | [] (auto& i, auto& s) { if (i == 0) s = 0; };

    auto run = [&] (auto neighb, auto ws,
                    auto col_direction,
                    auto row_direction) {
      pixel_wise(relative_access(sedt))(col_direction, row_direction, _no_threads) | [neighb, ws] (auto sn) {
        int min_dist = sn(0,0);

        auto nbh = neighb();

        // if (neighb().size() < 6)
        auto it = [&] (int i) {
          min_dist = std::min(min_dist, ws()[i] + sn(neighb()[i]));
        };

        typedef decltype(nbh) NBH;
        loop_unroll<std::tuple_size<NBH>::value - 1>(it);
        sn(0,0) = min_dist;
      };
    };

    run(forward, forward_ws, _top_to_bottom, _left_to_right);
    run(backward, backward_ws, _bottom_to_top, _right_to_left);
  }

  const auto d4_distance_transform = [] (auto& a, auto& b) {
    generic_incremental_distance_transform(a, b,
                                 [] () { return make_array(vint2{-1, 0}, vint2{0, -1}); },
                                 [] () { return make_array(1, 1); },
                                 [] () { return make_array(vint2{1, 0}, vint2{0, 1}); },
                                 [] () { return make_array(1, 1); });
  };

  const auto d8_distance_transform = [] (auto& a, auto& b) {
    generic_incremental_distance_transform(a, b,
                                 [] () { return make_array(vint2{-1, -1}, vint2{-1, 0}, vint2{-1, 1}, vint2{0, -1}); },
                                 [] () { return make_array(1,1,1,1); },
                                 [] () { return make_array(vint2{1, 1}, vint2{1, 0}, vint2{1, -1}, vint2{0, 1}); },
                                           [] () { return make_array(1, 1, 1, 1); });
  };

  const auto d3_4_distance_transform = [] (auto& a, auto& b) {
    generic_incremental_distance_transform(a, b,
                                           [] () { return make_array(vint2{-1, -1}, vint2{-1, 0}, vint2{-1, 1}, vint2{0, -1}); },
                                           [] () { return make_array(4,3,4,3); },
                                           [] () { return make_array(vint2{1, 1}, vint2{1, 0}, vint2{1, -1}, vint2{0, 1}); },
                                           [] () { return make_array(4, 3, 4, 3); }, std::integral_constant<int, 5>());
  };

  const auto d5_7_11_distance_transform = [] (auto& a, auto& b) {
    generic_incremental_distance_transform(a, b,

                                           [] () { return make_array(vint2{-2, -1}, vint2{-2, 1}, vint2{-1, -2}, vint2{-1, -1}, vint2{-1, 0}, vint2{-1, 1}, vint2{-1, 2}, vint2{0, -1}); },
                                           [] () { return make_array(11,11,11,7,5,7,11,5); },
                                           [] () { return make_array(vint2{0, 1}, vint2{1, -2}, vint2{1, -1}, vint2{1, 0}, vint2{1, 1}, vint2{1, 2}, vint2{2, -1}, vint2{2, 1}); },
                                           [] () { return make_array(5,11,7,5,7,11,11,11); },
                                           std::integral_constant<int, 5>());
  };

}


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    Distributed under the Boost Software License, Version 1.0. (See accompanying
    file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
#include <boost/config/warning_disable.hpp>
#include <input/parse_sexpr_impl.hpp>
#include <input/sexpr.hpp>
#include <input/parse_sexpr_impl.hpp>
#include <scheme/compiler.hpp>
#include <utree/io.hpp>

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int main()
{
    using scheme::interpreter;
    using scheme::function;
    using scheme::utree;

    utree src =
        "(define (factorial n) "
            "(if (<= n 0) 1 (* n (factorial (- n 1)))))";

    interpreter program(src);
    function factorial = program["factorial"];
    std::cout << factorial(10) << std::endl;

    return 0;
}


//
// Created by song on 1/26/18.
//

#include <opencv2/opencv.hpp>
#include <string>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace cv;

// this program shows how to use optical flow

string left_file = "./left.png";  // first image
string right_file = "./right.png";  // second image
string disparity_file = "./disparity.png" ;

double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
double baseline = 0.573;

// TODO implement this funciton
/**
 * single level optical flow
 * @param [in] left_img the first image
 * @param [in] right_img the second image
 * @param [in] kp1 keypoints in left_img
 * @param [in|out] kp2 keypoints in right_img, if empty, use initial guess in kp1
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse use inverse formulation?
 */
void OpticalFlowSingleLevel(
        const Mat &left_img,
        const Mat &right_img,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

// TODO implement this funciton
/**
 * multi level optical flow, scale of pyramid is set to 2 by default
 * the image pyramid will be create inside the function
 * @param [in] left_img the first pyramid
 * @param [in] right_img the second pyramid
 * @param [in] kp1 keypoints in left_img
 * @param [out] kp2 keypoints in right_img
 * @param [out] success true if a keypoint is tracked successfully
 * @param [in] inverse set true to enable inverse formulation
 */
void OpticalFlowMultiLevel(
        const Mat &left_img,
        const Mat &right_img,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse = false
);

/**
 * get a gray scale value from reference image (bi-linear interpolated)
 * @param img
 * @param x
 * @param y
 * @return
 */
inline float GetPixelValue(const cv::Mat &img, float x, float y) {
    uchar *data = &img.data[int(y) * img.step + int(x)];
    float xx = x - floor(x);
    float yy = y - floor(y);
    return float(
            (1 - xx) * (1 - yy) * data[0] +
            xx * (1 - yy) * data[1] +
            (1 - xx) * yy * data[img.step] +
            xx * yy * data[img.step + 1]
    );
}

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

    // images, note they are CV_8UC1, not CV_8UC3
    Mat left_img = imread(left_file, 0);
    Mat right_img = imread(right_file, 0);
    Mat disparity_img = imread(disparity_file, 0);

    // key points, using GFTT here.
    vector<KeyPoint> kp1;
    Ptr<GFTTDetector> detector = GFTTDetector::create(500, 0.01, 20); // maximum 500 keypoints
    detector->detect(left_img, kp1);

    // then test multi-level LK
    vector<KeyPoint> kp2;
    vector<bool> success;
    //TODO
    OpticalFlowMultiLevel(left_img, right_img, kp1, kp2, success);

    Mat left_img_show = left_img;
    cv::cvtColor(left_img, left_img_show, CV_GRAY2BGR);
    for (int i = 0; i < kp1.size(); i++) {

        cv::circle(left_img_show, kp1[i].pt, 2, cv::Scalar(0, 250, 0), 2);

    }
    // plot the differences of those functions
    Mat right_img_show;
    cv::cvtColor(right_img, right_img_show, CV_GRAY2BGR);
    for (int i = 0; i < kp2.size(); i++) {
        if (success[i]) {
            if(kp1[i].pt == kp2[i].pt) cout<<"==="<<endl;
            cv::circle(right_img_show, kp2[i].pt, 2, cv::Scalar(0, 250, 0), 2);
            cv::line(right_img_show, kp1[i].pt, kp2[i].pt, cv::Scalar(0, 250, 0));
        }
    }

    Mat match_img;
    vector<cv::DMatch> matches;
    for (int k =0; k <kp1.size();k++)
    {
        if(success[k])
        {
            cv::DMatch m;
            m.queryIdx=k;
            m.trainIdx=k;
            double disparity_ref = double(disparity_img.at<uchar>(kp1[k].pt.y,kp1[k].pt.x ));
            m.distance= float(abs(fx*baseline/(kp2[k].pt.x - kp1[k].pt.x) - fx*baseline /disparity_ref) );
            matches.push_back(m);

        }
    }

    // plot the matches
    cv::imshow("right_img_show", right_img_show);
    cv::imwrite("right_img_show.png", right_img_show);

    cv::drawMatches(left_img, kp1, right_img, kp2, matches, match_img);
    cv::imshow("matches", match_img);
    cv::imwrite("matches.png", match_img);

    cv::waitKey(0);

    double error = 0;
    int count = 0 ;
    for(auto m  : matches)
    {
        error+= m.distance;
        count ++;
    }
    cout<<"error: "<< error<<endl;
    cout<<"average error: "<<error/count<<endl;

    return 0;
}

void OpticalFlowSingleLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse
) {

    // parameters
    int half_patch_size = 10;
    int iterations = 10;
    bool have_initial = !kp2.empty();

    for (size_t i = 0; i < kp1.size(); i++) {
        auto kp = kp1[i];

        double dx = 0, dy = 0; // dx,dy need to be estimated
        if (have_initial) {
            dx = kp2[i].pt.x - kp.pt.x;
            dy = kp2[i].pt.y - kp.pt.y;
        }

        double cost = 0, lastCost = 0;
        bool succ = true; // indicate if this point succeeded

        // Gauss-Newton iterations
        for (int iter = 0; iter < iterations; iter++) {
            Eigen::Matrix2d H = Eigen::Matrix2d::Zero();
            Eigen::Vector2d b = Eigen::Vector2d::Zero();
            cost = 0;

            if (kp.pt.x + dx <= half_patch_size || kp.pt.x + dx >= img1.cols - half_patch_size ||
                kp.pt.y + dy <= half_patch_size || kp.pt.y + dy >= img1.rows - half_patch_size) {   // go outside
                succ = false;
                break;
            }

            // compute cost and jacobian
            for (int x = -half_patch_size; x < half_patch_size; x++)
                for (int y = -half_patch_size; y < half_patch_size; y++) {

                    // TODO START YOUR CODE HERE (~8 lines)

                    float x1 = kp.pt.x + x;
                    float y1 = kp.pt.y + y;
                    double error = GetPixelValue(img1,x1,y1)-GetPixelValue(img2,x1+dx,y1+dy);
                    Eigen::Vector2d J;  // Jacobian
                    if (inverse == false) {
                        // Forward Jacobian
                        J[0] = (GetPixelValue(img2,x1+dx+1,y1+dy) - GetPixelValue(img2,x1+dx-1,y1+dy))/2;
                        J[1] = (GetPixelValue(img2,x1+dx,y1+dy+1) - GetPixelValue(img2,x1+dx,y1+dy-1))/2;
                    } else {
                        // Inverse Jacobian
                        // NOTE this J does not change when dx, dy is updated, so we can store it and only compute error
                        J[0] = (GetPixelValue(img1,x1+1,y1) - GetPixelValue(img1,x1-1,y1))/2;
                        J[1] = (GetPixelValue(img1,x1,y1+1) - GetPixelValue(img1,x1,y1-1))/2;
                    }

                    // compute H, b and set cost;
                    // TODO END YOUR CODE HERE
                    H += J * J.transpose() ;
                    b += J.transpose()*error;
                    cost += error * error;
                }

            // compute update
            // TODO START YOUR CODE HERE (~1 lines)
            Eigen::Vector2d update = H.ldlt().solve(b);
            // TODO END YOUR CODE HERE

            if (isnan(update[0])) {
                // sometimes occurred when we have a black or white patch and H is irreversible
                cout << "update is nan" << endl;
                succ = false;
                break;
            }
            if (iter > 0 && cost > lastCost) {
                //cout << "cost increased: " << cost << ", " << lastCost << endl;
                break;
            }

            // update dx, dy
            dx += update[0];
            dy += update[1];
            lastCost = cost;
            succ = true;
        }

        success.push_back(succ);

        // set kp2
        if (have_initial) {
            kp2[i].pt = kp.pt + Point2f(dx, dy);
        } else {
            KeyPoint tracked = kp;
            tracked.pt += cv::Point2f(dx, dy);
            kp2.push_back(tracked);
        }
    }
}

void OpticalFlowMultiLevel(
        const Mat &img1,
        const Mat &img2,
        const vector<KeyPoint> &kp1,
        vector<KeyPoint> &kp2,
        vector<bool> &success,
        bool inverse) {

    // parameters
    int pyramids = 4;
    double pyramid_scale = 0.5;
    double scales[] = {1.0, 0.5, 0.25, 0.125};

    // create pyramids
    vector<Mat> pyr1, pyr2; // image pyramids
    // TODO START YOUR CODE HERE (~8 lines)
    for (int i = 0; i < pyramids; i++) {
        Mat uplevel1;
        Mat uplevel2;
        cv::resize(img1, uplevel1, cv::Size(), scales[i], scales[i]);
        cv::resize(img2, uplevel2, cv::Size(), scales[i], scales[i]);
        pyr1.push_back(uplevel1.clone());
        pyr2.push_back(uplevel2.clone());
    }
    // TODO END YOUR CODE HERE

    // coarse-to-fine LK tracking in pyramids
    // TODO START YOUR CODE HERE
    vector<KeyPoint> kp2_scale;
    for(int i = pyramids - 1; i >= 0; --i)
    {
        vector<KeyPoint> kp1_scale = kp1;
        for(int j = 0; j != kp1_scale.size(); ++j)
        {
            kp1_scale[j].pt.x *= scales[i];
            kp1_scale[j].pt.y *= scales[i];
        }

        for(int j = 0; j != kp2_scale.size(); ++j)
        {
            kp2_scale[j].pt.x *= scales[i];
            kp2_scale[j].pt.y *= scales[i];
        }

        OpticalFlowSingleLevel(pyr1[i], pyr2[i], kp1_scale, kp2_scale, success, inverse);

        for(int j = 0; j != kp2_scale.size(); ++j)
        {
            kp2_scale[j].pt.x /= scales[i];
            kp2_scale[j].pt.y /= scales[i];
        }
    }

    kp2 = kp2_scale;
    // TODO END YOUR CODE HERE
    // don't forget to set the results into kp2
}