/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);

/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points, look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


/*
 *
 *
 *
 */

#include <pcl/surface/on_nurbs/fitting_curve_2d_sdm.h>
#include <stdexcept>

using namespace pcl;
using namespace on_nurbs;

FittingCurve2dSDM::FittingCurve2dSDM (int order, NurbsDataCurve2d *data) :
  FittingCurve2dPDM (order, data)
{

}

FittingCurve2dSDM::FittingCurve2dSDM (NurbsDataCurve2d *data, const ON_NurbsCurve &ns) :
  FittingCurve2dPDM (data, ns)
{

}

void
FittingCurve2dSDM::assemble (const FittingCurve2dPDM::Parameter &parameter)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  int nCageReg = m_nurbs.m_cv_count - 2 * cp_red;
  int nInt = int (m_data->interior.size ());

  double wInt = 1.0;
  if (!m_data->interior_weight.empty ())
    wInt = m_data->interior_weight[0];

  unsigned nrows = 4 * nInt + 2 * nCageReg;

  m_solver.assign (nrows, ncp * 2, 1);

  unsigned row (0);

  if (wInt > 0.0)
    assembleInterior (wInt, parameter.rScale, row);

  if (parameter.smoothness > 0.0)
    addCageRegularisation (parameter.smoothness, row);

  if (row < nrows)
  {
    m_solver.resize (row);
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assemble] Warning: rows do not match: %d %d\n", row, nrows);
  }
}

double
FittingCurve2dSDM::solve (double damp)
{
  double cps_diff (0.0);

  if (m_solver.solve ())
    cps_diff = updateCurve (damp);

  return cps_diff;
}

double
FittingCurve2dSDM::updateCurve (double damp)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;

  double cps_diff (0.0);

  // TODO this implementation rotates the control points,

 look up fitting_curve_2d_apdm for correct implementation

  for (int j = 0; j < ncp; j++)
  {

    ON_3dPoint cp_prev;
    m_nurbs.GetCV (j + cp_red, cp_prev);

    double x = m_solver.x (2 * j + 0, 0);
    double y = m_solver.x (2 * j + 1, 0);

    cps_diff += sqrt ((x - cp_prev.x) * (x - cp_prev.x) + (y - cp_prev.y) * (y - cp_prev.y));

    ON_3dPoint cp;
    cp.x = cp_prev.x + damp * (x - cp_prev.x);
    cp.y = cp_prev.y + damp * (y - cp_prev.y);
    cp.z = 0.0;

    m_nurbs.SetCV (j + cp_red, cp);
  }

  for (int j = 0; j < cp_red; j++)
  {

    ON_3dPoint cp;
    m_nurbs.GetCV (m_nurbs.m_cv_count - 1 - cp_red + j, cp);
    m_nurbs.SetCV (j, cp);

    m_nurbs.GetCV (cp_red - j, cp);
    m_nurbs.SetCV (m_nurbs.m_cv_count - 1 - j, cp);
  }
  return cps_diff / ncp;
}

void
FittingCurve2dSDM::addPointConstraint (const double &param, const Eigen::Vector2d &point,
                                       const Eigen::Vector2d &normal, const Eigen::Vector2d &tangent, double rho,
                                       double d, double weight, unsigned &row)
{
  int cp_red = m_nurbs.m_order - 2;
  int ncp = m_nurbs.m_cv_count - 2 * cp_red;
  double *N = new double[m_nurbs.m_order * m_nurbs.m_order];

  int E = ON_NurbsSpanIndex (m_nurbs.m_order, m_nurbs.m_cv_count, m_nurbs.m_knot, param, 0, 0);

  ON_EvaluateNurbsBasis (m_nurbs.m_order, m_nurbs.m_knot + E, param, N);

  m_solver.f (row, 0, normal (0) * point (0) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 0, weight * normal (0) * N[i]);
  }
  row++;

  m_solver.f (row, 0, normal (1) * point (1) * weight);
  for (int i = 0; i < m_nurbs.m_order; i++)
  {
    m_solver.K (row, 2 * ((E + i) % ncp) + 1, weight * normal (1) * N[i]);
  }
  row++;

  if (d < 0.0)
  {

    double a = d / (d - rho);

    m_solver.f (row, 0, a * a * tangent (0) * point (0) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 0, a * a * weight * tangent (0) * N[i]);
    row++;

    m_solver.f (row, 0, a * a * tangent (1) * point (1) * weight);
    for (int i = 0; i < m_nurbs.m_order; i++)
      m_solver.K (row, 2 * ((E + i) % ncp) + 1, a * a * weight * tangent (1) * N[i]);
    row++;

  }

  delete [] N;
}

void
FittingCurve2dSDM::addCageRegularisation (double weight, unsigned &row)
{
  int cp_red = (m_nurbs.m_order - 2);
  int ncp = (m_nurbs.m_cv_count - 2 * cp_red);

  for (int j = 1; j < ncp + 1; j++)
  {
    m_solver.K (row, 2 * ((j + 0) % ncp) + 0, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 0, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 0, 1.0 * weight);
    row++;

    m_solver.K (row, 2 * ((j + 0) % ncp) + 1, -2.0 * weight);
    m_solver.K (row, 2 * ((j - 1) % ncp) + 1, 1.0 * weight);
    m_solver.K (row, 2 * ((j + 1) % ncp) + 1, 1.0 * weight);
    row++;
  }
}

void
FittingCurve2dSDM::assembleInterior (double wInt, double rScale, unsigned &row)
{
  unsigned nInt = int (m_data->interior.size ());
  m_data->interior_line_start.clear ();
  m_data->interior_line_end.clear ();
  m_data->interior_error.clear ();
  m_data->interior_normals.clear ();

  unsigned updateTNR (false);
  if (m_data->interior_ncps_prev != m_nurbs.CVCount ())
  {
    if (!m_quiet)
      printf ("[FittingCurve2dSDM::assembleInterior] updating T, N, rho\n");
    m_data->interior_tangents.clear ();
    m_data->interior_normals.clear ();
    m_data->interior_rho.clear ();
    m_data->interior_ncps_prev = m_nurbs.CVCount ();
    updateTNR = true;
  }

  for (unsigned p = 0; p < nInt; p++)
  {
    Eigen::Vector2d &pcp = m_data->interior[p];

    // inverse mapping
    double param;
    Eigen::Vector2d pt, t, n;
    double error;
    if (p < static_cast<unsigned int>(m_data->interior_param.size ()))
    {
      param = findClosestElementMidPoint (m_nurbs, pcp, m_data->interior_param[p]);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param[p] = param;
    }
    else
    {
      param = findClosestElementMidPoint (m_nurbs, pcp);
      param = inverseMapping (m_nurbs, pcp, param, error, pt, t, rScale, in_max_steps, in_accuracy, m_quiet);
      m_data->interior_param.push_back (param);
    }

    m_data->interior_error.push_back (error);

    double dt, kappa, rho, rho_prev;
    Eigen::Vector2d n_prev, t_prev;

    double pointAndTangents[6];
    m_nurbs.Evaluate (param, 2, 2, pointAndTangents);
    pt (0) = pointAndTangents[0];
    pt (1) = pointAndTangents[1];
    t (0) = pointAndTangents[2];
    t (1) = pointAndTangents[3];
    n (0) = pointAndTangents[4];
    n (1) = pointAndTangents[5];

    dt = t.norm ();
    t /= dt;
    Eigen::Vector2d in (t (1), -t (0));
    n /= dt; // TODO something is wrong with the normal from nurbs.Evaluate(...)
    n = in * in.dot (n);

    kappa = n.norm ();
    rho = (1.0 / kappa);
    n *= rho;

    if (!updateTNR && m_data->interior_rho.size () == nInt)
    {
      n_prev = m_data->interior_normals[p];
      t_prev = m_data->interior_tangents[p];
      rho_prev = m_data->interior_rho[p];
    }
    else
    {
      m_data->interior_tangents.push_back (t);
      m_data->interior_normals.push_back (n);
      m_data->interior_rho.push_back (rho);
      n_prev = n;
      t_prev = t;
      rho_prev = rho;
    }

    // signed distance
    double d;
    if ((pcp - pt).dot (n) >= 0.0)
      d = (pcp - pt).norm ();
    else
      d = -(pcp - pt).norm ();

    if (p < m_data->interior_weight.size ())
      wInt = m_data->interior_weight[p];

    m_data->interior_line_start.push_back (pt);
    m_data->interior_line_end.push_back (pcp);

    addPointConstraint (m_data->interior_param[p], m_data->interior[p], n_prev, t_prev, rho_prev, d, wInt, row);
  }

}


class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */

{
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


{
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B)


{
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B)

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        i

        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */


if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);

            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bound(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();


class Solution {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        if (A.size() > B.size()) {
            return smallestDifference(B, A);
        }
        sort(A.begin(), A.end());

        int min_diff = numeric_limits<int>::max();
        for (const auto& b : B) {
            auto it = lower_bou

nd(A.cbegin(), A.cend(), b);
            if (it != A.cend()) {
                min_diff = min(min_diff, *it - b);
            }
            if (it != A.cbegin()) {
                --it;
                min_diff = min(min_diff, b - *it);
            }
        }

        return min_diff;
    }
};

class Solution2 {
public:
    /**
     * @param A, B: Two integer arrays.
     * @return: Their smallest difference.
     */
    int smallestDifference(vector<int> &A, vector<int> &B) {
        sort(A.begin(), A.end());
        sort(B.begin(), B.end());

        int i = 0;
        int j = 0;
        int min_diff = numeric_limits<int>::max();

        while (i < A.size() && j < B.size()) {
            min_diff = min(min_diff, abs(A[i] - B[j]));
            A[i] < B[j] ? ++i : ++j;
        }

        return min_diff;
    }
};


cplus/github/marioyc/Online-Judge-Solutions/Timus Online Judge/1354 - Palindrome. Again Palindrome.cpp

#include <cstdio>
#include <cstring>

using namespace std;


#include <cstdio>
#include <cstring>

using namespace std;

int main()


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    printf("%s",s1);
    for(int i = k;i < L;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}


printf("%s",s1);
    for(int i = k;i < L;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}

#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    int pref[10001];

    pref[0] = 0;

    for(int i = 1,k = 0;i < L;++i){
        while(k > 0 && s2[k] != s2[i]) k = pref[k - 1];
        if(s2[k] == s2[i]) ++k;
        pref[i] = k;
    }

    int k = 0;

    for(int i = 0;i < L;++i){
        while(k > 0 && s1[i] != s2[k]) k = pref[k - 1];
        if(s1[i] == s2[k]) ++k;
        if(k == L) k = pref[k - 1];
    }


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    int pref[10001];

    pref[0] = 0;

    for(int i = 1,k = 0;i < L;++i){
        while(k > 0 && s2[k] != s2[i]) k = pref[k - 1];
        if(s2[k] == s2[i]) ++k;
        pref[i] = k;
    }

    for(int i = 0;i < L;++i){
        while(k > 0 && s1[i] != s2[k]) k = pref[k - 1];
        if(s1[i] == s2[k]) ++k;
        if(k == L) k = pref[k - 1];
    }

    printf("%s",s1);
    for(int i = k;i < L;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    int pref[10001];

    pref[0] = 0;

    for(int i = 1,k = 0;i < L;++i){
        while(k > 0 && s2[k] != s2[i]) k = pref[k - 1];
        if(s2[k] == s2[i]) ++k;
        pref[i] = k;
    }

    int k = 0;

    for(int i = 0;i < L;++i){
        while(k > 0 && s1[i] != s2[k]) k = pref[k - 1];

        if(k == L) k = pref[k - 1];
    }

    printf("%s",s1);
    for(int i = k;i < L;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    int pref[10001];

    pref[0] = 0;

    for(int i = 1,k = 0;i < L;++i){
        while(k > 0 && s2[k] != s2[i]) k = pref[k - 1];
        if(s2[k] == s2[i]) ++k;
        pref[i] = k;
    }

    int k = 0;

    for(int i = 0;i < L;++i){

        if(s1[i] == s2[k]) ++k;
        if(k == L) k = pref[k - 1];
    }

    printf("%s",s1);
    for(int i = k;i < L;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}


#include <cstdio>
#include <cstring>

using namespace std;

int main(){
    char s1[10001],s2[10001];

    scanf("%s",s1);

    int L = strlen(s1);

    for(int i = 0;i < L;++i) s2[L-1-i] = s1[i];

    int pref[10001];

    pref[0] = 0;

    for(int i = 1,k = 0;i < L;++i){
        while(k > 0 && s2[k] != s2[i]) k = pref[k - 1];
        if(s2[k] == s2[i]) ++k;
        pref[i] = k;
    }

    int k = 0;

    for(int i = 0;i < L;++i){
        while(k > 0 && s1[i] != s2[k]) k = pref[k - 1];
        if(s1[i] == s2[k]) ++k;
        if(k == L) k = pref[k - 1];
    }

    printf("%s",s1);
    for(int i = k;i < L

;++i) putchar(s2[i]);
    putchar('\n');

    return 0;
}


void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)

#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>

{
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);
        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


{
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }

#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)
    {
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);
        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)

};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


{
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);

#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)

        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)
    {
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }

#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)
    {
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);
        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adjacent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)
    {
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);
        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7

    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}


#include "support/pstl_test_config.h"

#include <execution>
#include <algorithm>

#include "support/utils.h"

using namespace TestUtils;

struct test_adj

acent_find
{
    template <typename Policy, typename Iterator, typename Pred>
    void
    operator()(Policy&& exec, Iterator first, Iterator last, Pred pred)
    {
        using namespace std;

        auto k = std::adjacent_find(first, last, pred);
        auto i = adjacent_find(exec, first, last, pred);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find with predicate");

        i = adjacent_find(exec, first, last);
        EXPECT_TRUE(i == k, "wrong return value from adjacent_find without predicate");
    }
};

template <typename T>
void
test_adjacent_find_by_type()
{

    size_t counts[] = {2, 3, 500};
    for (size_t c = 0; c < const_size(counts); ++c)
    {

        for (size_t e = 0; e < (counts[c] >= 64 ? 64 : (counts[c] == 2 ? 1 : 2)); ++e)
        {
            Sequence<T> in(counts[c], [](size_t v) -> T { return T(v); }); //fill 0...n
            in[e] = in[e + 1] = -1;                                        //make an adjacent pair

            auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
            EXPECT_TRUE(i == in.cbegin() + e, "std::adjacent_find returned wrong result");

            invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
            invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
        }
    }

    for (size_t expect = 0; expect < 1; ++expect)
    {
        Sequence<T> in(expect, [](size_t v) -> T { return T(v); }); //fill 0...n
        auto i = std::adjacent_find(in.cbegin(), in.cend(), std::equal_to<T>());
        EXPECT_TRUE(i == in.cbegin() + expect, "std::adjacent_find returned wrong result");

        invoke_on_all_policies(test_adjacent_find(), in.begin(), in.end(), std::equal_to<T>());
        invoke_on_all_policies(test_adjacent_find(), in.cbegin(), in.cend(), std::equal_to<T>());
    }

    Sequence<T> a1 = {5, 5, 5, 6, 7, 8, 9};
    invoke_on_all_policies(test_adjacent_find(), a1.begin(), a1.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.begin() + 1, a1.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a1.cbegin(), a1.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a1.cbegin() + 1, a1.cend(), std::equal_to<T>());

    Sequence<T> a2 = {5, 6, 7, 8, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.begin(), a2.end() - 1, std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend(), std::equal_to<T>());
    invoke_on_all_policies(test_adjacent_find(), a2.cbegin(), a2.cend() - 1, std::equal_to<T>());

    Sequence<T> a3 = {5, 6, 6, 6, 7, 9, 9, 9, 9};
    invoke_on_all_policies(test_adjacent_find(), a3.begin(), a3.end(), std::equal_to<T>());

    invoke_on_all_policies(test_adjacent_find(), a3.cbegin(), a3.cend(), std::equal_to<T>());
}

template <typename T>
struct test_non_const
{
    template <typename Policy, typename Iterator>
    void
    operator()(Policy&& exec, Iterator iter)
    {
        adjacent_find(exec, iter, iter, non_const(std::equal_to<T>()));
    }
};

int
main()
{

    test_adjacent_find_by_type<int32_t>();
    test_adjacent_find_by_type<float64_t>();

    test_algo_basic_single<int32_t>(run_for_rnd_bi<test_non_const<int32_t>>());

    std::cout << done() << std::endl;
    return 0;
}