#include <iostream> // std::cout, std::cerr
#include <cstdlib> // EXIT_SUCCESS, EXIT_FAILURE
#include <vector> // std::vector<>
#include <algorithm> // std::min(), std::max()
#include <cmath> // std::pow(), std::sqrt(), std::sin(), std::atan()
#include <exception> // std::exception
#include <string> // std::string, std::to_string()
#include <functional> // std::function<>, std::bind(), std::placeholders::_1
#include <limits> // std::numeric_limits<>
#include <map> // std::map<>

#include <boost/math/special_functions/sign.hpp> // boost::math::sign()
#include <boost/algorithm/string/predicate.hpp> // boost::iequals()
#include <boost/algorithm/string/join.hpp> // boost::algorithm::join()

#if defined(__gnu_linux__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#elif defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#endif

#include <boost/math/constants/constants.hpp> // boost::math::constants::pi<>()
#include <boost/program_options/options_description.hpp> // boost::program_options::options_description,
                                                         // boost::program_options::value<>
#include <boost/program_options/variables_map.hpp> // boost::program_options::variables_map,
                                                   // boost::program_options::store(),
                                                   // boost::program_options::notify()
#include <boost/program_options/parsers.hpp> // boost::program_options::cmd_line::parser
#include <boost/filesystem/operations.hpp> // boost::filesystem::exists()
#include <boost/filesystem/convenience.hpp> // boost::filesystem::change_extension()

#if defined(__gnu_linux__)
#pragma GCC diagnostic pop
#elif defined(__clang__)
#pragma clang diagnostic pop
#endif

#if defined(_WIN32)
#include <windows.h> // CONSOLE_SCREEN_BUFFER_INFO, GetConsoleScreenBufferInfo, GetStdHandle, STD_OUTPUT_HANDLE
#elif defined(__unix__)
#include <sys/ioctl.h> // struct winsize, ioctl(), TIOCGWINSZ
#endif

#include "ChanVeseCommon.hpp"        // ChanVese::
#include "InteractiveDataCirc.hpp"   // InteractiveDataCirc
#include "InteractiveDataRect.hpp"   // InteractiveDataRect
#include "VideoWriterManager.hpp"    // VideoWriterManager
#include "ParallelPixelFunction.hpp" // ParallelPixelFunction

                                     // Everything above comes with cv::

/**
 * @file
 * @todo
 * Algorithm-specific:
 *       - add level set reinitialization
 *           - some sources suggest that this avoids @em flattening of the zero-level set
 *       - consider other color spaces than RGB, e.g. YUV (experimental)
 *       - subpixel segmentation (experimental)
 *           - perform segmentation on an enlarged image, then scale back to original size
 *       - implicit instead of explicit scheme
 *           - should improve convergence rate (however, it might be the case that
 *             implicit scheme requires more computational power and thus levels off the gain
 *             in convergence rate)
 *           - might provide better numerical stability
 *           - needs preliminary analysis
 *       - determine proper stopping condition (needs preliminary analysis)
 *           - current stopping condition is rather arbitrary
 * Implementation-specific:
 *       - consider optional headless (i.e. non-GUI) build
 *           - would make the Qt dependency optional
 *       - drop the Makefile and switch to cmake
 *           - would provide a better compatibility with different platforms, compilers and
 *             build systems (e.g. ninja)
 *       - develop a method to test the algorithm
 *           - first, dig in the literature to see, whether there exist any well-accepted systematic
 *             methods to test an image segmentation algorithm
 *                - is there a universal/standard set of images (segmented and non-segmented)
 *                  to test with?
 *                - if so, how to assess the deviation from a perfectly segmented image?
 *                  @f$L_2@f$ norm of the difference over @f$L_2@f$ of the perfectly segmented image?
 *           - look for an automated solution
 *       - provide an interface to the algorithm
 *           - currently, the drawback is that the whole thing sits in the main() function
 *           - the greater picture here is that PM+CSV is supposed to be an intermediate step
 *             in a more robust image segmentation algorithm
 *                - one idea, for instance, is that the input parameters are found by analyzing
 *                  the original image
 *                - consider implementing a training algorithm as a possibility
 * @mainpage
 * @section intro_sec Introduction
 * This is the implementation of Perona-Malik + Chan-Sandberg-Vese segmentation algorithm in C++.
 * The premise of this code is that CSV segmentation is very sensitive to noise.
 * In order to get rid of the noise, we use Perona-Malik (which is optional) to smooth noisy
 * regions in the image. PM is optimal, because it preserves edges (unlike ordinary Gaussian blur).
 * The contour is calculated
 *
 * The article @cite Getreuer2012 is taken as a starting point in implementing CSV segmentation.
 * However, the text was short of describing vector-valued, i.e. multi-channel (RGB) images.
 * Fortunately, the original paper @cite Chan2000 proved to be useful.
 * PM segmentation is entirely based on the seminal paper @cite Perona1990.
 *
 * The code works for both grayscale and RGB images (any popular format is supported).
 * It mostly relies on OpenCV routines @cite Bradski2000. Some parallelization is done
 * across the channels with OpenMP preprocessors; one method is parallelized with TBB via
 * OpenCV interface.
 */

const cv::Scalar ChanVese::Colors::white   = CV_RGB(255, 255, 255);
const cv::Scalar ChanVese::Colors::black   = CV_RGB(  0,   0,   0);
const cv::Scalar ChanVese::Colors::red     = CV_RGB(255,   0,   0);
const cv::Scalar ChanVese::Colors::green   = CV_RGB(  0, 255,   0);
const cv::Scalar ChanVese::Colors::blue    = CV_RGB(  0,   0, 255);
const cv::Scalar ChanVese::Colors::magenta = CV_RGB(255,   0, 255);
const cv::Scalar ChanVese::Colors::yellow  = CV_RGB(255, 255,   0);
const cv::Scalar ChanVese::Colors::cyan    = CV_RGB(0,   255, 255);

const cv::Mat ChanVese::Kernel::fwd_x = (cv::Mat_<double>(1, 3) << 0,-1,1);
const cv::Mat ChanVese::Kernel::fwd_y = (cv::Mat_<double>(3, 1) << 0,-1,1);
const cv::Mat ChanVese::Kernel::bwd_x = (cv::Mat_<double>(1, 3) << -1,1,0);
const cv::Mat ChanVese::Kernel::bwd_y = (cv::Mat_<double>(3, 1) << -1,1,0);
const cv::Mat ChanVese::Kernel::ctr_x = (cv::Mat_<double>(1, 3) << -0.5,0,0.5);
const cv::Mat ChanVese::Kernel::ctr_y = (cv::Mat_<double>(3, 1) << -0.5,0,0.5);

/**
 * @brief Calculates the terminal/console width.
 *        Should work on all popular platforms.
 * @return Terminal width
 * @note Untested on Windows and MacOS.
 *       Credit to user 'quantum': http://stackoverflow.com/q/6812224/4056193
 */
int
get_terminal_width()
{
  int terminal_width = 80; // default
#if defined(_WIN32)
  CONSOLE_SCREEN_BUFFER_INFO csbi;
  GetConsoleScreenBufferInfo(GetStdHandle(STD_OUTPUT_HANDLE), &csbi);
  terminal_width = static_cast<int>(csbi.srWindow.Right -
                                    csbi.srWindow.Left  + 1);
#elif defined(unix) || defined(__unix__) || defined(__unix)
  struct winsize max;
  ioctl(0, TIOCGWINSZ, &max);
  terminal_width = static_cast<int>(max.ws_col);
#endif
  return terminal_width;
}

/**
 * @brief Adds suffix to the file name
 * @param path   Path to the file
 * @param suffix Suffix
 * @param delim  String separating the original base name and the suffix
 * @return New file name with the suffix
 */
std::string
add_suffix(const std::string & path,
           const std::string & suffix,
           const std::string & delim = "_")
{
  namespace fs = boost::filesystem;
  const fs::path p(path);
  const fs::path nw_p = p.parent_path() / fs::path(p.stem().string() + delim + suffix + p.extension().string());
  return nw_p.string();
}

/**
 * @brief Displays error message surrounded by newlines and exits.
 * @param msg Message to display.
*/
[[ noreturn ]] void
msg_exit(const std::string & msg)
{
  std::cerr << "\n" << msg << "\n\n";
  std::exit(EXIT_FAILURE);
}

/**
 * @brief Regularized (smoothed) Heaviside step function
 * @f[ H_\epsilon(x)=\frac{1}{2}\Big[1+\frac{2}{\pi}\arctan\Big(\frac{x}{\epsilon}\Big)\Big] @f]
 * where @f$x@f$ is the argument and @f$\epsilon@f$ the smoothing parameter
 * @param x   Argument of the step function, @f$x@f$
 * @param eps Smoothing parameter, @f$\epsilon@f$
 * @return Value of the step function at @f$x@f$
 */
double
regularized_heaviside(double x,
                      double eps = 1)
{
  const double pi = boost::math::constants::pi<double>();
  return (1 + 2 / pi * std::atan(x / eps)) / 2;
}

/**
 * @brief Regularized (smoothed) Dirac delta function
 * @f[ \delta_\epsilon(x)=\frac{\epsilon}{\pi(\epsilon^2+x^2)}\,, @f]
 * where @f$x@f$ is the argument and @f$\epsilon@f$ the smoothing parameter
 * @param x   Argument of the delta function, @f$x@f$
 * @param eps Smoothing parameter, @f$\epsilon@f$
 * @return Value of the delta function at @f$x@f$
 */
constexpr double
regularized_delta(double x,
                  double eps = 1)
{
  const double pi = boost::math::constants::pi<double>();
  return eps / (pi * (std::pow(eps, 2) + std::pow(x, 2)));
}

/**
 * @brief Creates a level set with a checkerboard pattern at zero level
 *        The zero level set is found via the formula
 *        @f[ \mathrm{sign}\Big[\sin\Big(\frac{x}{5}\Big)\sin\Big(\frac{y}{5}\Big)\Big]\,, @f]
 *        where @f$x@f$ and @f$y@f$ are the positions in the image
 * @param w Width of the level set matrix
 * @param h Height of the level set matrix
 * @return The levelset
 */
cv::Mat
levelset_checkerboard(int h,
                      int w)
{
  cv::Mat u(h, w, CV_64FC1);
  const double pi = boost::math::constants::pi<double>();
  double * const u_ptr = reinterpret_cast<double *>(u.data);
  for(int i = 0; i < h; ++i)
    for(int j = 0; j < w; ++j)
      u_ptr[i * w + j] = (boost::math::sign(std::sin(pi * i / 5) *
                                            std::sin(pi * j / 5)));
  return u;
}

/**
 * @brief Calculates average regional variance
 * @f[ c_i = \frac{\int_\Omega I_i(x,y)g(u(x,y))\mathrm{d}x\mathrm{d}y}{
                   \int_\Omega g(u(x,y))\mathrm{d}x\mathrm{d}y}\,, @f]
 * where @f$u(x,y)@f$ is the level set function,
 * @f$I_i@f$ is the @f$i@f$-th channel in the image and
 * @f$g@f$ is either the Heaviside function @f$H(x)@f$
 * (for region encolosed by the contour) or @f$1-H(x)@f$ (for region outside
 * the contour).
 * @param img       Input image (channel), @f$I_i(x,y)@f$
 * @param u         Level set, @f$u(x,y)@f$
 * @param h         Height of the image
 * @param w         Width of the image
 * @param region    Region either inside or outside the contour
 * @param heaviside Heaviside function, @f$H(x)@f$
 *                  One might also try different regularized heaviside functions
 *                  or even a non-smoothed one; that's why we've left it as a parameter
 * @return          Average variance of the given region in the image
 * @sa variance_penalty, Region
 */
double
region_variance(const cv::Mat & img,
                const cv::Mat & u,
                int h,
                int w,
                ChanVese::Region region,
                std::function<double(double)> heaviside)
{
  double nom = 0.0,
         denom = 0.0;
  const auto H = (region == ChanVese::Region::Inside)
                  ? heaviside
                  : [&heaviside](double x) -> double { return 1 - heaviside(x); };

  const double * const u_ptr = reinterpret_cast<double *>(u.data);
  const uchar * const img_ptr = img.data;

  for(int i = 0; i < h; ++i)
    for(int j = 0; j < w; ++j)
    {
      const double h = H(u_ptr[i * w + j]);
      nom += img_ptr[i * w + j] * h;
      denom += h;
    }

  return nom / denom;
}

/**
 * @brief Calculates variance penalty matrix,
 * @f[ \lambda_i\int_\Omega|I_i(x,y)-c_i|^2 g(u(x,y))\,\mathrm{d}x\mathrm{d}y\,, @f]
 * where @f$u(x,y)@f$ is the level set function,
 * @f$I_i@f$ is the @f$i@f$-th channel in the image and
 * @f$g@f$ is either the Heaviside function @f$H(x)@f$
 * (for region encolosed by the contour) or @f$1-H(x)@f$ (for region outside
 * the contour).
 * @param channel Channel of the input image, @f$I_i(x,y)@f$
 * @param h       Height of the image
 * @param w       Width of the image
 * @param c       Variance of particular region in the image, @f$c_i@f$
 * @param lambda  Penalty parameter, @f$\lambda_i@f$
 * @return Variance penalty matrix
 * @sa region_variance
 */
cv::Mat
variance_penalty(const cv::Mat & channel,
                 int h,
                 int w,
                 double c,
                 double lambda)
{
  cv::Mat channel_term(cv::Mat::zeros(h, w, CV_64FC1));
  channel.convertTo(channel_term, channel_term.type());
  channel_term -= c;
  cv::pow(channel_term, 2, channel_term);
  channel_term *= lambda;
  return channel_term;
}

/**
 * @brief Calculates the curvature (divergence of normalized gradient)
 *        of the level set:
 *        @f[
 *        \kappa=
 * \Delta_x^-\left(\frac{\Delta_x^+u_{i,j}}
 * {\sqrt{\eta^2+(\Delta_x^+u_{i,j})^2+(\Delta_y^0u_{i,j})^2}}\right)+
 * \Delta_y^-\left(\frac{\Delta_y^+u_{i,j}}
 * {\sqrt{\eta^2+(\Delta_x^0u_{i,j})^2+(\Delta_y^+u_{i,j})^2}}\right)\,,
 *        @f]
 * where
 *   - @f$ \Delta_x^{\pm} @f$ and @f$ \Delta_y^{\pm} @f$ correspond to forward (@f$+@f$)
 *     and backward (@f$-@f$) difference in @f$x@f$ and @f$y@f$ direction, respectively
 *   - @f$\Delta_x^0@f$ and @f$\Delta_y^0@f$ correspond to central differences in
 *     @f$x@f$ and @f$y@f$ direction, respectively
 *   - @f$\eta@f$ is a small parameter to avoid division by zero
 *   - @f$u_{i,j}@f$ is the level set for @f$m\times n@f$ image
 * The curvature is calculated by convoluting forward, backward and central difference
 * kernels with the level set. The method assumes duplicating the pixels near the border:
 * @f[ u_{-1,j}=u_{0,j}\,,\quad u_{m,j}=u_{m-1,j}\,,\quad
 *     u_{i,-1}=u_{i,0}\,,\quad u_{i,n}=u_{n-1,j}\,. @f]
 * This method ensures that the curvature is centered at a given point and only one
 * extra pixel is needed per calculation.
 * @param u       The level set, @f$u_{i,j}@f$
 * @param h       Height of the level set matrix
 * @param w       Width of the level set matrix
 * @return Curvature
 */
cv::Mat
curvature(const cv::Mat & u,
          int h,
          int w)
{
  const double eta = 1E-8;
  const double eta2 = std::pow(eta, 2);
  cv::Mat upx (h, w, CV_64FC1), upy (h, w, CV_64FC1),
          ucx (h, w, CV_64FC1), ucy (h, w, CV_64FC1);
  cv::filter2D(u, upx, CV_64FC1, ChanVese::Kernel::fwd_x, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
  cv::filter2D(u, upy, CV_64FC1, ChanVese::Kernel::fwd_y, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
  cv::filter2D(u, ucx, CV_64FC1, ChanVese::Kernel::ctr_x, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
  cv::filter2D(u, ucy, CV_64FC1, ChanVese::Kernel::ctr_y, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);

  double * const upx_ptr = reinterpret_cast<double *>(upx.data);
  double * const upy_ptr = reinterpret_cast<double *>(upy.data);
  const double * const ucx_ptr = reinterpret_cast<double *>(ucx.data);
  const double * const ucy_ptr = reinterpret_cast<double *>(ucy.data);

#pragma omp parallel for num_threads(NUM_THREADS)
  for(int i = 0; i < h; ++i)
    for(int j = 0; j < w; ++j)
      {
        upx_ptr[i * w + j] = upx_ptr[i * w + j] /
                             std::sqrt(std::pow(upx_ptr[i * w + j], 2) + std::pow(ucx_ptr[i * w + j], 2) + eta2);
        upy_ptr[i * w + j] = upy_ptr[i * w + j] /
                             std::sqrt(std::pow(upy_ptr[i * w + j], 2) + std::pow(ucy_ptr[i * w + j], 2) + eta2);
      }

  cv::filter2D(upx, upx, CV_64FC1, ChanVese::Kernel::bwd_x, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
  cv::filter2D(upy, upy, CV_64FC1, ChanVese::Kernel::bwd_y, cv::Point(-1, -1), 0, cv::BORDER_REPLICATE);
  upx += upy;
  return upx;
}

/**
 * @brief Separates the region enclosed by the contour in the image
 * @param img    Original image
 * @param u      Level set (the zero level of which gives us the region)
 * @param h      Height of the image
 * @param w      Width of the image
 * @param invert Invert the selected region
 * @return Image with a white background and the selected object(s) in the foreground
 */
cv::Mat
separate(const cv::Mat & img,
         const cv::Mat & u,
         int h,
         int w,
         bool invert = false)
{
  cv::Mat selection(h, w, CV_8UC3);
  cv::Mat mask(h, w, CV_8U);
  cv::Mat u_cp(h, w, CV_32F); // for some reason cv::threshold() works only with 32-bit floats

  u.convertTo(u_cp, u_cp.type());
  cv::threshold(u_cp, mask, 0, 1, cv::THRESH_BINARY);
  mask.convertTo(mask, CV_8U);
  if(invert) mask = 1 - mask;

  selection.setTo(cv::Scalar(255, 255, 255));
  img.copyTo(selection, mask);
  return selection;
}

/**
 * @section perona_malik Perona-Malik segmentation
 *
 * The method implements Perona-Malik segmentation for a multichannel image
 * (the smoothing is applied to each channel separately, hence easily parallelizable).
 * The idea is to evolve the image @f$I=I(t;\,x,\,y)@f$ according to
 *   @f[ I_{t} = \nabla c\cdot\nabla I+c\Delta I\,, @f]
 * where @f$c=c(t;\,x,\,y)@f$ is the diffusion coefficient function.  For the choice of @f$c@f$
 * we picked
 *   @f[ c(||\nabla I||) = \frac{1}{1+\left(\frac{||\nabla I||}{K}\right)^{2}}\,. @f]
 * Note that there are other possibilities for @f$c@f$, e.g. an exponential function
 * dependant on the magnitude of normalized image gradient, @f$||\nabla I||@f$. In general,
 * any function @f$f(x)@f$ for which @f$f(0) = 1@f$ and @f$\lim_{x\to\infty}f(x)=0@f$
 * would do the trick. Parameter @f$K@f$ here regulates the sensitivity of detecting the edges:
 * the larger it is, the more the edges will be smeared.
 *
 * To make any sense of this, consider the case where @f$c@f$ is constant. This way the first
 * term in the RHS of the e.o.m is zero and the equation reduces to that of Gaussian blurring.
 * If the image contains any edges, they will be smoothed out. However, if we use a function
 * that will be zero in the region containing no edges and non-zero if there's an edge,
 * we'd smooth only the regions with no edges.
 *
 * In every time step we first calculate the magnitude of normalized image gradient by
 * convoluting the image with Sobel kernels,
 *   @f[
 *        \mathbf{G}_{x}=\begin{pmatrix}-1&0&1\\-2&0&2\\-1&0&1\end{pmatrix}*I\,,\quad
 *        \mathbf{G}_{y}=\begin{pmatrix}-1&-2&-1\\0&0&0\\1&2&1\end{pmatrix}*I\,,
 *   @f]
 * and calculating its square modulus with
 * @f$||\nabla I||^2=\mathbf{G}_{x}^{2}+\mathbf{G}_{x}^{2}@f$. This step is actually
 * combined with the calculation of @f$c(||\nabla I||)@f$. At the image border we keep
 * the function constant, though: @f$\left.c\right|_{\partial I}=1@f$.
 *
 * The e.o.m is approximated by (per original paper @cite Perona1990):
 *   @f[
 *       I_{i,j}^{t+1}=I_{i,j}^{t}+L[
 *                     c_{S}\nabla_{S}I+
 *                     c_{E}\nabla_{E}I+
 *                     c_{N}\nabla_{N}I+
 *                     c_{W}\nabla_{W}I
 *                    ]_{i,j}^{t}\,, @f]
 * where
 *   - @f$L\in(0,1/4)@f$ is there for the numerical scheme to be stable (note that in the paper
 *     the authors use @f$\lambda@f$ instead of @f$L@f$); it serves as a time step as well
 *   - @f$\nabla_{k}@f$ with @f$k=\{S,\,E,\,N,\,W\}@f$ denote nearest neighbour differences:
 *       @f{eqnarray*}{ &\nabla_{S}I_{i,j}=I_{i+1,j}-I_{i,j}\,,\\
 *                      &\nabla_{E}I_{i,j}=I_{i,j+1}-I_{i,j}\,,\\
 *                      &\nabla_{N}I_{i,j}=I_{i-1,j}-I_{i,j}\,,\\
 *                      &\nabla_{W}I_{i,j}=I_{i,j-1}-I_{i,j}\,;
 *       @f}
 *   - and @f$c_{k}@f$ denote respective diffusion coefficients:
 *       @f{eqnarray*}{
 * &c_{S_{i,j}}^{t}=c(||(\nabla I)_{i-1/2,j}^{t}||)
 *                  \approx\frac{1}{4}[c(||(\nabla I)_{i+1,j}||)+c(||(\nabla I)_{i,j}||)]^{t}\,,\\
 * &c_{E_{i,j}}^{t}=c(||(\nabla I)_{i,j+1/2}^{t}||)
 *                  \approx\frac{1}{4}[c(||(\nabla I)_{i,j+1}||)+c(||(\nabla I)_{i,j}||)]^{t}\,,\\
 * &c_{N_{i,j}}^{t}=c(||(\nabla I)_{i+1/2,j}^{t}||)
 *                  \approx\frac{1}{4}[c(||(\nabla I)_{i-1,j}||)+c(||(\nabla I)_{i,j}||)]^{t}\,,\\
 * &c_{W_{i,j}}^{t}=c(||(\nabla I)_{i,j-1/2}^{t}||)
 *                  \approx\frac{1}{4}[c(||(\nabla I)_{i,j-1}||)+c(||(\nabla I)_{i,j}||)]^{t}\,.
 *       @f}
 * The iteration process lasts until specified time @f$T@f$.
 *
 * @param channels The original image split into channels.
 * @param h        Height of the image
 * @param w        Width of the image
 * @param K        Edge detection parameter @f$K@f$ (same for all channels).
 * @param L        Laplacian parameter @f$L@f$ (same for all channels).
 * @param T        Time scale @f$T@f$ (same for all channels).
 * @return Smoothed image @f$I(T)@f$.
 */
cv::Mat
perona_malik(const std::vector<cv::Mat> & channels,
             int h,
             int w,
             double K,
             double L,
             double T)
{
  const int nof_channels = channels.size();
  std::vector<cv::Mat> smoothed_channels(nof_channels);

#pragma omp parallel for num_threads(nof_channels)
  for(int k = 0; k < nof_channels; ++k)
  {
    cv::Mat I_prev(h, w, CV_64FC1), // image at previous time step
            I_curr(h, w, CV_64FC1), // image at current time step
            I_res(h, w, CV_8UC1);   // the resulting image
    channels[k].copyTo(I_prev);
    I_prev.convertTo(I_prev, CV_64FC1);

    for (double t = 0; t < T; t += L)
    {
      cv::Mat g(h, w, CV_64FC1),
              dx(h, w, CV_64FC1),
              dy(h, w, CV_64FC1);
      cv::Sobel(I_prev, dx, CV_64FC1, 1, 0, 3);
      cv::Sobel(I_prev, dy, CV_64FC1, 0, 1, 3);
      I_curr = cv::Mat::zeros(h, w, CV_64F);

      const double * const I_prev_ptr = reinterpret_cast<double *>(I_prev.data);
      const double * const dx_ptr = reinterpret_cast<double *>(dx.data);
      const double * const dy_ptr = reinterpret_cast<double *>(dy.data);
      double * const I_curr_ptr = reinterpret_cast<double *>(I_curr.data);
      double * const g_ptr = reinterpret_cast<double *>(g.data);

      for(int i = 0; i < h; ++i)
        for(int j = 0; j < w; ++j)
        {
            const double gx = dx_ptr[i * w + j];
            const double gy = dy_ptr[i * w + j];
            const double d = i == 0 || i == h - 1 || j == 0 || j == w - 1 ?
                             1 :
                             std::pow(1.0 + (std::pow(gx, 2) + std::pow(gy, 2)) / (std::pow(K, 2)), -1);
            g_ptr[i * w + j] = d;
       }

      for(int i = 0; i < h; ++i)
        for(int j = 0; j < w; ++j)
        {
          const int in = i == h - 1 ? i : i + 1;
          const int ip = i == 0     ? i : i - 1;
          const int jn = j == w - 1 ? j : j + 1;
          const int jp = j == 0     ? j : j - 1;

          const double Is = I_prev_ptr[in * w + j ];
          const double Ie = I_prev_ptr[i  * w + jn];
          const double In = I_prev_ptr[ip * w + j ];
          const double Iw = I_prev_ptr[i  * w + jp];
          const double I0 = I_prev_ptr[i  * w + j ];

          const double cs = g_ptr[in * w + j ];
          const double ce = g_ptr[i  * w + jn];
          const double cn = g_ptr[ip * w + j ];
          const double cw = g_ptr[i  * w + jp];
          const double c0 = g_ptr[i  * w + j ];

          I_curr_ptr[i * w + j] = I0 + L * ((cs + c0) * (Is - I0) +
                                            (ce + c0) * (Ie - I0) +
                                            (cn + c0) * (In - I0) +
                                            (cw + c0) * (Iw - I0) ) / 4;
        }

      I_curr.copyTo(I_prev);
      I_prev.convertTo(I_res, CV_8UC1);
    }

    smoothed_channels[k] = I_res;
  }
  cv::Mat smoothed_img;
  cv::merge(smoothed_channels, smoothed_img);

  return smoothed_img;
}

/**
 * @brief Callback function for drawing contour on the image
 *        Calls InteractiveData virtual function mouse_on, which is implemented
 *        in its subclasses InteractiveDataRect (rectangular contour) and
 *        InteractiveDataCirc (circular contour)
 * @param event Event number
 * @param x     x-coordinate of the mouse in the window
 * @param y     y-coordinate of the mouse in the window
 * @param id    Additional data, which will be converted into InteractiveData pointer
 */
void
on_mouse(int event,
         int x,
         int y,
         int,
         void * id)
{
  InteractiveData * ptr = static_cast<InteractiveData *>(id);
  ptr -> mouse_on(event, x, y);
}

int
main(int argc,
     char ** argv)
{
/// @section csv_segmentation Chan-Sandberg-Vese segmentation
///
/// @subsection csv_theory Theory
/// Since the routine contains too many free parameters which makes it unreasonable to place it into a separate
/// function, all the code is kept in main(). Here's a rough explanation of what's Chan-Sandberg-Vese all about,
/// which is based on paper @cite Getreuer2012.
///
/// The Chan-Vese method seeks a contour @f$\mathcal{C}@f$ which minimizes the functional
/// @f[
///    \mathcal{F}[I;\,\mathcal{C},\,c_{1},\,c_{2}]=
///        \mu\mathrm{Length}(\mathcal{C})+
///        \nu\mathrm{Area}(\mathcal{C})+
///        \lambda_{1}\int_{\mathcal{C}}|I-c_{1}|^{2}\,\mathrm{d}x\mathrm{d}y+
///        \lambda_{2}\int_{\Omega\setminus\mathcal{C}}|I-c_{2}|^{2}\,\mathrm{d}x\mathrm{d}y\,,
/// @f]
/// where
///    - the single-channel image @f$I=I(x,\,y)@f$ is defined on the region @f$\Omega=[0,\,a]\times[0,\,b]@f$;
///         - regions in the integral limits, @f$\mathcal{C}@f$ and @f$\Omega\setminus\mathcal{C}@f$,
///           denote the region enclosed by the contour and the region outside the contour, respectively;
///    -  @f$\mu(=0.5)@f$, @f$\nu(=0)@f$, @f$\lambda_{1}(=1)@f$ and @f$\lambda_{2}(=1)@f$ are free parameters,
///       whereby only @f$\nu@f$ can be negative (default values in parentheses);
///    - @f$c_{1}@f$ and @f$c_{2}@f$ are constants that depend on the information of the regions enclosed by and
///      outside of the contour.
///
/// Instead of dealing with @f$\mathcal{C}@f$ explicitly, it's custom to define a level set function @f$u(x,\,y;\,t)@f$
/// so that its zero-level iso-surface (also: zero level set) coincides with the contour:
/// @f$\mathcal{C}=\{\Omega\ni(x,\,y)\,:\,u(x,\,y;\,t)=0\forall t\}@f$. This in turn leads us to a new definition
/// of the functional:
/// @f[
///      \mathcal{F}[I;\,u,\,c_{1},\,c_{2}] =
///             \mu\left(\int_{\Omega}|\nabla H(u)|\,\mathrm{d}x\mathrm{d}y\right)^{p}+
///             \nu\int_{\Omega}H(u)\,\mathrm{d}x\mathrm{d}y+
///             \lambda_{1}\int_{\Omega}|I-c_{1}|^{2}H(u)\,\mathrm{d}x\mathrm{d}y+
///             \lambda_{2}\int_{\Omega}|I-c_{2}|^{2}(1-H(u))\,\mathrm{d}x\mathrm{d}y\,.
/// @f]
/// In our implementation we've picked @f$p=1@f$, so that the first integral reduces to
/// @f[
///      \left.\mu\left(\int_{\Omega}|\nabla H(u)|\,\mathrm{d}x\mathrm{d}y\right)^{p}\right|_{p=1}=
///       \mu\int_{\Omega}\delta(u)|\nabla u|\,\mathrm{d}x\mathrm{d}y\,,
/// @f]
/// where @f$H(x)@f$ and @f$\delta(x)=H'(x)@f$ are Heaviside's step and Dirac's delta functions.
/// In this prescription @f$c_{1}@f$ and @f$c_{2}@f$ are now region averages and take the following form:
/// @f[
///      c_{1}=\frac{\int_{\Omega}IH(u)\mathrm{d}x\mathrm{d}y}{\int_{\Omega}H(u)\,\mathrm{d}x\mathrm{d}y}\,,\quad
///      c_{2}=\frac{\int_{\Omega}I(1-H(u))\mathrm{d}x\mathrm{d}y}{\int_{\Omega}(1 - H(u))\,\mathrm{d}x\mathrm{d}y}\,.
/// @f]
/// For practical reasons the functions are replaced by smooth/regularized versions (see regularized_heaviside() and
/// regularized_delta()):
/// @f[
///      H_{\epsilon}(x)=\frac{1}{2}\left[1+\frac{2}{\pi}\arctan\left(\frac{x}{\epsilon}\right)\right]\,,\quad
///      \delta_{\epsilon}(x)=\frac{\epsilon}{\pi\left(\epsilon^{2}+x^{2}\right)}\,,
/// @f]
/// with @f$\epsilon=1@f$ by default.
/// The interpretation of the functional @f$\mathcal{F}@f$ is the following:
///     - the first term penalizes the length of @f$\mathcal{C}@f$;
///     - the second term penalizes the area enclosed by the curve;
///     - the 3rd and 4th term penalize region averages inside and outside of the contour; in other words
///       it keeps track of the discrepancy between the two regions.
///
/// A stationary solution to @f$\mathcal{F}@f$, or equivalently the equation of motion (e.o.m) for the contour,
/// can be found by solving it with Euler-Lagrange equation, which results in
/// @f[
///    u_{t} = \delta_{\epsilon}(u)\left[\mu\kappa-\nu-\lambda_{1}(I-c_{1})^{2}+\lambda_{2}(I-c_{2})^{2}\right]\,,
/// @f]
/// where @f$\kappa=\nabla\cdot\left(\frac{\nabla u}{|\nabla u|}\right)@f$ is curvature of @f$u@f$.
///
/// If the (still 2D) image has @f$I@f$ has @f$N@f$ channels @f$\{I_{i}(x,\,y)\}_{i=1}^{N}@f$, there should still
/// be a single level set @f$u@f$, which leads us the following functional:
/// @f[
///    \mathcal{F}[I;\,u,\,\mathbf{c_{1}},\,\mathbf{c}_{2}]=
///      \mu\int_{\Omega}|\nabla H(u)|\mathrm{d}x\mathrm{d}y+
///      \nu\int_{\Omega}H(u)\mathrm{d}x\mathrm{d}y+
///      \int_{\Omega}\frac{1}{N}\sum_{i=1}^{N}\lambda_{1}^{(i)}|I_{i}-c_{1}^{(i)}|^{2}H(u)\mathrm{d}x\mathrm{d}y+
///      \int_{\Omega}\frac{1}{N}\sum_{i=1}^{N}\lambda_{2}^{(i)}|I_{i}-c_{2}^{(i)}|^{2}(1-H(u))\mathrm{d}x\mathrm{d}y\,.
/// @f]
/// Variables @f$\{c_{1}^{(i)},\,c_{2}^{(i)}\}_{i=1}^{N}@f$ retain their original meaning,
/// @f[
///     c_{1}^{(i)}=\frac{\int_{\Omega}I_{i}H(u)\mathrm{d}x\mathrm{d}y}{\int_{\Omega}H(u)\mathrm{d}x\mathrm{d}y}\,,\quad
///     c_{2}^{(i)}=\frac{\int_{\Omega}I_{i}(1-H(u))\mathrm{d}x\mathrm{d}y}{\int_{\Omega}(1-H(u))\mathrm{d}x\mathrm{d}y}
///     \quad\forall i=\{1,\,\ldots,\,N\}\,;
/// @f]
/// the constants @f$\{\lambda_{1}^{(i)},\,\lambda_{2}^{(i)}\}_{i=1}^{N}@f$ are defined for each channel separately.
/// This implementation consider only grayscale (@f$N=1@f$) and RGB (@f$N=3@f$) images, for which @f$\lambda_{i}=1@f$
/// by default for any @f$i@f$-th channel.
/// The corresponding e.o.m reads
/// @f[
///      u_{t}=\delta_{\epsilon}(u)\left[\mu\kappa-\nu-
///            \frac{1}{N}\sum_{i=1}^{N}\lambda_{1}^{(i)}\left(I_{i}-c_{1}^{(i)}\right)^{2}+
///            \frac{1}{N}\sum_{i=1}^{N}\lambda_{2}^{(i)}\left(I_{i}-c_{2}^{(i)}\right)^{2}\right]\,.
/// @f]
///
/// @subsection csv_numsch Numerical scheme
///
/// Finite difference expression for the curvature @f$\kappa@f$ is explained in curvature(). The advantage of this scheme
/// is that we only need nearest neighbouring points at current point while keeping the derivative centered at current point,
/// whereas naive implementation would use more distant points. Since we're dealing with a finite domain and therefore
/// boundaries, we don't have to "extend" the region by two pixels each direction. Instead, we just duplicate border pixels.
///
/// Rest of the calculation is actually quite straightforward -- the zero level set is iteratively updated with
/// @f[
///      u_{i,j}^{n+1}=u_{i,j}^{n}+\mathrm{d}t\;\delta_{\epsilon}(u_{i,j}^{n})\left[\kappa_{i,j}^{n}-\nu-
///      \frac{1}{N}\sum_{k=1}^N\lambda_{1}^{(k)}\left(I_{i,j}-c_{1}^{n,(k)}\right)+
///      \frac{1}{N}\sum_{k=1}^N\lambda_{2}^{(k)}\left(I_{i,j}-c_{2}^{n,(k)}\right)\right]\,.
/// @f]
/// The method is inherently implicit and is implemented with ordinary matrix operations. The first term in the brackets
/// has already been discussed; the second term is trivial; the final two terms are explained in region_variance() and
/// variance_penalty().
///
/// There are various ways to initialize the level set, and since we're solving a differential equation, different initial
/// conditions lead to different outcome. The simplest way is to let the user draw either rectangular or circular contour.
/// The level set will be evaluated with @f$+1@f$'s inside the contour and with @f$-1@f$'s outside of it.
/// A more optimal (here the default) contour would be checkerboard
/// @f[
///      u(i,\,j;\,0)=\sin\left(\frac{\pi}{5}i\right)\sin\left(\frac{\pi}{5}j\right)\,,
/// @f]
/// because it converges faster to a solution (see levelset_checkerboard()). The solution is reached when the maximum number
/// of iterations, @f$T_\max@f$, is reached or when @f$||u_{i,j}^{n+1}-u_{i,j}^{n}||_{2}\leqslant\delta ||\bar{I}||_{2}@f$,
/// where the subscript denotes @f$L_{2}@f$-norm, @f$\delta=(10^{-3})@f$ is tolerance parameter and @f$\bar{I}@f$ is
/// the intensity average in the image (averaged across the channels).
///
/// @subsection csv_summary Summary
///
/// The main logic described above starts with a timestep loop (look for the comment below); everything else preciding
/// that is actually sugar coating just to make the program usable for anyone.
///
/// If it isn't clear from above text or the code below, here is the list of variables which the user can pass as an argument
/// (the default values in the parentheses): @f$\mu(=0.5)@f$, @f$\nu(=0)@f$, @f$\mathrm{d}t(=1)@f$,
/// @f$\lambda_{1}^{(i)}(=1)@f$ and @f$\lambda_{1}^{(i)}(=1)@f$ @f$\forall i=1\ldots N@f$, @f$\epsilon(=1)@f$,
/// @f$\delta(=10^{-3})@f$, @f$T_\max@f$(=INT_MAX), @f$N(=1\;\mbox{or}\;3)@f$ (number of channels).
///
/// Other general options include:
///    - object selection (-s) -- the region enclosed by the contour will be cut out and placed onto white canvas and saved;
///    - region inversion (-I) -- sometimes the ROI is inverted; there's an option to circumvent that (goes with -s option);
///    - video output (-V) -- see contour evolution in a video (*.avi, the same filename as the image; see VideoWriterManager);
///    - overlay text (-O) -- puts overlay text (timesteps) on the video (goes with the previous option);
///    - frame rate (-f) -- frame rate of the video;
///    - line color (-l) -- color of the contour line (see Colors);
///    - rectangular (-R) or circular (-C) contour -- lets the user draw it on the image (see InteractiveData and its subclasses);
///    - grayscale image (-g) -- sometimes we just want do perform it on a black-white image, but the original source is RGB.
///
/// For Perona-Malik-specific parameters @f$K@f$, @f$L@f$, @f$T@f$, see perona_malik().
///
/// @sa curvature, region_variance, variance_penalty, levelset_checkerboard, VideoWriterManager, InteractiveData, Colors, perona_malik

  double mu, nu, eps, tol, dt, fps, K, L, T;
  int max_steps;
  std::vector<double> lambda1,
                      lambda2;
  std::string input_filename,
              text_position,
              line_color_str;
  bool grayscale         = false,
       write_video       = false,
       overlay_text      = false,
       object_selection  = false,
       invert            = false,
       segment           = false,
       rectangle_contour = false,
       circle_contour    = false;
  ChanVese::TextPosition pos = ChanVese::TextPosition::TopLeft;
  cv::Scalar contour_color = ChanVese::Colors::blue;

//-- Parse command line arguments
//   Negative values in multitoken are not an issue, b/c it doesn't make much sense
//   to use negative values for lambda1 and lambda2
  try
  {
    namespace po = boost::program_options;
    po::options_description desc("Allowed options", get_terminal_width());
    desc.add_options()
      ("help,h",                                                                               "this message")
      ("input,i",            po::value<std::string>(&input_filename),                          "input image")
      ("mu",                 po::value<double>(&mu) -> default_value(0.5),                     "length penalty parameter (must be positive or zero)")
      ("nu",                 po::value<double>(&nu) -> default_value(0),                       "area penalty parameter")
      ("dt",                 po::value<double>(&dt) -> default_value(1),                       "timestep")
      ("lambda1",            po::value<std::vector<double>>(&lambda1) -> multitoken(),         "penalty of variance inside the contour (default: 1's)")
      ("lambda2",            po::value<std::vector<double>>(&lambda2) -> multitoken(),         "penalty of variance outside the contour (default: 1's)")
      ("epsilon,e",          po::value<double>(&eps) -> default_value(1),                      "smoothing parameter in Heaviside/delta")
      ("tolerance,t",        po::value<double>(&tol) -> default_value(0.001),                  "tolerance in stopping condition")
      ("max-steps,N",        po::value<int>(&max_steps) -> default_value(-1),                  "maximum nof iterations (negative means unlimited)")
      ("fps,f",              po::value<double>(&fps) -> default_value(10),                     "video fps")
      ("overlay-pos,P",      po::value<std::string>(&text_position) -> default_value("TL"),    "overlay tex position; allowed only: TL, BL, TR, BR")
      ("line-color,l",       po::value<std::string>(&line_color_str) -> default_value("blue"), "contour color (allowed only: black, white, R, G, B, Y, M, C")
      ("edge-coef,K",        po::value<double>(&K) -> default_value(10),                       "coefficient for enhancing edge detection in Perona-Malik")
      ("laplacian-coef,L",   po::value<double>(&L) -> default_value(0.25),                     "coefficient in the gradient FD scheme of Perona-Malik (must be [0, 1/4])")
      ("segment-time,T",     po::value<double>(&T) -> default_value(20),                       "number of smoothing steps in Perona-Malik")
      ("segment,S",          po::bool_switch(&segment),                                        "segment the image with Perona-Malik beforehand")
      ("grayscale,g",        po::bool_switch(&grayscale),                                      "read in as grayscale")
      ("video,V",            po::bool_switch(&write_video),                                    "enable video output (changes the extension to '.avi')")
      ("overlay-text,O",     po::bool_switch(&overlay_text),                                   "add overlay text")
      ("invert-selection,I", po::bool_switch(&invert),                                         "invert selected region (see: select)")
      ("select,s",           po::bool_switch(&object_selection),                               "separate the region encolosed by the contour (adds suffix '_selection')")
      ("rectangle,R",        po::bool_switch(&rectangle_contour),                              "select rectangular contour interactively")
      ("circle,C",           po::bool_switch(&circle_contour),                                 "select circular contour interactively")
    ;
    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);
    po::notify(vm);

    if(vm.count("help"))
    {
      std::cout << desc << "\n";
      return EXIT_SUCCESS;
    }
    if(! vm.count("input"))
      msg_exit("Error: you have to specify input file name!");
    else if(vm.count("input") && ! boost::filesystem::exists(input_filename))
      msg_exit("Error: file \"" + input_filename + "\" does not exists!");
    if(vm.count("dt") && dt <= 0)
      msg_exit("Cannot have negative or zero timestep: " + std::to_string(dt) + ".");
    if(vm.count("mu") && mu < 0)
      msg_exit("Length penalty parameter cannot be negative: " + std::to_string(mu) + ".");
    if(vm.count("lambda1"))
    {
      if(grayscale && lambda1.size() != 1)
        msg_exit("Too many lambda1 values for a grayscale image.");
      else if(! grayscale && lambda1.size() != 3)
        msg_exit("Number of lambda1 values must be 3 for a colored input image.");
      else if(grayscale && lambda1[0] < 0)
        msg_exit("The value of lambda1 cannot be negative.");
      else if(! grayscale && (lambda1[0] < 0 || lambda1[1] < 0 || lambda1[2] < 0))
        msg_exit("Any value of lambda1 cannot be negative.");
    }
    else if(! vm.count("lambda1"))
    {
      if(grayscale) lambda1 = {1};
      else          lambda1 = {1, 1, 1};
    }
    if(vm.count("lambda2"))
    {
      if(grayscale && lambda2.size() != 1)
        msg_exit("Too many lambda2 values for a grayscale image.");
      else if(! grayscale && lambda2.size() != 3)
        msg_exit("Number of lambda2 values must be 3 for a colored input image.");
      else if(grayscale && lambda2[0] < 0)
        msg_exit("The value of lambda2 cannot be negative.");
      else if(! grayscale && (lambda2[0] < 0 || lambda2[1] < 0 || lambda2[2] < 0))
        msg_exit("Any value of lambda2 cannot be negative.");
    }
    else if(! vm.count("lambda2"))
    {
      if(grayscale) lambda2 = {1};
      else          lambda2 = {1, 1, 1};
    }
    if(vm.count("eps") && eps < 0)
      msg_exit("Cannot have negative smoothing parameter: " + std::to_string(eps) + ".");
    if(vm.count("tol") && tol < 0)
      msg_exit("Cannot have negative tolerance: " + std::to_string(tol) + ".");
    if(vm.count("overlay-pos"))
    {
      if     (boost::iequals(text_position, "TL")) pos = ChanVese::TextPosition::TopLeft;
      else if(boost::iequals(text_position, "BL")) pos = ChanVese::TextPosition::BottomLeft;
      else if(boost::iequals(text_position, "TR")) pos = ChanVese::TextPosition::TopRight;
      else if(boost::iequals(text_position, "BR")) pos = ChanVese::TextPosition::BottomRight;
      else
        msg_exit("Invalid text position requested.\n"\
                 "Correct values are: TL -- top left\n"\
                 "                    BL -- bottom left\n"\
                 "                    TR -- top right\n"\
                 "                    BR -- bottom right"\
                );
    }
    if(vm.count("line-color"))
    {
      if     (boost::iequals(line_color_str, "red"))     contour_color = ChanVese::Colors::red;
      else if(boost::iequals(line_color_str, "green"))   contour_color = ChanVese::Colors::green;
      else if(boost::iequals(line_color_str, "blue"))    contour_color = ChanVese::Colors::blue;
      else if(boost::iequals(line_color_str, "black"))   contour_color = ChanVese::Colors::black;
      else if(boost::iequals(line_color_str, "white"))   contour_color = ChanVese::Colors::white;
      else if(boost::iequals(line_color_str, "magenta")) contour_color = ChanVese::Colors::magenta;
      else if(boost::iequals(line_color_str, "yellow"))  contour_color = ChanVese::Colors::yellow;
      else if(boost::iequals(line_color_str, "cyan"))    contour_color = ChanVese::Colors::cyan;
      else
        msg_exit("Invalid contour color requested.\n"\
                 "Correct values are: red, green, blue, black, white, magenta, yellow, cyan.");
    }
    if(vm.count("laplacian-coef") && (L > 0.25 || L < 0))
      msg_exit("The Laplacian coefficient in Perona-Malik segmentation must be between 0 and 0.25.");
    if(vm.count("segment-time") && (T < L))
      msg_exit("The segmentation duration must exceed the value of Laplacian coefficient, " +
               std::to_string(L) + ".");
    if(rectangle_contour && circle_contour)
      msg_exit("Cannot initialize with both rectangular and circular contour");
  }
  catch(std::exception & e)
  {
    msg_exit("error: " + std::string(e.what()));
  }

//-- Read the image (grayscale or BGR? RGB? BGR? help)
  cv::Mat _img;
  if(grayscale) _img = cv::imread(input_filename, CV_LOAD_IMAGE_GRAYSCALE);
  else          _img = cv::imread(input_filename, CV_LOAD_IMAGE_COLOR);
  if(! _img.data)
    msg_exit("Error on opening \"" + input_filename + "\" (probably not an image)!");

//-- Second conversion needed since we want to display a colored contour on a grayscale image
  cv::Mat img;
  if(grayscale) cv::cvtColor(_img, img, CV_GRAY2RGB);
  else          img = _img;
  _img.release();

//-- Determine the constants and define functionals
  max_steps = max_steps < 0 ? std::numeric_limits<int>::max() : max_steps;
  const int h = img.rows;
  const int w = img.cols;
  const int nof_channels = grayscale ? 1 : img.channels();
  const auto heaviside = std::bind(regularized_heaviside, std::placeholders::_1, eps);
  const auto delta = std::bind(regularized_delta, std::placeholders::_1, eps);

//-- Construct the level set
  cv::Mat u;
  if(rectangle_contour || circle_contour)
  {
    std::unique_ptr<InteractiveData> id;
    cv::startWindowThread();
    cv::namedWindow(WINDOW_TITLE, cv::WINDOW_NORMAL);

    if     (rectangle_contour)
      id = std::unique_ptr<InteractiveDataRect>(new InteractiveDataRect(&img, contour_color));
    else if(circle_contour)
      id = std::unique_ptr<InteractiveDataCirc>(new InteractiveDataCirc(&img, contour_color));

    if(id) cv::setMouseCallback(WINDOW_TITLE, on_mouse, id.get());
    cv::imshow(WINDOW_TITLE, img);
    cv::waitKey();
    cv::destroyWindow(WINDOW_TITLE);

    if(id)
    {
      if(! id -> is_ok())
        msg_exit("You must specify the contour with non-zero dimensions");
      u = id -> get_levelset(h, w);
    }
  }
  else
    u = levelset_checkerboard(h, w);

//-- Set up the video writer (and save the first frame)
  VideoWriterManager vwm;
  if(write_video)
  {
    vwm = VideoWriterManager(input_filename, img, contour_color, fps, pos, overlay_text);
    vwm.write_frame(u, overlay_text ? "t = 0" : "");
  }

//-- Split the channels
  std::vector<cv::Mat> channels;
  channels.reserve(nof_channels);
  cv::split(img, channels);
  if(grayscale) channels.erase(channels.begin() + 1, channels.end());

//-- Smooth the image with Perona-Malik
  cv::Mat smoothed_img;
  if(segment)
  {
    smoothed_img = perona_malik(channels, h, w, K, L, T);
    channels.clear();
    cv::split(smoothed_img, channels);
    cv::imwrite(add_suffix(input_filename, "pm"), smoothed_img);
  }

//-- Find intensity sum and derive the stopping condition
  cv::Mat intensity_avg = cv::Mat(h, w, CV_64FC1);
#pragma omp parallel for num_threads(nof_channels)
  for(int k = 0; k < nof_channels; ++k)
  {
    cv::Mat channel(h, w, intensity_avg.type());
    channels[k].convertTo(channel, channel.type());
    intensity_avg += channel;
  }
  intensity_avg /= nof_channels;
  double stop_cond = tol * cv::norm(intensity_avg, cv::NORM_L2);
  intensity_avg.release();

//-- Timestep loop
  for(int t = 1; t <= max_steps; ++t)
  {
    cv::Mat u_diff(cv::Mat::zeros(h, w, CV_64FC1));

//-- Channel loop
#pragma omp parallel for num_threads(nof_channels)
    for(int k = 0; k < nof_channels; ++k)
    {
      cv::Mat channel = channels[k];
//-- Find the average regional variances
      const double c1 = region_variance(channel, u, h, w, ChanVese::Region::Inside, heaviside);
      const double c2 = region_variance(channel, u, h, w, ChanVese::Region::Outside, heaviside);

//-- Calculate the contribution of one channel to the level set
      const cv::Mat variance_inside = variance_penalty(channel, h, w, c1, lambda1[k]);
      const cv::Mat variance_outside = variance_penalty(channel, h, w, c2, lambda2[k]);
      u_diff += -variance_inside + variance_outside;
    }
//-- Calculate the curvature (divergence of normalized gradient)
    const cv::Mat kappa = curvature(u, h, w);

//-- Mash the terms together
    u_diff = dt * (mu * kappa - nu + u_diff / nof_channels);

//-- Run delta function on the level set
    cv::Mat u_cp = u.clone();
    cv::parallel_for_(cv::Range(0, h * w), ParallelPixelFunction(u_cp, w, delta));

//-- Shift the level set
    cv::multiply(u_diff, u_cp, u_diff);
    const double u_diff_norm = cv::norm(u_diff, cv::NORM_L2);
    u += u_diff;

//-- Save the frame
    if(write_video) vwm.write_frame(u, overlay_text ? "t = " + std::to_string(t) : "");

//-- Check if we have achieved the desired precision
    if(u_diff_norm <= stop_cond) break;
  }

//-- Select the region enclosed by the contour and save it to the disk
  if(object_selection)
    cv::imwrite(add_suffix(input_filename, "selection"), separate(img, u, h, w, invert));

  return EXIT_SUCCESS;
}


// Copyright (c) 2005 - 2014 Marc de Kamps
// 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 copyright holder 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.
//
//      If you use this software in work leading to a scientific publication, you should include a reference there to
//      the 'currently valid reference', which can be found at http://miind.sourceforge.net
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <boost/circular_buffer.hpp>
#include <GeomLib.hpp>

using namespace GeomLib;

BOOST_AUTO_TEST_CASE(QIFOdeSystemTest)
{

    Number n_bins = 400;

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset = -10.0;
    Potential gamma   =  0.5;

    QifParameter
    par_qif
    (
        gamma
    );

	NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    SpikingQifNeuralDynamics dyn(par_ode, par_qif);
    QifOdeSystem sys(dyn);

    vector<double> array_interpretation(n_bins, 0);
    vector<double> array_density(n_bins);

    double single_bin = 1.0;

    sys.PrepareReport(&array_interpretation[0], &array_density[0]);
    BOOST_CHECK_CLOSE(single_bin, array_density[0] * (array_interpretation[1] - array_interpretation[0]), 1e-3);
    BOOST_CHECK_CLOSE(par_ode._V_min, array_interpretation[0], 1e-3);
}

BOOST_AUTO_TEST_CASE(QIFEvolvePositive)
{

    Number n_bins = 10; // total number of bins
    Number n_shift = 5; // number of times times t_step, i.e. the number of bins that the density peak will be shifted.

    Time t_mem        =  10e-3;
    Time t_ref        =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset = -10.0;
    Potential gamma   =  0.5;

    QifParameter
    par_qif
    (
        gamma
    );

	NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);

    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    SpikingQifNeuralDynamics dyn(par_ode, par_qif);
    QifOdeSystem sys(dyn);

    vector<double> buffer_interpretation(n_bins, 0);
    vector<double> array_density(n_bins, 0);

    sys.PrepareReport(&buffer_interpretation[0], &array_density[0]);
    Time t = dyn.TimeToInf(par_ode._V_min) / n_bins;

    for (Index i = 0; i < n_shift; i++)
        sys.Evolve(t);

    sys.PrepareReport(&buffer_interpretation[0], &array_density[0]);

    // density was initialised at _V_reset; this has now shifted
    double sqr = sqrt(par_qif._gamma);
    double V_new = sqr * tan(sqr * (n_shift * t / par_ode._par_pop._tau) + atan(par_ode._V_min / sqr));

    BOOST_CHECK(fabs(V_new - buffer_interpretation[n_shift]) < 1e-10); // do not use BOOST_CHECK_CLOSE as it compares 1e-16 with 1e-17 and fails
    // to show that density is a density, convert back to probability
    BOOST_CHECK_CLOSE(1.0, array_density[n_shift] * (buffer_interpretation[n_shift + 1] - buffer_interpretation[n_shift]), 1e-3);
}

BOOST_AUTO_TEST_CASE(RateCalculationTest)
{
    Number n_bins = 10; // total number of bins

    Time t_mem   =  10e-3;
    Time t_ref   =  0.0;
    Potential V_min   = -10.0;
    Potential V_peak  =  10.0;
    Potential V_reset = -10.0;
    Potential gamma   =  0.5;

    QifParameter
    par_qif
    (
        gamma
    );

	NeuronParameter par_neuron(V_peak, V_reset, V_reset, t_ref, t_mem);
    // create a peak
    OdeParameter
    par_ode
    (
    	n_bins,
		V_min,
		par_neuron,
        InitialDensityParameter(V_reset, 0.0)
    );

    SpikingQifNeuralDynamics dyn(par_ode, par_qif);
    QifOdeSystem sys(dyn);

    vector<double> buffer_interpretation(n_bins, 0);
    vector<double> array_density(n_bins, 0);

    sys.PrepareReport(&buffer_interpretation[0], &array_density[0]);
    Time t = dyn.TimeToInf(par_ode._V_min) / n_bins;

    for (Index i = 0; i < n_bins - 1; i++)
        sys.Evolve(t);

    sys.PrepareReport(&buffer_interpretation[0], &array_density[0]);
    // all probability should now be shift in the highest bin and the firing rate should have been quiet throughout
    MPILib::Rate r_0 = sys.CurrentRate();

    BOOST_CHECK(r_0 == 0.0);

    // this should pust probability through threshold, causing a large peak in firing rate
    t =  dyn.TimeToInf(par_ode._V_min) / n_bins;
    sys.Evolve(t);
    sys.PrepareReport(&buffer_interpretation[0], &array_density[0]);
    MPILib::Rate r_1 = sys.CurrentRate();
    BOOST_CHECK_CLOSE(r_1 * dyn.TimeToInf(par_ode._V_min) / n_bins, 1.0, 1e-3);
}


#ifndef STAN_MATH_REV_MAT_FUN_TRACE_QUAD_FORM_HPP
#define STAN_MATH_REV_MAT_FUN_TRACE_QUAD_FORM_HPP

#include <boost/utility/enable_if.hpp>
#include <boost/type_traits.hpp>
#include <stan/math/rev/core.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/typedefs.hpp>
#include <stan/math/rev/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/value_of.hpp>
#include <stan/math/rev/mat/fun/typedefs.hpp>
#include <stan/math/prim/mat/fun/trace_quad_form.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <stan/math/prim/mat/err/check_square.hpp>

namespace stan {
  namespace math {
    namespace {
      template <typename TA, int RA, int CA, typename TB, int RB, int CB>
      class trace_quad_form_vari_alloc : public chainable_alloc {
      public:
        trace_quad_form_vari_alloc(const Eigen::Matrix<TA, RA, CA>& A,
                                   const Eigen::Matrix<TB, RB, CB>& B)
          : A_(A), B_(B)
        { }

        double compute() {
          return trace_quad_form(value_of(A_),
                                 value_of(B_));
        }

        Eigen::Matrix<TA, RA, CA>  A_;
        Eigen::Matrix<TB, RB, CB>  B_;
      };

      template <typename TA, int RA, int CA, typename TB, int RB, int CB>
      class trace_quad_form_vari : public vari {
      protected:
        static inline void chainA(Eigen::Matrix<double, RA, CA>& A,
                                  const Eigen::Matrix<double, RB, CB>& Bd,
                                  double adjC) {}
        static inline void chainB(Eigen::Matrix<double, RB, CB>& B,
                                  const Eigen::Matrix<double, RA, CA>& Ad,
                                  const Eigen::Matrix<double, RB, CB>& Bd,
                                  double adjC) {}

        static inline void chainA(Eigen::Matrix<var, RA, CA>& A,
                                  const Eigen::Matrix<double, RB, CB>& Bd,
                                  double adjC) {
          Eigen::Matrix<double, RA, CA>     adjA(adjC*Bd*Bd.transpose());
          for (int j = 0; j < A.cols(); j++)
            for (int i = 0; i < A.rows(); i++)
              A(i, j).vi_->adj_ += adjA(i, j);
        }
        static inline void chainB(Eigen::Matrix<var, RB, CB>& B,
                                  const Eigen::Matrix<double, RA, CA>& Ad,
                                  const Eigen::Matrix<double, RB, CB>& Bd,
                                  double adjC) {
          Eigen::Matrix<double, RA, CA>     adjB(adjC*(Ad + Ad.transpose())*Bd);
          for (int j = 0; j < B.cols(); j++)
            for (int i = 0; i < B.rows(); i++)
              B(i, j).vi_->adj_ += adjB(i, j);
        }

        inline void chainAB(Eigen::Matrix<TA, RA, CA>& A,
                            Eigen::Matrix<TB, RB, CB>& B,
                            const Eigen::Matrix<double, RA, CA>& Ad,
                            const Eigen::Matrix<double, RB, CB>& Bd,
                            double adjC) {
          chainA(A, Bd, adjC);
          chainB(B, Ad, Bd, adjC);
        }

      public:
        explicit
        trace_quad_form_vari
        (trace_quad_form_vari_alloc<TA, RA, CA, TB, RB, CB> *impl)
          : vari(impl->compute()), impl_(impl) { }

        virtual void chain() {
          chainAB(impl_->A_, impl_->B_,
                  value_of(impl_->A_), value_of(impl_->B_),
                  adj_);
        }

        trace_quad_form_vari_alloc<TA, RA, CA, TB, RB, CB> *impl_;
      };
    }

    template <typename TA, int RA, int CA, typename TB, int RB, int CB>
    inline typename
    boost::enable_if_c< boost::is_same<TA, var>::value ||
    boost::is_same<TB, var>::value,
                        var >::type
      trace_quad_form(const Eigen::Matrix<TA, RA, CA>& A,
                      const Eigen::Matrix<TB, RB, CB>& B) {
      check_square("trace_quad_form", "A", A);
      check_multiplicable("trace_quad_form",
                          "A", A,
                          "B", B);

      trace_quad_form_vari_alloc<TA, RA, CA, TB, RB, CB> *baseVari
        = new trace_quad_form_vari_alloc<TA, RA, CA, TB, RB, CB>(A, B);

      return var(new trace_quad_form_vari<TA, RA, CA, TB, RB, CB>(baseVari));
    }

  }
}
#endif


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

#include <complex>
#include <utility>
#include <memory>
#include <vector>
#include <bitset>
#include <cmath>
#include <tuple>

#include <Eigen/Dense>

#include "qpp.h"

/* Lexer */
#include "lexer/token.hpp"

/* AST */
/* Declarations */
#include "representer/ast/decl/variable.hpp"
#include "representer/ast/decl/type.hpp"
/* Expressions */
#include "representer/ast/expr/reference_expression.hpp"
#include "representer/ast/expr/literal_expression.hpp"
#include "representer/ast/expr/call_expression.hpp"
#include "representer/ast/expr/expr.hpp"

/* Builtins */
#include "representer/builtins/lang/avalon_qubit.hpp"
#include "representer/builtins/lang/avalon_cgate.hpp"
#include "representer/builtins/lang/avalon_gate.hpp"
#include "representer/builtins/lang/avalon_bit.hpp"

/* Builtin functions */
#include "interpreter/builtins/lang/avalon_qubit.hpp"

/* Quantum processor */
#include "interpreter/qprocessor.hpp"

/* Exceptions */
#include "interpreter/exceptions/invalid_call.hpp"

namespace avalon {
    static std::complex<double> get_00(double theta, double phi, double lambda) {
        std::complex<double> i(0, 1);
        std::complex<double> e = std::exp(i * ((-phi - lambda)/2));
        double c = cos(theta / 2);
        return e * c;
    }

    static std::complex<double> get_10(double theta, double phi, double lambda) {
        std::complex<double> i(0, 1);
        std::complex<double> e = std::exp(i * ((phi - lambda)/2));
        double s = sin(theta / 2);
        return e * s;
    }

    static std::complex<double> get_01(double theta, double phi, double lambda) {
        std::complex<double> i(0, 1);
        std::complex<double> e = -std::exp(i * ((-phi + lambda)/2));
        double s = sin(theta / 2);
        return e * s;
    }

    static std::complex<double> get_11(double theta, double phi, double lambda) {
        std::complex<double> i(0, 1);
        std::complex<double> e = std::exp(i * ((phi + lambda)/2));
        double c = cos(theta / 2);
        return e * c;
    }

    /**
     * qubit_apply
     * applies a quantum gate to the qubits stored at the given reference
     */
    std::shared_ptr<expr> qubit_apply(std::shared_ptr<qprocessor>& qproc, std::vector<std::shared_ptr<expr> >& arguments) {
        // qubit type
        avalon_qubit avl_qubit;
        type_instance qubit_instance = avl_qubit.get_type_instance();

        // gate type
        avalon_gate avl_gate;
        type_instance gate_instance = avl_gate.get_type_instance();

        // make sure we got only two arguments
        if(arguments.size() != 2)
            throw invalid_call("[compiler error] the quantum <apply> function expects only two arguments.");

        // make sure the first argument is a call expression and the second a reference expression
        std::shared_ptr<expr>& arg_one = arguments[0];
        std::shared_ptr<expr>& arg_two = arguments[1];
        if(arg_one -> is_call_expression() == false)
            throw invalid_call("[compiler error] the quantum <apply> function expects the first argument to be a gate definition.");
        if(arg_two -> is_reference_expression() == false)
            throw invalid_call("[compiler error] the quantum <apply> function expects the second argument to be a reference to a quantum variable.");

        // we cast the arguments
        std::shared_ptr<call_expression> const & arg_one_cons = std::static_pointer_cast<call_expression>(arg_one);
        std::shared_ptr<reference_expression> const & arg_two_ref = std::static_pointer_cast<reference_expression>(arg_two);

        // we double check the type instances
        type_instance arg_one_instance = arg_one_cons -> get_type_instance();
        if(type_instance_strong_compare(arg_one_instance, gate_instance) == false)
            throw invalid_call("[compiler error] the quantum <apply> function expects the first argument to be a gate definition of type <gate>.");

        type_instance arg_two_instance = arg_two_ref -> get_type_instance();
        type_instance arg_two_instance_dref = arg_two_instance.get_params()[0];
        if(type_instance_strong_compare(arg_two_instance_dref, qubit_instance) == false)
            throw invalid_call("[compiler error] the quantum <apply> function expects the second argument to be a reference to a single <qubit> variable. Instead we got <" + mangle_type_instance(arg_two_instance_dref) + ">.");

        // transform the gate definition into an Eigen matrix
        // 1. first, we get the gate parameters and transform them into double values
        std::vector<std::pair<token, std::shared_ptr<expr> > >& gate_args = arg_one_cons -> get_arguments();
        // work on theta
        std::shared_ptr<expr> theta_expr = gate_args[0].second;
        std::shared_ptr<literal_expression> const & theta_lit = std::static_pointer_cast<literal_expression>(theta_expr);
        double theta = theta_lit -> get_float_value();
        // work on phi
        std::shared_ptr<expr> phi_expr = gate_args[1].second;
        std::shared_ptr<literal_expression> const & phi_lit = std::static_pointer_cast<literal_expression>(phi_expr);
        double phi = phi_lit -> get_float_value();
        // work on lambda
        std::shared_ptr<expr> lambda_expr = gate_args[2].second;
        std::shared_ptr<literal_expression> const & lambda_lit = std::static_pointer_cast<literal_expression>(lambda_expr);
        double lambda = lambda_lit -> get_float_value();

        // 2. we built the 2x2 Eigen matrix to be used as gate
        Eigen::MatrixXcd unitary(2, 2);
        unitary(0, 0) = get_00(theta, phi, lambda);
        unitary(1, 0) = get_10(theta, phi, lambda);
        unitary(0, 1) = get_01(theta, phi, lambda);
        unitary(1, 1) = get_11(theta, phi, lambda);

        // we now apply the gate to the ket contained in the second argument
        // 1. get the content of the referenced variable
        std::shared_ptr<variable>& ref_var = arg_two_ref -> get_variable();
        std::shared_ptr<expr>& var_expr = ref_var -> get_value();
        std::shared_ptr<literal_expression> const & qubit_expr = std::static_pointer_cast<literal_expression>(var_expr);
        // we make sure that the qubit was not measured already
        if(qubit_expr -> was_measured())
            throw invalid_call("[compiler error] the quantum <apply> function second argument has already been measured and further gates cannot be applied to it.");

        // 2. apply the gate to qubit
        qproc -> apply(unitary, qubit_expr -> get_start_index(), qubit_expr -> get_end_index());

        // DONE.
        return nullptr;
    }

    /**
     * qubit_capply
     * applies a controlled quantum gate to the qubit given as third argument using the qubit given in second argument as control
     */
    std::shared_ptr<expr> qubit_capply(std::shared_ptr<qprocessor>& qproc, std::vector<std::shared_ptr<expr> >& arguments) {
        // qubit type
        avalon_qubit avl_qubit;
        type_instance qubit_instance = avl_qubit.get_type_instance();

        // gate type
        avalon_gate avl_gate;
        type_instance gate_instance = avl_gate.get_type_instance();

        // cgate type
        avalon_cgate avl_cgate;
        type_instance cgate_instance = avl_cgate.get_type_instance();

        // make sure we got only three arguments
        if(arguments.size() != 3)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects only three arguments.");

        // make sure the first argument is a call expression and the second a reference expression
        std::shared_ptr<expr>& arg_one = arguments[0];
        std::shared_ptr<expr>& arg_two = arguments[1];
        std::shared_ptr<expr>& arg_three = arguments[2];
        if(arg_one -> is_call_expression() == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the first argument to be a gate definition.");
        if(arg_two -> is_reference_expression() == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the second argument to be a reference to a quantum variable.");
        if(arg_two -> is_reference_expression() == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the second argument to be a reference to a quantum variable.");

        // we cast the arguments
        std::shared_ptr<call_expression> const & arg_one_cons = std::static_pointer_cast<call_expression>(arg_one);
        std::shared_ptr<reference_expression> const & arg_two_ref = std::static_pointer_cast<reference_expression>(arg_two);
        std::shared_ptr<reference_expression> const & arg_three_ref = std::static_pointer_cast<reference_expression>(arg_three);

        // we double check the type instances
        type_instance arg_one_instance = arg_one_cons -> get_type_instance();
        if(type_instance_strong_compare(arg_one_instance, cgate_instance) == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the first argument to be a gate definition of type <cgate>.");

        type_instance arg_two_instance = arg_two_ref -> get_type_instance();
        if(arg_two_instance.is_reference() == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the second argument to be a reference to a single <qubit>.");
        type_instance arg_two_instance_dref = arg_two_instance.get_params()[0];
        if(type_instance_strong_compare(arg_two_instance_dref, qubit_instance) == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the second argument to be a reference to a single <qubit> variable. Instead we got <" + mangle_type_instance(arg_two_instance_dref) + ">.");

        type_instance arg_three_instance = arg_three_ref -> get_type_instance();
        if(arg_three_instance.is_reference() == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the third argument to be a reference to a single <qubit>.");
        type_instance arg_three_instance_dref = arg_three_instance.get_params()[0];
        if(type_instance_strong_compare(arg_three_instance_dref, qubit_instance) == false)
            throw invalid_call("[compiler error] the quantum <apply> function for controlled gates expects the third argument to be a reference to a single <qubit> variable. Instead we got <" + mangle_type_instance(arg_three_instance_dref) + ">.");

        // transform the gate definition into an Eigen matrix
        // 0. We get the gate that parametrizes this controlled gate
        std::vector<std::pair<token, std::shared_ptr<expr> > >& cgate_args = arg_one_cons -> get_arguments();
        std::shared_ptr<call_expression> const & cgate_arg = std::static_pointer_cast<call_expression>(cgate_args[0].second);
        // 1. first, we get the gate parameters and transform them into double values

        std::vector<std::pair<token, std::shared_ptr<expr> > >& gate_args = cgate_arg -> get_arguments();
        // work on theta
        std::shared_ptr<expr> theta_expr = gate_args[0].second;
        std::shared_ptr<literal_expression> const & theta_lit = std::static_pointer_cast<literal_expression>(theta_expr);
        double theta = theta_lit -> get_float_value();
        // work on phi
        std::shared_ptr<expr> phi_expr = gate_args[1].second;
        std::shared_ptr<literal_expression> const & phi_lit = std::static_pointer_cast<literal_expression>(phi_expr);
        double phi = phi_lit -> get_float_value();
        // work on lambda
        std::shared_ptr<expr> lambda_expr = gate_args[2].second;
        std::shared_ptr<literal_expression> const & lambda_lit = std::static_pointer_cast<literal_expression>(lambda_expr);
        double lambda = lambda_lit -> get_float_value();

        // 2. we built the 2x2 Eigen matrix to be used as control gate
        Eigen::MatrixXcd unitary(2, 2);
        unitary(0, 0) = get_00(theta, phi, lambda);
        unitary(1, 0) = get_10(theta, phi, lambda);
        unitary(0, 1) = get_01(theta, phi, lambda);
        unitary(1, 1) = get_11(theta, phi, lambda);

        // we now apply the gate to the ket contained in the second argument
        // 1. get the content of the first referenced variable (argument two)
        std::shared_ptr<variable>& ref_var_two = arg_two_ref -> get_variable();
        std::shared_ptr<expr>& var_expr_two = ref_var_two -> get_value();
        std::shared_ptr<literal_expression> const & qubit_expr_two = std::static_pointer_cast<literal_expression>(var_expr_two);
        // we make sure that the qubit was not measured already
        if(qubit_expr_two -> was_measured())
            throw invalid_call("[compiler error] the quantum <apply> function second argument has already been measured and further gates cannot be applied to it.");

        // 1. get the content of the second referenced variable (argument three)
        std::shared_ptr<variable>& ref_var_three = arg_three_ref -> get_variable();
        std::shared_ptr<expr>& var_expr_three = ref_var_three -> get_value();
        std::shared_ptr<literal_expression> const & qubit_expr_three = std::static_pointer_cast<literal_expression>(var_expr_three);
        // we make sure that the qubit was not measured already
        if(qubit_expr_three -> was_measured())
            throw invalid_call("[compiler error] the quantum <apply> function third argument has already been measured and further gates cannot be applied to it.");

        // 2. apply the controlled operation
        qproc -> capply(unitary, qubit_expr_two -> get_start_index(), qubit_expr_three -> get_end_index());

        // DONE.
        return nullptr;
    }

    /**
     * qubit_measure
     * performs a measure on the qubits stored at the given reference in the Z basis
     */
    std::shared_ptr<expr> qubit_measure(std::shared_ptr<qprocessor>& qproc, std::vector<std::shared_ptr<expr> >& arguments) {
        // qubit type
        avalon_qubit avl_qubit;
        type_instance qubit_instance = avl_qubit.get_type_instance();

        // bit type
        avalon_bit avl_bit;
        type_instance bit_instance = avl_bit.get_type_instance();

        // make sure we got only one argument
        if(arguments.size() != 1)
            throw invalid_call("[compiler error] the quantum <measure> function expects only one argument.");

        // make sure the argument a reference expression
        std::shared_ptr<expr>& arg = arguments[0];
        if(arg -> is_reference_expression() == false)
            throw invalid_call("[compiler error] the quantum <measure> function expects the argument to be a reference to a quantum variable.");

        // we cast the argument
        std::shared_ptr<reference_expression> const & arg_ref = std::static_pointer_cast<reference_expression>(arg);

        // we double check the type instance
        type_instance arg_instance = arg_ref -> get_type_instance();
        type_instance arg_instance_dref = arg_instance.get_params()[0];
        if(type_instance_strong_compare(arg_instance_dref, qubit_instance) == false)
            throw invalid_call("[compiler error] the quantum <measure> function expects the argument to be a reference to a single <qubit> variable.");

        // get the qubit held inside the variable
        // 1. get the content of the referenced variable
        std::shared_ptr<variable>& ref_var = arg_ref -> get_variable();
        std::shared_ptr<expr>& var_expr = ref_var -> get_value();
        std::shared_ptr<literal_expression> const & qubit_expr = std::static_pointer_cast<literal_expression>(var_expr);

        // 2. check if the qubit has not been measured before
        // TODO: it doesn't make sense to throw an exception here - best to just return the existing measurement
        if(qubit_expr -> was_measured())
            throw invalid_call("[compiler error] The qubit given has already been measured");

        // 3. perform the measurement
        std::vector<qpp::idx> results = qproc -> measure(qubit_expr -> get_start_index(), qubit_expr -> get_end_index());
        qpp::idx result = results[0];

        // 4. we set the qubit measurement result
        qubit_expr -> was_measured(true);
        qpp::ket l_ket = result == 0 ? qpp::mket({0}) : qpp::mket({1});
        qubit_expr -> set_qubit_value(l_ket);

        // 5. Create a bit expression with the measurement result
        std::string res_str = std::to_string(result);
        token lit_tok(BITS, res_str, 0, 0, "__bil__");
        std::shared_ptr<literal_expression> res_lit = std::make_shared<literal_expression>(lit_tok, BIT_EXPR, res_str);
        res_lit -> set_type_instance(bit_instance);

        return res_lit;
    }

    /**
     * qubit_cast
     * Performs a measurement of the qubits stored at the given reference in the Z basis
     */
    std::shared_ptr<expr> qubit_cast(std::shared_ptr<qprocessor>& qproc, std::vector<std::shared_ptr<expr> >& arguments, type_instance& ret_instance) {
        // bit type
        avalon_bit avl_bit;
        type_instance bit_instance = avl_bit.get_type_instance();

        if(type_instance_strong_compare(ret_instance, bit_instance)) {
            return qubit_measure(qproc, arguments);
        }
        else {
            throw invalid_call("[compiler error] the qubit __cast__ function cannot cast to <" + mangle_type_instance(ret_instance) + ">.");
        }
    }
}


#ifndef INCLUDE_OKVIS_INVERSE_TRANSFORM_MULTIPLY_JACOBIAN_HPP
#define INCLUDE_OKVIS_INVERSE_TRANSFORM_MULTIPLY_JACOBIAN_HPP

#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>

namespace swift_vio {
// Jacobians for $T_z = T_x^{-1} * T_y$.
class InverseTransformMultiplyJacobian {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  InverseTransformMultiplyJacobian() {}
  InverseTransformMultiplyJacobian(
      const okvis::kinematics::Transformation& T_WA,
      const okvis::kinematics::Transformation& T_WB)
      : T_WA_(T_WA.r(), T_WA.q()), T_WB_(T_WB.r(), T_WB.q()) {}
  void initialize(const okvis::kinematics::Transformation& T_WA,
                  const okvis::kinematics::Transformation& T_WB) {
    T_WA_.first = T_WA.r();
    T_WA_.second = T_WA.q();
    T_WB_.first = T_WB.r();
    T_WB_.second = T_WB.q();
  }
  inline okvis::kinematics::Transformation multiplyT() const {
    Eigen::Quaterniond q_AB = T_WA_.second.conjugate() * T_WB_.second;
    Eigen::Vector3d t_AB =
        T_WA_.second.conjugate() * (T_WB_.first - T_WA_.first);
    return okvis::kinematics::Transformation(t_AB, q_AB);
  }

  void dp_dp_WA(Eigen::Matrix3d* j) {
    *j = -T_WA_.second.toRotationMatrix().transpose();
  }

  void dp_dtheta_WA(Eigen::Matrix3d* j) {
    Eigen::Vector3d t_AB =
        T_WA_.second.conjugate() * (T_WB_.first - T_WA_.first);
    *j = okvis::kinematics::crossMx(t_AB) *
         T_WA_.second.toRotationMatrix().transpose();
  }

  void dp_dp_WB(Eigen::Matrix3d* j) {
    *j = T_WA_.second.toRotationMatrix().transpose();
  }
  void dtheta_dtheta_WA(Eigen::Matrix3d* j) {
    *j = -T_WA_.second.toRotationMatrix().transpose();
  }
  void dtheta_dtheta_WB(Eigen::Matrix3d* j) {
    *j = T_WA_.second.toRotationMatrix().transpose();
  }

  void dT_dT_WA(Eigen::Matrix<double, 6, 6>* Jzx) {
    Jzx->topLeftCorner<3, 3>() = -T_WA_.second.toRotationMatrix().transpose();
    Eigen::Vector3d t_AB =
        T_WA_.second.conjugate() * (T_WB_.first - T_WA_.first);
    Jzx->topRightCorner<3, 3>() = okvis::kinematics::crossMx(t_AB) *
        T_WA_.second.toRotationMatrix().transpose();
    Jzx->bottomLeftCorner<3, 3>().setZero();
    Jzx->bottomRightCorner<3, 3>() =  -T_WA_.second.toRotationMatrix().transpose();
  }

  void dT_dT_WB(Eigen::Matrix<double, 6, 6>* Jzy) {
    Jzy->topLeftCorner<3, 3>() = T_WA_.second.toRotationMatrix().transpose();
    Jzy->topRightCorner<3, 3>().setZero();
    Jzy->bottomLeftCorner<3, 3>().setZero();
    Jzy->bottomRightCorner<3, 3>() = T_WA_.second.toRotationMatrix().transpose();
  }

 private:
  std::pair<Eigen::Vector3d, Eigen::Quaterniond> T_WA_;
  std::pair<Eigen::Vector3d, Eigen::Quaterniond> T_WB_;
};
}  // namespace swift_vio
#endif // INCLUDE_OKVIS_INVERSE_TRANSFORM_MULTIPLY_JACOBIAN_HPP


/******************************************************************************
 * Copyright 2017-2018 Baidu Robotic Vision Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "stdafx.h"
#include "Intrinsic.h"
#include "Parameter.h"
#include <Eigen/Core>

#include <iostream>
#ifdef CFG_DEBUG
//#define FTR_UNDIST_VERBOSE  1
//#define FTR_UNDIST_VERBOSE  2
#endif
#define FTR_UNDIST_DOG_LEG

void Intrinsic::UndistortionMap::Set(const Intrinsic &K) {
  const float sx = float(FTR_UNDIST_LUT_SIZE - 1) / K.w();
  const float sy = float(FTR_UNDIST_LUT_SIZE - 1) / K.h();
  m_fx = sx * K.k().m_fx; m_fy = sy * K.k().m_fy;//将图片缩放到FTR_UNDIST_LUT_SIZE×FTR_UNDIST_LUT_SIZEsize,然后进行畸变表的存储
  m_cx = sx * K.k().m_cx; m_cy = sy * K.k().m_cy;
  const float fxI = 1.0f / m_fx, fyI = 1.0f / m_fy;
  m_xns.Resize(FTR_UNDIST_LUT_SIZE, FTR_UNDIST_LUT_SIZE);//11*11这个在干吗？

#ifdef FTR_UNDIST_VERBOSE
  UT::PrintSeparator();
#endif
  Point2D xd, xn;
  float v;
  int x[2] = {FTR_UNDIST_LUT_SIZE / 2, FTR_UNDIST_LUT_SIZE / 2};
  int b[2][2] = {{x[0] - 1, x[0] + 1}, {x[1] - 1, x[1] + 1}};
  const int d[2] = {-1, 1};
  const int N = m_xns.Size();
  for(int i = 0, ix = 0, id = 0; i < N; ++i) {
    xd.x() = fxI * (x[0] - m_cx);
    xd.y() = fyI * (x[1] - m_cy);
    if (i == 0) {
      xn = xd;
    } else {
      xn = xd * v;
    }
    if (!K.Undistort(xd, &xn, NULL, NULL, true)) {
        continue;
//      exit(0);
    }
    v = sqrtf(xn.SquaredLength() / xd.SquaredLength());//大概的一个畸变比例
//#ifdef CFG_DEBUG
#if 0
    xn.x() = v;
#endif
    m_xns[x[1]][x[0]] = xn;//对应的无畸变的归一化坐标的值
//#ifdef CFG_DEBUG
#if 0
    //UT::Print("%d %d\n", x[0], x[1]);
    const float s = 0.5f;
    const int _w = K.gyr(), _h = K.h();
    const float _fx = K.fx() * s, _fy = K.fy() * s;
    const float _cx = (_w - 1) * 0.5f, _cy = (_h - 1) * 0.5f;
    CVD::Image<CVD::Rgb<ubyte> > I;
    I.resize(CVD::ImageRef(_w, _h));
    I.zero();
    UT::ImageDrawCross(I, int(_fx * xd.x() + _cx), int(_fy * xd.y() + _cy), 5, CVD::Rgb<ubyte>(255, 0, 0));
    UT::ImageDrawCross(I, int(_fx * xn.x() + _cx), int(_fy * xn.y() + _cy), 5, CVD::Rgb<ubyte>(0, 255, 0));
    UT::ImageSave(UT::String("D:/tmp/test%04d.jpg", i), I);
#endif
    //这里就是把 0,0 - 10,10范围内的都遍历一遍,这些像素点假设是带畸变的,m_xns保存的是对应的无畸变的归一话坐标的值
    if (x[ix] == b[ix][id]) {
      b[ix][id] += d[id];
      ix = 1 - ix;
      if (ix == 0) {
        id = 1 - id;
      }
    }
    x[ix] += d[id];
  }
#ifdef FTR_UNDIST_VERBOSE
  UT::PrintSeparator();
#endif
}
//去畸变,如果没有UndistortionMap畸变对照表的时候,需要将图片缩到FTR_UNDIST_LUT_SIZE*FTR_UNDIST_LUT_SIZE去做畸变对照。
//如果没有UndistortionMap输入或者生成UndistortionMap的时候,会用dogleg方法进行迭代求解。如果用了UndistortionMap作为初值的话,就用G-N求解即可
bool Intrinsic::Undistort(const Point2D &xd/*归一化坐标前两维*/, Point2D *xn/*待优化变量,最终反映的是无失真归一化坐标*/,
        LA::AlignedMatrix2x2f *JT/*畸变的雅克比J.t*/,UndistortionMap *UM/*畸变对应表*/, const bool initialized) const {
#ifdef CFG_DEBUG
  if (FishEye()) {
    UT::Error("TODO (haomin)\n");
  }
#endif
  if (!NeedUndistortion()) {//如果不需要去畸变
    *xn = xd;
    JT->MakeIdentity();
    return true;
  }
  float dr, j, dx2;
  LA::AlignedMatrix2x2f J;
  LA::SymmetricMatrix2x2f A;
  LA::AlignedMatrix2x2f AI;
  LA::Vector2f e, dx;//e就是残差
#ifdef FTR_UNDIST_DOG_LEG
  float dx2GN/*G-N解出的步长的欧式距离*/, dx2GD/*pU点的欧式距离*/, delta2/*信赖域半径*/, beta;
  LA::Vector2f dxGN/*G-N解出的增量*/, dxGD/*pU点*/;
  bool update, converge ,success;
    success = false;
#endif
  if (UM) {//如果预先做了畸变表,然么就用这里的畸变作为初值
    if (UM->Empty()) {
      UM->Set(*this);//如果没有畸变表，需要生成,因为只是做初值,所以就用FTR_UNDIST_LUT_SIZE*FTR_UNDIST_LUT_SIZE去做就好了,
      // key是畸变的像素坐标,value是无畸变的归一化坐标的值
    }
    *xn = UM->Get(xd);//用畸变表里对应的无畸变的归一化坐标作为初值
//#ifdef CFG_DEBUG
#if 0
    *xn = xd * xn->x();
#endif
  } else if (!initialized) {
    *xn = xd;//给个初值
  }
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 1
  const Point2D _xd = m_k.GetNormalizedToImage(xd);
  UT::Print("x = %03d %03d", int(_xd.x() + 0.5f), int(_xd.y() + 0.5f));
  UT::Print("  e = %f", sqrtf((GetDistorted(*xn) - xd).SquaredLength() * m_k.m_fx * m_k.m_fy));
#endif
  const float *ds/*0_k1,1_k2,2_p1,3_p2,4_k3,5_k4,6_k5,7_k6*/ = m_k.m_ds, *jds = m_k.m_jds;//jds就是为了雅克比计算村的参数
  const float dx2Conv = FTR_UNDIST_CONVERGE * fxyI();//收敛条件
  //const float dx2Conv = FTR_UNDIST_CONVERGE * m_k.m_fxyI;
#ifdef FTR_UNDIST_DOG_LEG
  delta2 = FTR_UNDIST_DL_RADIUS_INITIAL;//初始化一下信赖域半径
#endif
  for (int iIter = 0; iIter < FTR_UNDIST_MAX_ITERATIONS; ++iIter)
  {//最大迭代10次
#if 0
//#if 1
    if (UT::Debugging()) {
      UT::Print("%d %e %e\n", iIter, xn->x(), xn->y());
    }
#endif
    ////针对k1,k2,k3,p1,p2而言 r^2 = x^2 + y^2
    ////残差2*1 归一化坐标的2维： min F(x,y) = 0.5*r(x,y)^2 约束： x^2+y^2 <=
    ///      r.x = x*(1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p1*x*y + p2*(r^2 + 2*x^2) - x0
    ////     r.y = y*(1 + k1*r^2 + k2*r^4 + k3*r^6) + 2*p2*x*y + p1*(r^2 + 2*y^2) - y0
    ////迭代法求,这里用的狗腿。下面是雅克比矩阵J,代码里是先给J矩阵径向部分然后再给切向部分
    ////J00 = div(r.x)/div(x) = (1 + k1*r^2 + k2*r^4 + k3*r^6) + (k1 + 2*k2*r^2 + 3*k3*r^4)*2*x^2 + 2*p1*y + 6*p2*x
    ////J01 = div(r.x)/div(y) = (k1 + 2*k2*r^2 + 3*k3*r^4)*x*y + 2*p1*x + 2*p2*y
    ////J10 = div(r.y)/div(x) = (k1 + 2*k2*r^2 + 3*k3*r^4)*x*y + 2*p1*x + 2*p2*y
    ////J11 = div(r.y)/div(y) = (1 + k1*r^2 + k2*r^4 + k3*r^6) + (k1 + 2*k2*r^2 + 3*k3*r^4)*2*y^2 + 6*p1*y + 2*p2*x
    const float x = xn->x(), x2 = x * x, y = xn->y(), y2 = y * y, xy = x * y;
    if(!m_fishEye)
    {
        const float r2 = x2 + y2, r4 = r2 * r2, r6 = r2 * r4;
        dr = ds[4] * r6 + ds[1] * r4 + ds[0] * r2 + 1.0f;// 1 + k1*r^2 + k2*r^4 + k3*r^6
        j = jds[4] * r4 + jds[1] * r2 + ds[0];//k1 + 2*k2*r^2 + 3*k3*r^4
        if (m_radial6)
        {
            const float dr2I = 1.0f / (ds[7] * r6 + ds[6] * r4 + ds[5] * r2 + 1.0f);
            dr *= dr2I;
            j = (j - (jds[7] * r4 + jds[6] * r2 + ds[5]) * dr) * dr2I;
        }
        //进行径向畸变
        e.x() = dr * x;//x*(1 + k1*r^2 + k2*r^4 + k3*r^6)
        e.y() = dr * y;//y*(1 + k1*r^2 + k2*r^4 + k3*r^6)
        //先对径向畸变的雅克比进行赋值
        J.m00() = j * (x2 + x2) + dr;//(1 + k1*r^2 + k2*r^4 + k3*r^6) + (k1 + 2*k2*r^2 + 3*k3*r^4)*2*x^2
        J.m01() = j * (xy + xy);//(k1 + 2*k2*r^2 + 3*k3*r^4)*x*y
        J.m11() = j * (y2 + y2) + dr;//(1 + k1*r^2 + k2*r^4 + k3*r^6) + (k1 + 2*k2*r^2 + 3*k3*r^4)*2*y^2
//#ifdef CFG_DEBUG
#if 0
        J.m00() = 1 - 3 * x2 - y2;
    J.m01() = -2 * xy;
    J.m11() = 1 - x2 - 3 * y2;
#endif
        if (m_tangential) {
            //切向畸变部分
            const float dx = jds[2] * xy + ds[3] * (r2 + x2 + x2);//2*p1*x*y + p2*(r^2 + 2*x^2)
            const float dy = jds[3] * xy + ds[2] * (r2 + y2 + y2);//2*p2*x*y + p1*(r^2 + 2*y^2)
            e.x() = dx + e.x();//在径向畸变后进行切向畸变
            e.y() = dy + e.y();
            const float d2x = jds[2] * x, d2y = jds[2] * y;//
            const float d3x = jds[3] * x, d3y = jds[3] * y;
            J.m00() = d3x + d3x + d3x + d2y + J.m00();// 2*p1*y + 6*p2*x + 径向雅克比
            J.m01() = d2x + d3y + J.m01();//2*p1*x + 2*p2*y + 径向雅克比
            J.m11() = d3x + d2y + d2y + d2y + J.m11();//6*p1*y + 2*p2*x + 径向雅克比
        }
        J.m10() = J.m01();
    }else
    {
        ////针对k1,k2,k2,k3的等距投影模型, r^2 = x^2 + y^2
        ////残差2*1 归一化坐标的2维： min F(x,y) = 0.5*r(x,y)^2 约束： x^2+y^2 <=
        ///      r.x = x*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - x0
        ////     r.x = y*(theta*(1 + k1*theta^2 + k2*theta^4 + k3 *theta^6 + k4*theta^8)/r) - y0
        // distortion first:
        const float u0 = x;//x
        const float u1 = y;//y
        const float r = sqrt(u0 * u0 + u1 * u1);//r = sqrt(x^2 + y^2)
        const float theta = atan(r);
        const float theta2 = theta * theta;
        const float theta4 = theta2 * theta2;
        const float theta6 = theta4 * theta2;
        const float theta8 = theta4 * theta4;
        const float thetad = theta * (1 + ds[0] * theta2 + ds[1] * theta4 + ds[2] * theta6 + ds[3] * theta8);

        const float scaling = (r > 1e-8) ? thetad / r : 1.0;
        e.x() = scaling * u0;
        e.y() = scaling * u1;

        if (r > 1e-8){
            // mostly matlab generated...
            float t2;//x^2
            float t3;//y^2
            float t4;//r^2 = x^2+y^2
            float t6;//theta
            float t7;//theta^2
            float t8;// 1/r
            float t9;//theta^4
            float t11;// 1 / ((x^2+y^2) + 1.0);
            float t17;// (((k1_ * theta^2 + k2_ * theta^4) + k3_ * t7 * t9) + k4_ * (t9 * t9)) + 1.0
            float t18;
            float t19;
            float t20;
            float t25;

            t2 = u0 * u0;
            t3 = u1 * u1;
            t4 = t2 + t3;
            t6 = atan(sqrt(t4));
            t7 = t6 * t6;
            t8 = 1.0 / sqrt(t4);
            t9 = t7 * t7;
            t11 = 1.0 / ((t2 + t3) + 1.0);
            t17 = (((ds[0] * t7 + ds[1] * t9) + ds[2] * t7 * t9) + ds[3] * (t9 * t9)) + 1.0;
            t18 = 1.0 / t4;
            t19 = 1.0 / sqrt(t4 * t4 * t4);
            t20 = t6 * t8 * t17;
            t25 = ((ds[1] * t6 * t7 * t8 * t11 * u1 * 4.0
                    + ds[2] * t6 * t8 * t9 * t11 * u1 * 6.0)
                   + ds[3] * t6 * t7 * t8 * t9 * t11 * u1 * 8.0)
                  + ds[0] * t6 * t8 * t11 * u1 * 2.0;
            t4 = ((ds[1] * t6 * t7 * t8 * t11 * u0 * 4.0
                   + ds[2] * t6 * t8 * t9 * t11 * u0 * 6.0)
                  + ds[3] * t6 * t7 * t8 * t9 * t11 * u0 * 8.0)
                 + ds[0] * t6 * t8 * t11 * u0 * 2.0;
            t7 = t11 * t17 * t18 * u0 * u1;
            J.m01() = (t7 + t6 * t8 * t25 * u0) - t6 * t17 * t19 * u0 * u1;
            J.m11() = ((t20 - t3 * t6 * t17 * t19) + t3 * t11 * t17 * t18)
                      + t6 * t8 * t25 * u1;
            J.m00() = ((t20 - t2 * t6 * t17 * t19) + t2 * t11 * t17 * t18)
                      + t6 * t8 * t4 * u0;
            J.m10() = (t7 + t6 * t8 * t4 * u1) - t6 * t17 * t19 * u0 * u1;
        } else {
            J.m00() = 1;
            J.m01() = 0;
            J.m11() = 1;
            J.m10() = 0;
        }
    }
    e -= xd;//计算残差
    LA::SymmetricMatrix2x2f::AAT(J, A);//构造H矩阵
//#ifdef CFG_DEBUG
#if 0
    A.Set(J.m00(), J.m01(), J.m11());
#endif
    const LA::Vector2f b = J * e;//构造Ax = -b
    if (!A.GetInverse(AI)) {//求解A逆
      //return false;
      break;
    }

    LA::AlignedMatrix2x2f::Ab(AI, b, dx);//求解-x

      dx.MakeMinus();//求出x
      dx2 = dx.SquaredLength();//x的欧式距离
#ifdef FTR_UNDIST_DOG_LEG
    if (!UM)
    {
      dxGN = dx;//G-N法求出的增量
      dx2GN = dx2;//G-N法求出的增量的欧式距离
      dx2GD = 0.0f;//dogleg
      const float F = e.SquaredLength();//dogleg迭代开始前残差的欧式距离
      const Point2D xnBkp = *xn;//待优化变量
      update = true;
      converge = false;//狗腿迭代次数
      for (int iIterDL = 0; iIterDL < FTR_UNDIST_DL_MAX_ITERATIONS; ++iIterDL) {
        if (dx2GN > delta2 && dx2GD == 0.0f) {//如果G-N增量在信赖域外且dx2GD还没有初始化
          const float bl = sqrtf(b.SquaredLength());//模长
          const LA::Vector2f g = b * (1.0f / bl);//梯度方向
          const LA::Vector2f Ag = A * g;
          const float xl = bl / g.Dot(Ag);//计算pU点的步长
          g.GetScaled(-xl, dxGD);//负梯度*步长,pU点
          dx2GD = xl * xl;//pU点的半径
#ifdef CFG_DEBUG
         UT::AssertEqual(dxGD.SquaredLength(), dx2GD);
#endif
        }
        //三种情况,1：GN极值点在域内直接变成无约束条件 2:GN和pU点都在域外,那么就在给pU点的步长一个比例因子(域半径/自己的步长^2（因为用的是最小2乘）),让它刚好落在域半径上
        //3:GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
        if (dx2GN <= delta2) {//如果G-N的极值在信赖域内,那么就是一个无约束问题,就直接用GN法求出的增量就可以
          dx = dxGN;
          dx2 = dx2GN;
          beta = 1.0f;
        } else if (dx2GD >= delta2) {//如果G-N和pU点求的最优点都在信赖域外
          if (delta2 == 0.0f) {//信赖域为0,直接用pU点求得最优值
            dx = dxGD;
            dx2 = dx2GD;
          } else {
            dxGD.GetScaled(sqrtf(delta2 / dx2GD), dx);//乘比例因子
            dx2 = delta2;
          }
          beta = 0.0f;
        } else {//GN在域外,pU点在域内,那么增量就是GN极值点和pU点的连线与信赖域的交点
          const LA::Vector2f v = dxGN - dxGD;//方向
          const float d = dxGD.Dot(v), v2 = v.SquaredLength();
          //beta = float((-d + sqrt(double(d) * d + (delta2 - dx2GD) * double(v2))) / v2);
          beta = (-d + sqrtf(d * d + (delta2 - dx2GD) * v2)) / v2;//算得是在域外那段连线的长度
          dx = dxGD;
          dx += v * beta;
          dx2 = delta2;
        }
        *xn += dx;//加上这一次的增量
        const float dFa = F - (GetDistorted(*xn) - xd).SquaredLength();//实际下降的
        const float dFp = F - (e + J * dx).SquaredLength();//理论下降值,直接用J*dx近似下降值了
        const float rho = dFa > 0.0f && dFp > 0.0f ? dFa / dFp : -1.0f;//求实际/理论的比值,理论不可能为负,实际为负的时候拒绝这次更新
        //信赖域： Numerical Optimization 第二版 p69

          //rho < 0.25 如果大于0说明近似的不好,需要减小信赖域,减小近似的范围。如果<0就说明是错误的近似,那么就拒绝这次的增量
        if (rho < FTR_UNDIST_DL_GAIN_RATIO_MIN) {
          delta2 *= FTR_UNDIST_DL_RADIUS_FACTOR_DECREASE;
          if (delta2 < FTR_UNDIST_DL_RADIUS_MIN) {
            delta2 = FTR_UNDIST_DL_RADIUS_MIN;
          }
          *xn = xnBkp;//取消这次增量
          update = false;//不更新
          converge = false;
          continue;
        } else if (rho > FTR_UNDIST_DL_GAIN_RATIO_MAX) //rho > 0.75,可以扩大信赖域半径
        {
          delta2 = std::max(delta2, FTR_UNDIST_DL_RADIUS_FACTOR_INCREASE * dx2);
          if (delta2 > FTR_UNDIST_DL_RADIUS_MAX) {//信赖域半径最大值
            delta2 = FTR_UNDIST_DL_RADIUS_MAX;
          }
        }
        update = true;//
          const double chi2 = e.SquaredLength();;
          if (chi2 < dx2Conv * 1e5) {
              success = true;
          }
        converge = dx2 < dx2Conv;//增量小于阈值,认为收敛
        break;
      }
      if (!update || converge) {
          success = true;
        break;
      }
    } else
#endif
    {
      *xn += dx;
//      std::cout<<"iIter:"<<iIter<<" "<<"dx2:"<<dx2<<std::endl;
          const double chi2 = e.SquaredLength();;
          if (chi2 < dx2Conv * 1e5) {
              success = true;
          }
      if (dx2 < dx2Conv) {//收敛
          success = true;
        break;
      }
    }
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 2
    const std::string str = UT::String("%02d  ", iIter);
    if (iIter == 0) {
      UT::PrintSeparator();
      m_k.GetNormalizedToImage(xd).Print(std::string(str.size(), ' ') + "x = ", false, true);
    }
    GetNormalizedToImage(xnBkp).Print(str + "x = ", false, false);
    UT::Print("  e = %f  dx = %f  beta = %f\n", sqrtf(F * fxy()), sqrtf(dx2 * fxy()), beta);
#endif
  }
  if (JT) {
    J.GetTranspose(*JT);
  }
#if defined FTR_UNDIST_VERBOSE && FTR_UNDIST_VERBOSE == 1
  UT::Print(" --> %f\n", sqrtf((GetDistorted(*xn) - xd).SquaredLength() * m_k.m_fx * m_k.m_fy));
#endif
  return true;
}