<commit_before>#include "AdaptiveBoole.h"
#include <queue>
#include <algorithm>
#include <limits>

struct BooleHelper
{
  BooleHelper() : error(0.0){};
  std::vector<double> fa, fb, fc, fd, fe, ff, fg, fh, fi;
  std::vector<double> S, Sleft, Sright;
  double lowerlimit;
  double upperlimit;
  double epsilon;
  double error;
  bool operator<(const BooleHelper &rhs) const { return this->error / this->epsilon < rhs.error / rhs.epsilon; }
};

class AdaptiveBoole::BooleImpl
{
public:
  BooleImpl() : m_epsilon(1.0e-5), m_maximumDivisions(10){};
  std::vector<double> sumPieces(std::priority_queue<BooleHelper> &pieces);
  void createElement(const BooleHelper &mostError,BooleHelper &element, bool first);
  void splitElement(const BooleHelper &mostError,BooleHelper &element1,BooleHelper &element2);
  std::vector<double> integrate();
  std::function<std::vector<double>(std::deque<double> &evaluationPoints)> m_integrand;
  double m_lowerBound, m_upperBound, m_epsilon;
  std::vector<double> m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions;
  std::deque<double> m_evaluationPointsOuterDimensions;
  int m_maximumDivisions;
  std::unique_ptr<BooleImpl> clone();
};

std::unique_ptr<AdaptiveBoole::BooleImpl> AdaptiveBoole::BooleImpl::clone()
{
  return std::unique_ptr<AdaptiveBoole::BooleImpl>(new BooleImpl(*this));
}

void AdaptiveBoole::BooleImpl::createElement(const BooleHelper &mostError, BooleHelper& element, bool first)
{
  double a, b;
  if (first)
  {
    a = mostError.lowerlimit;
    b = (mostError.lowerlimit + mostError.upperlimit) / 2.0;
  }
  else
  {
    a = (mostError.lowerlimit + mostError.upperlimit) / 2.0;
    b = mostError.upperlimit;
  }
  double tmp = (b - a) / 8.0;
  double c = a + tmp;
  double e = a + 3.0 * tmp;
  double g = a + 5.0 * tmp;
  double i = a + 7.0 * tmp;
  if (first)
  {
    element.fa = std::move(mostError.fa);
    element.fb = mostError.ff;
    element.fd = std::move(mostError.fc);
    element.ff = std::move(mostError.fd);
    element.fh = std::move(mostError.fe);
    element.S = std::move(mostError.Sleft);
  }
  else
  {
    element.fa = std::move(mostError.ff);
    element.fb = std::move(mostError.fb);
    element.fd = std::move(mostError.fg);
    element.ff = std::move(mostError.fh);
    element.fh = std::move(mostError.fi);
    element.S = std::move(mostError.Sright);
  }

  if (m_lowerBoundsInnerDimensions.size() > 0)
  {
    AdaptiveBoole test;
    test.setFunction(m_integrand);
    test.setInterval(m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions);
    test.setMaximumDivisions(m_maximumDivisions);
    test.setPrecision(m_epsilon);
    m_evaluationPointsOuterDimensions[0] = c;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fc = test.integrate();
    m_evaluationPointsOuterDimensions[0] = e;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fe = test.integrate();
    m_evaluationPointsOuterDimensions[0] = g;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fg = test.integrate();
    m_evaluationPointsOuterDimensions[0] = i;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fi = test.integrate();
  }
  else
  {
    m_evaluationPointsOuterDimensions[0] = c;
    element.fc = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = e;
    element.fe = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = g;
    element.fg = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = i;
    element.fi = m_integrand(m_evaluationPointsOuterDimensions);
  }

  std::size_t size = element.fa.size();

  double prefactor = (b - a) / 180.0;
  element.Sleft.reserve(size);
  element.Sright.reserve(size);
  for (std::size_t i = 0; i < size; i++)
  {
    element.Sleft.emplace_back(prefactor * (7.0 * element.fa[i] + 32.0 * element.fc[i] + 12.0 * element.fd[i] +
                                            32.0 * element.fe[i] + 7.0 * element.ff[i]));
    element.Sright.emplace_back(prefactor * (7.0 * element.ff[i] + 32.0 * element.fg[i] + 12.0 * element.fh[i] +
                                             32.0 * element.fi[i] + 7.0 * element.fb[i]));
    element.error = std::max(element.error, std::abs(63.0 * (element.Sleft[i] + element.Sright[i] - element.S[i])));
  }
  element.lowerlimit = a;
  element.upperlimit = b;
  element.epsilon = std::max(mostError.epsilon * M_SQRT1_2, std::numeric_limits<double>::epsilon());
  // std::cout << element.epsilon << " " << element.error << std::endl;
}

void AdaptiveBoole::BooleImpl::splitElement(const BooleHelper &mostError, BooleHelper &element1, BooleHelper &element2)
{
  this->createElement(mostError, element1,true);
  this->createElement(mostError, element2,false);
  //return std::make_pair(element1, element2);
}

std::vector<double> AdaptiveBoole::BooleImpl::sumPieces(std::priority_queue<BooleHelper> &pieces)
{
  std::size_t size = pieces.top().Sleft.size();
  std::vector<double> sum(size);
  while (pieces.size() > 0)
  {
    const BooleHelper& element = pieces.top();
    for (std::size_t i = 0; i < size; i++)
    {
      double S2 = element.Sleft[i] + element.Sright[i];
      sum[i] += S2 + (S2 - element.S[i]) / 63.0;
    }
    pieces.pop();
  }
  return sum;
}

std::vector<double> AdaptiveBoole::BooleImpl::integrate()
{
  BooleHelper first;

  double a = m_lowerBound;
  double b = m_upperBound;

  double tmp = (b - a) / 8.0;
  double c = a + tmp;
  double d = a + 2.0 * tmp;
  double e = a + 3.0 * tmp;
  double f = a + 4.0 * tmp;
  double g = a + 5.0 * tmp;
  double h = a + 6.0 * tmp;
  double i = a + 7.0 * tmp;
  //std::cout << b << " " << a + 8.0 * tmp << std::endl;

  if (m_lowerBoundsInnerDimensions.size() > 0)
  {
    AdaptiveBoole test;
    test.setFunction(m_integrand);
    test.setInterval(m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions);
    test.setMaximumDivisions(m_maximumDivisions);
    test.setPrecision(m_epsilon);
    m_evaluationPointsOuterDimensions.push_front(a);
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fa = test.integrate();
    m_evaluationPointsOuterDimensions[0] = b;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fb = test.integrate();
    m_evaluationPointsOuterDimensions[0] = c;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fc = test.integrate();
    m_evaluationPointsOuterDimensions[0] = d;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fd = test.integrate();
    m_evaluationPointsOuterDimensions[0] = e;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fe = test.integrate();
    m_evaluationPointsOuterDimensions[0] = f;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.ff = test.integrate();
    m_evaluationPointsOuterDimensions[0] = g;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fg = test.integrate();
    m_evaluationPointsOuterDimensions[0] = h;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fh = test.integrate();
    m_evaluationPointsOuterDimensions[0] = i;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fi = test.integrate();
  }
  else
  {
    m_evaluationPointsOuterDimensions.push_front(a);
    first.fa = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = b;
    first.fb = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = c;
    first.fc = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = d;
    first.fd = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = e;
    first.fe = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = f;
    first.ff = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = g;
    first.fg = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = h;
    first.fh = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = i;
    first.fi = m_integrand(m_evaluationPointsOuterDimensions);
  }

  std::size_t size = first.fa.size();

  double prefactor = (b - a) / 180.0;
  first.S.reserve(size);
  first.Sleft.reserve(size);
  first.Sright.reserve(size);
  for (std::size_t i = 0; i < size; i++)
  {
    first.S.emplace_back(2.0 * prefactor * (7.0 * first.fa[i] + 32.0 * first.fd[i] + 12.0 * first.ff[i] +
                                            32.0 * first.fh[i] + 7.0 * first.fb[i]));
    first.Sleft.emplace_back(prefactor * (7.0 * first.fa[i] + 32.0 * first.fc[i] + 12.0 * first.fd[i] +
                                          32.0 * first.fe[i] + 7.0 * first.ff[i]));
    first.Sright.emplace_back(prefactor * (7.0 * first.ff[i] + 32.0 * first.fg[i] + 12.0 * first.fh[i] +
                                           32.0 * first.fi[i] + 7.0 * first.fb[i]));
    first.error = std::max(first.error, std::abs(63.0 * (first.Sleft[i] + first.Sright[i] - first.S[i])));
  }

  first.lowerlimit = m_lowerBound;
  first.upperlimit = m_upperBound;
  first.epsilon = m_epsilon;

  std::priority_queue<BooleHelper> myqueue;
  myqueue.emplace(first);

  while (myqueue.size() < m_maximumDivisions)
  {
    const BooleHelper& mostError = myqueue.top();
    if (mostError.error < mostError.epsilon)
      break;

    // std::cout << mostError.error << " " << mostError.epsilon << std::endl;

    // std::cout << "mostError: " << mostError.S[0] << " " << mostError.Sleft[0] << " " << mostError.Sright[0] << " "
    // << mostError.error << std::endl;
    BooleHelper element1, element2;
    splitElement(mostError,element1,element2);
    // std::cout << "element1: " << element1.S[0] << " " << element1.Sleft[0] << " " << element1.Sright[0] << " "  <<
    // element1.error << " "  << element1.epsilon << " " << myqueue.size() << std::endl;
    // std::cout << "element2: " << element2.S[0] << " " << element2.Sleft[0] << " " << element2.Sright[0] << " "  <<
    // element2.error << " " << element1.epsilon << std::endl;

    myqueue.pop();
    myqueue.emplace(std::move(element1));
    myqueue.emplace(std::move(element2));
  }

  return sumPieces(myqueue);
}

AdaptiveBoole::AdaptiveBoole() : m_p{new BooleImpl{}} {}

AdaptiveBoole::AdaptiveBoole(const AdaptiveBoole &other) : m_p(other.m_p->clone()) {}

AdaptiveBoole &AdaptiveBoole::operator=(const AdaptiveBoole &other)
{
  m_p = move(other.m_p->clone());
  return *this;
}

AdaptiveBoole::AdaptiveBoole(AdaptiveBoole &&other)
{
  m_p = move(other.m_p);
  other.m_p = NULL;
}

AdaptiveBoole &AdaptiveBoole::operator=(AdaptiveBoole &&other)
{
  if (m_p != other.m_p)
  {
    m_p = move(other.m_p);
    other.m_p = NULL;
  }
  return *this;
}

void
AdaptiveBoole::setFunction(const std::function<std::vector<double>(std::deque<double> &evaluationPoints)> &integrand)
{
  m_p->m_integrand = integrand;
}

void AdaptiveBoole::setInterval(const std::vector<double> &lowerBounds, const std::vector<double> &upperBounds)
{
  assert(lowerBounds.size() == upperBounds.size());
  m_p->m_lowerBound = lowerBounds.back();
  m_p->m_upperBound = upperBounds.back();
  if (lowerBounds.size() > 1)
  {
    m_p->m_lowerBoundsInnerDimensions = lowerBounds;
    m_p->m_lowerBoundsInnerDimensions.pop_back();

    m_p->m_upperBoundsInnerDimensions = upperBounds;
    m_p->m_upperBoundsInnerDimensions.pop_back();
  }
}

void AdaptiveBoole::setPrecision(double epsilon) { m_p->m_epsilon = epsilon; }

void AdaptiveBoole::setMaximumDivisions(int maximumDivisions) { m_p->m_maximumDivisions = maximumDivisions; }
std::vector<double> AdaptiveBoole::integrate() { return m_p->integrate(); }

void AdaptiveBoole::setAdditionalEvaluationPoints(const std::deque<double> &evaluationPoints)
{
  m_p->m_evaluationPointsOuterDimensions = evaluationPoints;
}

AdaptiveBoole::~AdaptiveBoole(){};
<commit_msg>change division into multiplication where possible<commit_after>#include "AdaptiveBoole.h"
#include <queue>
#include <algorithm>
#include <limits>

struct BooleHelper
{
  BooleHelper() : error(0.0){};
  std::vector<double> fa, fb, fc, fd, fe, ff, fg, fh, fi;
  std::vector<double> S, Sleft, Sright;
  double lowerlimit;
  double upperlimit;
  double epsilon;
  double error;
  bool operator<(const BooleHelper &rhs) const { return this->error / this->epsilon < rhs.error / rhs.epsilon; }
};

class AdaptiveBoole::BooleImpl
{
public:
  BooleImpl() : m_epsilon(1.0e-5), m_maximumDivisions(10){};
  std::vector<double> sumPieces(std::priority_queue<BooleHelper> &pieces);
  void createElement(const BooleHelper &mostError,BooleHelper &element, bool first);
  void splitElement(const BooleHelper &mostError,BooleHelper &element1,BooleHelper &element2);
  std::vector<double> integrate();
  std::function<std::vector<double>(std::deque<double> &evaluationPoints)> m_integrand;
  double m_lowerBound, m_upperBound, m_epsilon;
  std::vector<double> m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions;
  std::deque<double> m_evaluationPointsOuterDimensions;
  int m_maximumDivisions;
  std::unique_ptr<BooleImpl> clone();
};

std::unique_ptr<AdaptiveBoole::BooleImpl> AdaptiveBoole::BooleImpl::clone()
{
  return std::unique_ptr<AdaptiveBoole::BooleImpl>(new BooleImpl(*this));
}

void AdaptiveBoole::BooleImpl::createElement(const BooleHelper &mostError, BooleHelper& element, bool first)
{
  double a, b;
  if (first)
  {
    a = mostError.lowerlimit;
    b = 0.5*(mostError.lowerlimit + mostError.upperlimit);
  }
  else
  {
    a = 0.5*(mostError.lowerlimit + mostError.upperlimit);
    b = mostError.upperlimit;
  }
  double tmp = 0.125*(b - a);
  double c = a + tmp;
  double e = a + 3.0 * tmp;
  double g = a + 5.0 * tmp;
  double i = a + 7.0 * tmp;
  if (first)
  {
    element.fa = std::move(mostError.fa);
    element.fb = mostError.ff;
    element.fd = std::move(mostError.fc);
    element.ff = std::move(mostError.fd);
    element.fh = std::move(mostError.fe);
    element.S = std::move(mostError.Sleft);
  }
  else
  {
    element.fa = std::move(mostError.ff);
    element.fb = std::move(mostError.fb);
    element.fd = std::move(mostError.fg);
    element.ff = std::move(mostError.fh);
    element.fh = std::move(mostError.fi);
    element.S = std::move(mostError.Sright);
  }

  if (m_lowerBoundsInnerDimensions.size() > 0)
  {
    AdaptiveBoole test;
    test.setFunction(m_integrand);
    test.setInterval(m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions);
    test.setMaximumDivisions(m_maximumDivisions);
    test.setPrecision(m_epsilon);
    m_evaluationPointsOuterDimensions[0] = c;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fc = test.integrate();
    m_evaluationPointsOuterDimensions[0] = e;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fe = test.integrate();
    m_evaluationPointsOuterDimensions[0] = g;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fg = test.integrate();
    m_evaluationPointsOuterDimensions[0] = i;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    element.fi = test.integrate();
  }
  else
  {
    m_evaluationPointsOuterDimensions[0] = c;
    element.fc = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = e;
    element.fe = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = g;
    element.fg = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = i;
    element.fi = m_integrand(m_evaluationPointsOuterDimensions);
  }

  std::size_t size = element.fa.size();

  double prefactor = (b - a) / 180.0;
  element.Sleft.reserve(size);
  element.Sright.reserve(size);
  for (std::size_t i = 0; i < size; i++)
  {
    element.Sleft.emplace_back(prefactor * (7.0 * element.fa[i] + 32.0 * element.fc[i] + 12.0 * element.fd[i] +
                                            32.0 * element.fe[i] + 7.0 * element.ff[i]));
    element.Sright.emplace_back(prefactor * (7.0 * element.ff[i] + 32.0 * element.fg[i] + 12.0 * element.fh[i] +
                                             32.0 * element.fi[i] + 7.0 * element.fb[i]));
    element.error = std::max(element.error, std::abs(63.0 * (element.Sleft[i] + element.Sright[i] - element.S[i])));
  }
  element.lowerlimit = a;
  element.upperlimit = b;
  element.epsilon = std::max(mostError.epsilon * M_SQRT1_2, std::numeric_limits<double>::epsilon());
  // std::cout << element.epsilon << " " << element.error << std::endl;
}

void AdaptiveBoole::BooleImpl::splitElement(const BooleHelper &mostError, BooleHelper &element1, BooleHelper &element2)
{
  this->createElement(mostError, element1,true);
  this->createElement(mostError, element2,false);
  //return std::make_pair(element1, element2);
}

std::vector<double> AdaptiveBoole::BooleImpl::sumPieces(std::priority_queue<BooleHelper> &pieces)
{
  std::size_t size = pieces.top().Sleft.size();
  std::vector<double> sum(size);
  while (pieces.size() > 0)
  {
    const BooleHelper& element = pieces.top();
    for (std::size_t i = 0; i < size; i++)
    {
      double S2 = element.Sleft[i] + element.Sright[i];
      sum[i] += S2 + (S2 - element.S[i]) / 63.0;
    }
    pieces.pop();
  }
  return sum;
}

std::vector<double> AdaptiveBoole::BooleImpl::integrate()
{
  BooleHelper first;

  double a = m_lowerBound;
  double b = m_upperBound;

  double tmp = 0.125*(b - a);
  double c = a + tmp;
  double d = a + 2.0 * tmp;
  double e = a + 3.0 * tmp;
  double f = a + 4.0 * tmp;
  double g = a + 5.0 * tmp;
  double h = a + 6.0 * tmp;
  double i = a + 7.0 * tmp;
  //std::cout << b << " " << a + 8.0 * tmp << std::endl;

  if (m_lowerBoundsInnerDimensions.size() > 0)
  {
    AdaptiveBoole test;
    test.setFunction(m_integrand);
    test.setInterval(m_lowerBoundsInnerDimensions, m_upperBoundsInnerDimensions);
    test.setMaximumDivisions(m_maximumDivisions);
    test.setPrecision(m_epsilon);
    m_evaluationPointsOuterDimensions.push_front(a);
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fa = test.integrate();
    m_evaluationPointsOuterDimensions[0] = b;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fb = test.integrate();
    m_evaluationPointsOuterDimensions[0] = c;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fc = test.integrate();
    m_evaluationPointsOuterDimensions[0] = d;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fd = test.integrate();
    m_evaluationPointsOuterDimensions[0] = e;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fe = test.integrate();
    m_evaluationPointsOuterDimensions[0] = f;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.ff = test.integrate();
    m_evaluationPointsOuterDimensions[0] = g;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fg = test.integrate();
    m_evaluationPointsOuterDimensions[0] = h;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fh = test.integrate();
    m_evaluationPointsOuterDimensions[0] = i;
    test.setAdditionalEvaluationPoints(m_evaluationPointsOuterDimensions);
    first.fi = test.integrate();
  }
  else
  {
    m_evaluationPointsOuterDimensions.push_front(a);
    first.fa = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = b;
    first.fb = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = c;
    first.fc = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = d;
    first.fd = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = e;
    first.fe = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = f;
    first.ff = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = g;
    first.fg = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = h;
    first.fh = m_integrand(m_evaluationPointsOuterDimensions);
    m_evaluationPointsOuterDimensions[0] = i;
    first.fi = m_integrand(m_evaluationPointsOuterDimensions);
  }

  std::size_t size = first.fa.size();

  double prefactor = (b - a) / 180.0;
  first.S.reserve(size);
  first.Sleft.reserve(size);
  first.Sright.reserve(size);
  for (std::size_t i = 0; i < size; i++)
  {
    first.S.emplace_back(2.0 * prefactor * (7.0 * first.fa[i] + 32.0 * first.fd[i] + 12.0 * first.ff[i] +
                                            32.0 * first.fh[i] + 7.0 * first.fb[i]));
    first.Sleft.emplace_back(prefactor * (7.0 * first.fa[i] + 32.0 * first.fc[i] + 12.0 * first.fd[i] +
                                          32.0 * first.fe[i] + 7.0 * first.ff[i]));
    first.Sright.emplace_back(prefactor * (7.0 * first.ff[i] + 32.0 * first.fg[i] + 12.0 * first.fh[i] +
                                           32.0 * first.fi[i] + 7.0 * first.fb[i]));
    first.error = std::max(first.error, std::abs(63.0 * (first.Sleft[i] + first.Sright[i] - first.S[i])));
  }

  first.lowerlimit = m_lowerBound;
  first.upperlimit = m_upperBound;
  first.epsilon = m_epsilon;

  std::priority_queue<BooleHelper> myqueue;
  myqueue.emplace(first);

  while (myqueue.size() < m_maximumDivisions)
  {
    const BooleHelper& mostError = myqueue.top();
    if (mostError.error < mostError.epsilon)
      break;

    // std::cout << mostError.error << " " << mostError.epsilon << std::endl;

    // std::cout << "mostError: " << mostError.S[0] << " " << mostError.Sleft[0] << " " << mostError.Sright[0] << " "
    // << mostError.error << std::endl;
    BooleHelper element1, element2;
    splitElement(mostError,element1,element2);
    // std::cout << "element1: " << element1.S[0] << " " << element1.Sleft[0] << " " << element1.Sright[0] << " "  <<
    // element1.error << " "  << element1.epsilon << " " << myqueue.size() << std::endl;
    // std::cout << "element2: " << element2.S[0] << " " << element2.Sleft[0] << " " << element2.Sright[0] << " "  <<
    // element2.error << " " << element1.epsilon << std::endl;

    myqueue.pop();
    myqueue.emplace(std::move(element1));
    myqueue.emplace(std::move(element2));
  }

  return sumPieces(myqueue);
}

AdaptiveBoole::AdaptiveBoole() : m_p{new BooleImpl{}} {}

AdaptiveBoole::AdaptiveBoole(const AdaptiveBoole &other) : m_p(other.m_p->clone()) {}

AdaptiveBoole &AdaptiveBoole::operator=(const AdaptiveBoole &other)
{
  m_p = move(other.m_p->clone());
  return *this;
}

AdaptiveBoole::AdaptiveBoole(AdaptiveBoole &&other)
{
  m_p = move(other.m_p);
  other.m_p = NULL;
}

AdaptiveBoole &AdaptiveBoole::operator=(AdaptiveBoole &&other)
{
  if (m_p != other.m_p)
  {
    m_p = move(other.m_p);
    other.m_p = NULL;
  }
  return *this;
}

void
AdaptiveBoole::setFunction(const std::function<std::vector<double>(std::deque<double> &evaluationPoints)> &integrand)
{
  m_p->m_integrand = integrand;
}

void AdaptiveBoole::setInterval(const std::vector<double> &lowerBounds, const std::vector<double> &upperBounds)
{
  assert(lowerBounds.size() == upperBounds.size());
  m_p->m_lowerBound = lowerBounds.back();
  m_p->m_upperBound = upperBounds.back();
  if (lowerBounds.size() > 1)
  {
    m_p->m_lowerBoundsInnerDimensions = lowerBounds;
    m_p->m_lowerBoundsInnerDimensions.pop_back();

    m_p->m_upperBoundsInnerDimensions = upperBounds;
    m_p->m_upperBoundsInnerDimensions.pop_back();
  }
}

void AdaptiveBoole::setPrecision(double epsilon) { m_p->m_epsilon = epsilon; }

void AdaptiveBoole::setMaximumDivisions(int maximumDivisions) { m_p->m_maximumDivisions = maximumDivisions; }
std::vector<double> AdaptiveBoole::integrate() { return m_p->integrate(); }

void AdaptiveBoole::setAdditionalEvaluationPoints(const std::deque<double> &evaluationPoints)
{
  m_p->m_evaluationPointsOuterDimensions = evaluationPoints;
}

AdaptiveBoole::~AdaptiveBoole(){};
<|endoftext|>


<commit_before>#include "less/lessstylesheet/LessRuleset.h"

LessSelector::LessSelector(const Selector &original) {
  list<Selector> parts;
  list<Selector>::iterator it;
  Selector *old_selector;
  Selector new_selector;

  original.split(parts);

  _needsArguments = false;
  _unlimitedArguments = false;

  for (it = parts.begin(); it != parts.end(); it++) {
    old_selector = &(*it);

    while (!old_selector->empty()) {
      if (parseExtension(*old_selector, new_selector)) {
      } else if (parts.size() == 1 && !new_selector.empty()) {
        if ((new_selector.front().type == Token::HASH ||
             new_selector.front() == ".") &&
            parseArguments(*old_selector)) {
          _needsArguments = true;
          old_selector->ltrim();

        } else if (!parseConditions(*old_selector)) {
          new_selector.push_back(old_selector->front());
          old_selector->pop_front();
        }
      } else {
        new_selector.push_back(old_selector->front());
        old_selector->pop_front();
      }
    }
    if (!empty())
      push_back(Token::BUILTIN_COMMA);

    new_selector.trim();
    insert(end(), new_selector.begin(), new_selector.end());
    new_selector.clear();
  }

}

LessSelector::~LessSelector() {
}

bool LessSelector::parseExtension(Selector &selector, Selector &extension) {
  int parentheses = 1;
  TokenList::iterator i;
  Extension e;

  i = selector.begin();

  // ":", "extend", "("
  if (selector.size() < 3 || (*i).type != Token::COLON ||
      (*++i).type != Token::IDENTIFIER || (*i) != "extend" ||
      (*++i).type != Token::PAREN_OPEN)
    return false;

  i++;

  for (; i != selector.end() && parentheses > 0; i++) {
    if ((*i).type == Token::PAREN_OPEN)
      parentheses++;
    else if ((*i).type == Token::PAREN_CLOSED)
      parentheses--;

    if (parentheses > 0) {
      e.getTarget().push_back(*i);
    }
  }

  e.setExtension(extension);
  extensions.push_back(e);

  selector.erase(selector.begin(), i);

  return true;
}

bool LessSelector::parseArguments(TokenList &selector) {
  string delimiter;
  TokenList::iterator i;

  if (selector.front().type != Token::PAREN_OPEN)
    return false;

  if (selector.contains(Token::DELIMITER, ";"))
    delimiter = ";";
  else
    delimiter = ",";

  if (!validateArguments(selector, delimiter))
    return false;

  selector.pop_front();

  selector.ltrim();

  while (parseParameter(selector, delimiter)) {
    selector.ltrim();
  }

  i = selector.begin();

  if (selector.size() > 3 && (*i) == "." && (*++i) == "." && (*++i) == ".") {
    _unlimitedArguments = true;
    i++;
    selector.erase(selector.begin(), i);
  }

  selector.ltrim();

  if (selector.front().type != Token::PAREN_CLOSED) {
    throw new ParseException(
        selector.toString(), "matching parentheses.", 0, 0, "");
  }
  selector.pop_front();

  return true;
}

bool LessSelector::validateArguments(const TokenList &arguments,
                                     const std::string &delimiter) {
  TokenList::const_iterator i = arguments.begin();

  if ((*i).type != Token::PAREN_OPEN)
    return false;

  i++;

  while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
    i++;
  }

  while (i != arguments.end()) {
    if ((*i).type == Token::IDENTIFIER) {
      // switch
      i++;

    } else if ((*i).type == Token::ATKEYWORD) {
      // variable
      i++;

      if ((*i).type == Token::COLON) {
        // default value
        i++;
        while (i != arguments.end() && (*i).type != Token::PAREN_CLOSED &&
               *i != delimiter) {
          i++;
        }

      } else if (*i == ".") {
        i++;
        // rest
        if (i == arguments.end() || *i != "." || ++i == arguments.end() ||
            *i != ".") {
          return false;
        }
        i++;
        break;
      }
    } else
      break;

    if (*i != delimiter)
      break;
    i++;

    while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
      i++;
    }
  }

  while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
    i++;
  }

  // rest
  if (*i == ".") {
    i++;
    if (i == arguments.end() || *i != "." || ++i == arguments.end() ||
        *i != ".") {
      return false;
    }
    i++;
  }
  if ((*i).type != Token::PAREN_CLOSED)
    return false;

  return true;
}

bool LessSelector::parseParameter(TokenList &selector,
                                  const std::string &delimiter) {
  string keyword;
  TokenList value;
  TokenList::iterator i;

  if (selector.empty())
    return false;

  if (selector.front().type == Token::IDENTIFIER) {
    keyword = selector.front();
    selector.pop_front();

  } else if (selector.front().type == Token::ATKEYWORD) {
    keyword = selector.front();
    selector.pop_front();

    i = selector.begin();

    if (parseDefaultValue(selector, delimiter, value)) {
      // default value

    } else if (selector.size() > 3 && (*i) == "." && (*++i) == "." &&
               (*++i) == ".") {
      i++;
      // rest argument
      selector.erase(selector.begin(), i);

      restIdentifier = keyword;
      _unlimitedArguments = true;
      return true;
    }
  } else
    return false;

  selector.ltrim();

  if (!selector.empty() && selector.front() == delimiter)
    selector.pop_front();

  parameters.push_back(keyword);
  defaults.push_back(value);
  return true;
}

bool LessSelector::parseDefaultValue(TokenList &arguments,
                                     const std::string &delimiter,
                                     TokenList &value) {
  unsigned int parentheses = 0;

  if (arguments.front().type != Token::COLON)
    return false;

  arguments.pop_front();

  while (!arguments.empty() &&
         (parentheses > 0 || (arguments.front().type != Token::PAREN_CLOSED &&
                              arguments.front() != delimiter))) {
    if (arguments.front().type == Token::PAREN_OPEN)
      parentheses++;
    if (arguments.front().type == Token::PAREN_CLOSED)
      parentheses--;

    value.push_back(arguments.front());
    arguments.pop_front();
  }

  value.trim();

  if (value.empty()) {
    throw new ParseException("", "default value following ':'", 0, 0, "");
  }
  return true;
}

bool LessSelector::parseConditions(TokenList &selector) {
  TokenList condition;

  if (selector.empty() || selector.front() != "when")
    return false;

  selector.pop_front();
  selector.ltrim();

  while (!selector.empty()) {
    while (!selector.empty() && selector.front() != ",") {
      condition.push_back(selector.front());
      selector.pop_front();
    }
    if (!selector.empty() && selector.front() == ",")
      selector.pop_front();

    conditions.push_back(condition);
  }
  return true;
}

TokenList *LessSelector::getDefault(const std::string &keyword) {
  std::list<std::string>::iterator pit = parameters.begin();
  std::list<TokenList>::iterator dit = defaults.begin();

  for (; pit != parameters.end(); pit++, dit++) {
    if ((*pit) == keyword)
      return &(*dit);
  }
  return NULL;
}

std::list<std::string> &LessSelector::getParameters() {
  return parameters;
}

std::list<TokenList> &LessSelector::getConditions() {
  return conditions;
}

std::list<Extension> &LessSelector::getExtensions() {
  return extensions;
}

bool LessSelector::matchArguments(const MixinArguments &args) {
  std::list<std::string>::iterator p_it = parameters.begin();
  std::list<TokenList>::iterator d_it = defaults.begin();
  size_t pos = 0;

  for (; p_it != parameters.end(); p_it++, d_it++) {
    if (args.get(*p_it) == NULL && args.get(pos++) == NULL &&
        (*d_it).empty()) {
      return false;
    }
  }
  return (pos >= args.count() || unlimitedArguments());
}

bool LessSelector::needsArguments() {
  return _needsArguments;
}
bool LessSelector::unlimitedArguments() {
  return _unlimitedArguments;
}
string LessSelector::getRestIdentifier() {
  return restIdentifier;
}
<commit_msg>Update matchArguments() to check switch arguments.<commit_after>#include "less/lessstylesheet/LessRuleset.h"

LessSelector::LessSelector(const Selector &original) {
  list<Selector> parts;
  list<Selector>::iterator it;
  Selector *old_selector;
  Selector new_selector;

  original.split(parts);

  _needsArguments = false;
  _unlimitedArguments = false;

  for (it = parts.begin(); it != parts.end(); it++) {
    old_selector = &(*it);

    while (!old_selector->empty()) {
      if (parseExtension(*old_selector, new_selector)) {
      } else if (parts.size() == 1 && !new_selector.empty()) {
        if ((new_selector.front().type == Token::HASH ||
             new_selector.front() == ".") &&
            parseArguments(*old_selector)) {
          _needsArguments = true;
          old_selector->ltrim();

        } else if (!parseConditions(*old_selector)) {
          new_selector.push_back(old_selector->front());
          old_selector->pop_front();
        }
      } else {
        new_selector.push_back(old_selector->front());
        old_selector->pop_front();
      }
    }
    if (!empty())
      push_back(Token::BUILTIN_COMMA);

    new_selector.trim();
    insert(end(), new_selector.begin(), new_selector.end());
    new_selector.clear();
  }

}

LessSelector::~LessSelector() {
}

bool LessSelector::parseExtension(Selector &selector, Selector &extension) {
  int parentheses = 1;
  TokenList::iterator i;
  Extension e;

  i = selector.begin();

  // ":", "extend", "("
  if (selector.size() < 3 || (*i).type != Token::COLON ||
      (*++i).type != Token::IDENTIFIER || (*i) != "extend" ||
      (*++i).type != Token::PAREN_OPEN)
    return false;

  i++;

  for (; i != selector.end() && parentheses > 0; i++) {
    if ((*i).type == Token::PAREN_OPEN)
      parentheses++;
    else if ((*i).type == Token::PAREN_CLOSED)
      parentheses--;

    if (parentheses > 0) {
      e.getTarget().push_back(*i);
    }
  }

  e.setExtension(extension);
  extensions.push_back(e);

  selector.erase(selector.begin(), i);

  return true;
}

bool LessSelector::parseArguments(TokenList &selector) {
  string delimiter;
  TokenList::iterator i;

  if (selector.front().type != Token::PAREN_OPEN)
    return false;

  if (selector.contains(Token::DELIMITER, ";"))
    delimiter = ";";
  else
    delimiter = ",";

  if (!validateArguments(selector, delimiter))
    return false;

  selector.pop_front();

  selector.ltrim();

  while (parseParameter(selector, delimiter)) {
    selector.ltrim();
  }

  i = selector.begin();

  if (selector.size() > 3 && (*i) == "." && (*++i) == "." && (*++i) == ".") {
    _unlimitedArguments = true;
    i++;
    selector.erase(selector.begin(), i);
  }

  selector.ltrim();

  if (selector.front().type != Token::PAREN_CLOSED) {
    throw new ParseException(
        selector.toString(), "matching parentheses.", 0, 0, "");
  }
  selector.pop_front();

  return true;
}

bool LessSelector::validateArguments(const TokenList &arguments,
                                     const std::string &delimiter) {
  TokenList::const_iterator i = arguments.begin();

  if ((*i).type != Token::PAREN_OPEN)
    return false;

  i++;

  while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
    i++;
  }

  while (i != arguments.end()) {
    if ((*i).type == Token::IDENTIFIER) {
      // switch
      i++;

    } else if ((*i).type == Token::ATKEYWORD) {
      // variable
      i++;

      if ((*i).type == Token::COLON) {
        // default value
        i++;
        while (i != arguments.end() && (*i).type != Token::PAREN_CLOSED &&
               *i != delimiter) {
          i++;
        }

      } else if (*i == ".") {
        i++;
        // rest
        if (i == arguments.end() || *i != "." || ++i == arguments.end() ||
            *i != ".") {
          return false;
        }
        i++;
        break;
      }
    } else
      break;

    if (*i != delimiter)
      break;
    i++;

    while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
      i++;
    }
  }

  while (i != arguments.end() && (*i).type == Token::WHITESPACE) {
    i++;
  }

  // rest
  if (*i == ".") {
    i++;
    if (i == arguments.end() || *i != "." || ++i == arguments.end() ||
        *i != ".") {
      return false;
    }
    i++;
  }
  if ((*i).type != Token::PAREN_CLOSED)
    return false;

  return true;
}

bool LessSelector::parseParameter(TokenList &selector,
                                  const std::string &delimiter) {
  string keyword;
  TokenList value;
  TokenList::iterator i;

  if (selector.empty())
    return false;

  if (selector.front().type == Token::IDENTIFIER) {
    keyword = selector.front();
    selector.pop_front();

  } else if (selector.front().type == Token::ATKEYWORD) {
    keyword = selector.front();
    selector.pop_front();

    i = selector.begin();

    if (parseDefaultValue(selector, delimiter, value)) {
      // default value

    } else if (selector.size() > 3 && (*i) == "." && (*++i) == "." &&
               (*++i) == ".") {
      i++;
      // rest argument
      selector.erase(selector.begin(), i);

      restIdentifier = keyword;
      _unlimitedArguments = true;
      return true;
    }
  } else
    return false;

  selector.ltrim();

  if (!selector.empty() && selector.front() == delimiter)
    selector.pop_front();

  parameters.push_back(keyword);
  defaults.push_back(value);
  return true;
}

bool LessSelector::parseDefaultValue(TokenList &arguments,
                                     const std::string &delimiter,
                                     TokenList &value) {
  unsigned int parentheses = 0;

  if (arguments.front().type != Token::COLON)
    return false;

  arguments.pop_front();

  while (!arguments.empty() &&
         (parentheses > 0 || (arguments.front().type != Token::PAREN_CLOSED &&
                              arguments.front() != delimiter))) {
    if (arguments.front().type == Token::PAREN_OPEN)
      parentheses++;
    if (arguments.front().type == Token::PAREN_CLOSED)
      parentheses--;

    value.push_back(arguments.front());
    arguments.pop_front();
  }

  value.trim();

  if (value.empty()) {
    throw new ParseException("", "default value following ':'", 0, 0, "");
  }
  return true;
}

bool LessSelector::parseConditions(TokenList &selector) {
  TokenList condition;

  if (selector.empty() || selector.front() != "when")
    return false;

  selector.pop_front();
  selector.ltrim();

  while (!selector.empty()) {
    while (!selector.empty() && selector.front() != ",") {
      condition.push_back(selector.front());
      selector.pop_front();
    }
    if (!selector.empty() && selector.front() == ",")
      selector.pop_front();

    conditions.push_back(condition);
  }
  return true;
}

TokenList *LessSelector::getDefault(const std::string &keyword) {
  std::list<std::string>::iterator pit = parameters.begin();
  std::list<TokenList>::iterator dit = defaults.begin();

  for (; pit != parameters.end(); pit++, dit++) {
    if ((*pit) == keyword)
      return &(*dit);
  }
  return NULL;
}

std::list<std::string> &LessSelector::getParameters() {
  return parameters;
}

std::list<TokenList> &LessSelector::getConditions() {
  return conditions;
}

std::list<Extension> &LessSelector::getExtensions() {
  return extensions;
}

bool LessSelector::matchArguments(const MixinArguments &args) {
  std::list<std::string>::iterator p_it = parameters.begin();
  std::list<TokenList>::iterator d_it = defaults.begin();
  size_t pos = 0;
  const TokenList *a;

  for (; p_it != parameters.end(); p_it++, d_it++) {
    if ((a = args.get(*p_it)) == NULL &&
        (a = args.get(pos++)) == NULL &&
        (a = &(*d_it))->empty()) {
      return false;
    } else if ((*p_it)[0] != '@' &&
               a->size() == 1 &&
               (*p_it) != a->front()) {
      return false;
    }
  }
  return (pos >= args.count() || unlimitedArguments());
}

bool LessSelector::needsArguments() {
  return _needsArguments;
}
bool LessSelector::unlimitedArguments() {
  return _unlimitedArguments;
}
string LessSelector::getRestIdentifier() {
  return restIdentifier;
}
<|endoftext|>


<commit_before>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#pragma once
#ifndef _QIMESSAGING_SERVICE_INFO_HPP_
#define _QIMESSAGING_SERVICE_INFO_HPP_

#include <qimessaging/api.hpp>
#include <string>
#include <qimessaging/datastream.hpp>

namespace qi
{

  class QIMESSAGING_API ServiceInfo {
  public:
    ServiceInfo();

    inline void setName(const std::string &name)                { _name = name; }
    inline void setServiceId(unsigned int serviceId)            { _serviceId = serviceId; }
    inline void setMachineId(const std::string &machineId)      { _machineId = machineId; }
    inline void setProcessId(unsigned int processId)            { _processId = processId; }
    inline void setEndpoints(const std::vector<std::string> &v) { _endpoints = v; }
    inline void addEndpoint(const std::string &ep)              { _endpoints.push_back(ep); }

    inline const std::string              &name() const        { return _name; }
    inline unsigned int                    serviceId() const   { return _serviceId; }
    inline const std::string              &machineId() const   { return _machineId; }
    inline unsigned int                    processId() const   { return _processId; }
    inline const std::vector<std::string> &endpoints() const   { return _endpoints; }

    friend qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

  protected:
    std::string               _name;
    unsigned int              _serviceId;
    std::string               _machineId;
    unsigned int              _processId;
    std::vector<std::string>  _endpoints;
    void                     *_reserved;
  };

  QIMESSAGING_API qi::DataStream &operator<<(qi::DataStream &stream, const ServiceInfo &sinfo);
  QIMESSAGING_API qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

}; // !qi

#endif  // _QIMESSAGING_SERVICE_INFO_HPP_
<commit_msg>Fix : Windows compilation : 2 operator>> is too much<commit_after>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#pragma once
#ifndef _QIMESSAGING_SERVICE_INFO_HPP_
#define _QIMESSAGING_SERVICE_INFO_HPP_

#include <qimessaging/api.hpp>
#include <string>
#include <qimessaging/datastream.hpp>

namespace qi
{

  class QIMESSAGING_API ServiceInfo {
  public:
    ServiceInfo();

    inline void setName(const std::string &name)                { _name = name; }
    inline void setServiceId(unsigned int serviceId)            { _serviceId = serviceId; }
    inline void setMachineId(const std::string &machineId)      { _machineId = machineId; }
    inline void setProcessId(unsigned int processId)            { _processId = processId; }
    inline void setEndpoints(const std::vector<std::string> &v) { _endpoints = v; }
    inline void addEndpoint(const std::string &ep)              { _endpoints.push_back(ep); }

    inline const std::string              &name() const        { return _name; }
    inline unsigned int                    serviceId() const   { return _serviceId; }
    inline const std::string              &machineId() const   { return _machineId; }
    inline unsigned int                    processId() const   { return _processId; }
    inline const std::vector<std::string> &endpoints() const   { return _endpoints; }

    friend qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

  protected:
    std::string               _name;
    unsigned int              _serviceId;
    std::string               _machineId;
    unsigned int              _processId;
    std::vector<std::string>  _endpoints;
    void                     *_reserved;
  };

  QIMESSAGING_API qi::DataStream &operator<<(qi::DataStream &stream, const ServiceInfo &sinfo);
}; // !qi

#endif  // _QIMESSAGING_SERVICE_INFO_HPP_
<|endoftext|>


<commit_before>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#pragma once
#ifndef _QIMESSAGING_SERVICE_INFO_HPP_
#define _QIMESSAGING_SERVICE_INFO_HPP_

#include <qimessaging/api.hpp>
#include <string>
#include <qimessaging/datastream.hpp>

namespace qi
{

  class QIMESSAGING_API ServiceInfo {
  public:
    ServiceInfo();

    inline void setName(const std::string &name)                { _name = name; }
    inline void setServiceId(unsigned int serviceId)            { _serviceId = serviceId; }
    inline void setMachineId(const std::string &machineId)      { _machineId = machineId; }
    inline void setProcessId(unsigned int processId)            { _processId = processId; }
    inline void setEndpoints(const std::vector<std::string> &v) { _endpoints = v; }
    inline void addEndpoint(const std::string &ep)              { _endpoints.push_back(ep); }

    inline const std::string              &name() const        { return _name; }
    inline unsigned int                    serviceId() const   { return _serviceId; }
    inline const std::string              &machineId() const   { return _machineId; }
    inline unsigned int                    processId() const   { return _processId; }
    inline const std::vector<std::string> &endpoints() const   { return _endpoints; }

    friend qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

  protected:
    std::string               _name;
    unsigned int              _serviceId;
    std::string               _machineId;
    unsigned int              _processId;
    std::vector<std::string>  _endpoints;
    void                     *_reserved;
  };

  QIMESSAGING_API qi::DataStream &operator<<(qi::DataStream &stream, const ServiceInfo &sinfo);
  QIMESSAGING_API qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

}; // !qi

#endif  // _QIMESSAGING_SERVICE_INFO_HPP_
<commit_msg>Fix service_info operator>> linkage (windows)<commit_after>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Aldebaran Robotics
*/

#pragma once
#ifndef _QIMESSAGING_SERVICE_INFO_HPP_
#define _QIMESSAGING_SERVICE_INFO_HPP_

#include <qimessaging/api.hpp>
#include <string>
#include <qimessaging/datastream.hpp>

namespace qi
{

  class QIMESSAGING_API ServiceInfo {
  public:
    ServiceInfo();

    inline void setName(const std::string &name)                { _name = name; }
    inline void setServiceId(unsigned int serviceId)            { _serviceId = serviceId; }
    inline void setMachineId(const std::string &machineId)      { _machineId = machineId; }
    inline void setProcessId(unsigned int processId)            { _processId = processId; }
    inline void setEndpoints(const std::vector<std::string> &v) { _endpoints = v; }
    inline void addEndpoint(const std::string &ep)              { _endpoints.push_back(ep); }

    inline const std::string              &name() const        { return _name; }
    inline unsigned int                    serviceId() const   { return _serviceId; }
    inline const std::string              &machineId() const   { return _machineId; }
    inline unsigned int                    processId() const   { return _processId; }
    inline const std::vector<std::string> &endpoints() const   { return _endpoints; }

    friend QIMESSAGING_API qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

  protected:
    std::string               _name;
    unsigned int              _serviceId;
    std::string               _machineId;
    unsigned int              _processId;
    std::vector<std::string>  _endpoints;
    void                     *_reserved;
  };

  QIMESSAGING_API qi::DataStream &operator<<(qi::DataStream &stream, const ServiceInfo &sinfo);
  QIMESSAGING_API qi::DataStream &operator>>(qi::DataStream &stream, ServiceInfo &sinfo);

}; // !qi

#endif  // _QIMESSAGING_SERVICE_INFO_HPP_
<|endoftext|>


<commit_before>#include <Element.h>
#include <FWPlatform.h>
#include <TextLabel.h>
#include <LinearLayout.h>
#include <Command.h>

using namespace std;

void
Element::~Element() {
  if (isInitialized()) {
    sendCommand(Command(Command::DELETE_ELEMENT, getParentInternalId(), getInternalId()));
  }
}

void
Element::initialize(FWPlatform * _platform) {
  platform = _platform;
  internal_id = platform->getNextInternalId();
}

void
Element::sendCommand(const Command & command){
  platform->sendCommand(command);
}

Element &
Element::addChild(const std::string & text) {
  return addChild(make_shared<TextLabel>(text));
}

LinearLayout &
Element::addHorizontalLayout() {
  auto l = make_shared<LinearLayout>(FW_HORIZONTAL);
  addChild(l);
  return *l;
}

LinearLayout &
Element::addVerticalLayout() {
  auto l = make_shared<LinearLayout>(FW_VERTICAL);
  addChild(l);
  return *l;
}

void
Element::onEvent(EventBase & ev) {
  if (parent && !ev.isHandled()) {
    ev.dispatch(*parent);
  }
}

void
Element::showMessageDialog(const std::string & text) {
  Command c(Command::SHOW_MESSAGE_DIALOG);
  c.setTextValue(text);
  sendCommand(c);
}

std::string
Element::showInputDialog(const std::string & text) {
  Command c(Command::SHOW_INPUT_DIALOG);
  c.setTextValue(text);
  sendCommand(c);
  return platform->getModalResultText();
}
<commit_msg>fix typo, add internal ids to dialog creation commnd<commit_after>#include <Element.h>
#include <FWPlatform.h>
#include <TextLabel.h>
#include <LinearLayout.h>
#include <Command.h>

using namespace std;

Element::~Element() {
  if (isInitialized()) {
    sendCommand(Command(Command::DELETE_ELEMENT, getParentInternalId(), getInternalId()));
  }
}

void
Element::initialize(FWPlatform * _platform) {
  platform = _platform;
  internal_id = platform->getNextInternalId();
}

void
Element::sendCommand(const Command & command){
  platform->sendCommand(command);
}

Element &
Element::addChild(const std::string & text) {
  return addChild(make_shared<TextLabel>(text));
}

LinearLayout &
Element::addHorizontalLayout() {
  auto l = make_shared<LinearLayout>(FW_HORIZONTAL);
  addChild(l);
  return *l;
}

LinearLayout &
Element::addVerticalLayout() {
  auto l = make_shared<LinearLayout>(FW_VERTICAL);
  addChild(l);
  return *l;
}

void
Element::onEvent(EventBase & ev) {
  if (parent && !ev.isHandled()) {
    ev.dispatch(*parent);
  }
}

void
Element::showMessageDialog(const std::string & text) {
  Command c(Command::SHOW_MESSAGE_DIALOG, getParentInternalId(), getInternalId());
  c.setTextValue(text);
  sendCommand(c);
}

std::string
Element::showInputDialog(const std::string & text) {
  Command c(Command::SHOW_INPUT_DIALOG, getParentInternalId(), getInternalId());
  c.setTextValue(text);
  sendCommand(c);
  return platform->getModalResultText();
}
<|endoftext|>


<commit_before>#include "main.h"
#include "File.h"
#include "header.h"

#ifdef NO_PNG
namespace FilePNG {
#include "FileNotImplemented.h"
}
#else

namespace FilePNG {

#define PNG_DEBUG 3
#include <png.h>

void help() {
    printf(".png files. These have a bit depth of 8, and may have 1-4 channels. They may\n"
           "only have 1 frame.\n");
}

Image load(string filename) {
    png_byte header[8];        // 8 is the maximum size that can be checked
    png_structp png_ptr;
    png_infop info_ptr;
    int number_of_passes;
    png_bytep *row_pointers;

    /* open file and test for it being a png */
    FILE *f = fopen(filename.c_str(), "rb");
    assert(f, "File %s could not be opened for reading\n", filename.c_str());
    assert(fread(header, 1, 8, f) == 8, "File ended before end of header\n");
    assert(!png_sig_cmp(header, 0, 8), "File %s is not recognized as a PNG file\n", filename.c_str());

    /* initialize stuff */
    png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);

    assert(png_ptr, "[read_png_file] png_create_read_struct failed\n");

    info_ptr = png_create_info_struct(png_ptr);
    assert(info_ptr, "[read_png_file] png_create_info_struct failed\n");

    assert(!setjmp(png_jmpbuf(png_ptr)), "[read_png_file] Error during init_io\n");

    png_init_io(png_ptr, f);
    png_set_sig_bytes(png_ptr, 8);

    png_read_info(png_ptr, info_ptr);

    int width = png_get_image_width(png_ptr, info_ptr);
    int height = png_get_image_height(png_ptr, info_ptr);
    int channels = png_get_channels(png_ptr, info_ptr);
    int bit_depth = png_get_bit_depth(png_ptr, info_ptr);

    // Expand low-bpp images to have only 1 pixel per byte (As opposed to tight packing)
    if (bit_depth < 8) {
        png_set_packing(png_ptr);
    }

    Image im(width, height, 1, channels);

    number_of_passes = png_set_interlace_handling(png_ptr);
    png_read_update_info(png_ptr, info_ptr);

    // read the file
    assert(!setjmp(png_jmpbuf(png_ptr)), "[read_png_file] Error during read_image\n");

    row_pointers = new png_bytep[im.height];
    for (int y = 0; y < im.height; y++) {
        row_pointers[y] = new png_byte[png_get_rowbytes(png_ptr, info_ptr)];
    }

    png_read_image(png_ptr, row_pointers);

    fclose(f);

    // convert the data to floats
    if (bit_depth <= 8) {
        int bit_scale = 8/bit_depth;
        for (int y = 0; y < im.height; y++) {
            png_bytep srcPtr = row_pointers[y];
            for (int x = 0; x < im.width; x++) {
                for (int c = 0; c < im.channels; c++) {
                    im(x, y)[c] = LDRtoHDR(bit_scale* (*srcPtr++));
                }
            }
        }
    } else if (bit_depth == 16) {
        printf("Reading a 16-bit PNG image (Image may be darker than expected!)\n");
        for (int y = 0; y < im.height; y++) {
            png_bytep srcPtr = row_pointers[y];
            for (int x = 0; x < im.width; x++) {
                for (int c = 0; c < im.channels; c++) {
                    im(x, y)[c] = LDR16toHDR(*srcPtr);  // Note: Endian issues may be possible here, seems to work in WinXP
                    srcPtr+=2;
                }
            }
        }
    }

    // clean up
    for (int y = 0; y < im.height; y++) {
        delete[] row_pointers[y];
    }
    delete[] row_pointers;

    png_destroy_read_struct(&png_ptr, &info_ptr, NULL);

    return im;
}

void save(Window im, string filename) {
    png_structp png_ptr;
    png_infop info_ptr;
    png_bytep *row_pointers;
    png_byte color_type;

    assert(im.frames == 1, "Can't save a multi-frame PNG image\n");
    assert(im.channels > 0 && im.channels < 5,
           "Imagestack can't write PNG files that have other than 1, 2, 3, or 4 channels\n");

    png_byte color_types[4] = {PNG_COLOR_TYPE_GRAY, PNG_COLOR_TYPE_GRAY_ALPHA,
                               PNG_COLOR_TYPE_RGB,  PNG_COLOR_TYPE_RGB_ALPHA
                              };
    color_type = color_types[im.channels - 1];

    // open file
    FILE *f = fopen(filename.c_str(), "wb");
    assert(f, "[write_png_file] File %s could not be opened for writing\n", filename.c_str());

    // initialize stuff
    png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
    assert(png_ptr, "[write_png_file] png_create_write_struct failed\n");

    info_ptr = png_create_info_struct(png_ptr);
    assert(info_ptr, "[write_png_file] png_create_info_struct failed\n");

    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during init_io\n");

    png_init_io(png_ptr, f);

    // write header
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during writing header\n");

    png_set_IHDR(png_ptr, info_ptr, im.width, im.height,
                 8, color_type, PNG_INTERLACE_NONE,
                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);

    png_write_info(png_ptr, info_ptr);

    // convert the floats to bytes
    row_pointers = new png_bytep[im.height];
    for (int y = 0; y < im.height; y++) {
	row_pointers[y] = new png_byte[png_get_rowbytes(png_ptr, info_ptr)];
        png_bytep dstPtr = row_pointers[y];
        for (int x = 0; x < im.width; x++) {
            for (int c = 0; c < im.channels; c++) {
                *dstPtr++ = (png_byte)(HDRtoLDR(im(x, y)[c]));
            }
        }
    }

    // write data
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during writing bytes");

    png_write_image(png_ptr, row_pointers);

    // finish write
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during end of write");

    png_write_end(png_ptr, NULL);

    // clean up
    for (int y = 0; y < im.height; y++) {
        delete[] row_pointers[y];
    }
    delete[] row_pointers;

    fclose(f);

    png_destroy_write_struct(&png_ptr, &info_ptr);
}

}

#endif
#include "footer.h"
<commit_msg>Fixed 16-bit png handling<commit_after>#include "main.h"
#include "File.h"
#include "header.h"

#ifdef NO_PNG
namespace FilePNG {
#include "FileNotImplemented.h"
}
#else

namespace FilePNG {

#define PNG_DEBUG 3
#include <png.h>

void help() {
    printf(".png files. These have a bit depth of 8, and may have 1-4 channels. They may\n"
           "only have 1 frame.\n");
}

Image load(string filename) {
    png_byte header[8];        // 8 is the maximum size that can be checked
    png_structp png_ptr;
    png_infop info_ptr;
    int number_of_passes;
    png_bytep *row_pointers;

    /* open file and test for it being a png */
    FILE *f = fopen(filename.c_str(), "rb");
    assert(f, "File %s could not be opened for reading\n", filename.c_str());
    assert(fread(header, 1, 8, f) == 8, "File ended before end of header\n");
    assert(!png_sig_cmp(header, 0, 8), "File %s is not recognized as a PNG file\n", filename.c_str());

    /* initialize stuff */
    png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);

    assert(png_ptr, "[read_png_file] png_create_read_struct failed\n");

    info_ptr = png_create_info_struct(png_ptr);
    assert(info_ptr, "[read_png_file] png_create_info_struct failed\n");

    assert(!setjmp(png_jmpbuf(png_ptr)), "[read_png_file] Error during init_io\n");

    png_init_io(png_ptr, f);
    png_set_sig_bytes(png_ptr, 8);

    png_read_info(png_ptr, info_ptr);

    int width = png_get_image_width(png_ptr, info_ptr);
    int height = png_get_image_height(png_ptr, info_ptr);
    int channels = png_get_channels(png_ptr, info_ptr);
    int bit_depth = png_get_bit_depth(png_ptr, info_ptr);

    // Expand low-bpp images to have only 1 pixel per byte (As opposed to tight packing)
    if (bit_depth < 8) {
        png_set_packing(png_ptr);
    }

    Image im(width, height, 1, channels);

    number_of_passes = png_set_interlace_handling(png_ptr);
    png_read_update_info(png_ptr, info_ptr);

    // read the file
    assert(!setjmp(png_jmpbuf(png_ptr)), "[read_png_file] Error during read_image\n");

    row_pointers = new png_bytep[im.height];
    for (int y = 0; y < im.height; y++) {
        row_pointers[y] = new png_byte[png_get_rowbytes(png_ptr, info_ptr)];
    }

    png_read_image(png_ptr, row_pointers);

    fclose(f);

    // convert the data to floats
    if (bit_depth <= 8) {
        int bit_scale = 8/bit_depth;
        for (int y = 0; y < im.height; y++) {
            png_bytep srcPtr = row_pointers[y];
            for (int x = 0; x < im.width; x++) {
                for (int c = 0; c < im.channels; c++) {
                    im(x, y)[c] = LDRtoHDR(bit_scale* (*srcPtr++));
                }
            }
        }
    } else if (bit_depth == 16) {
        printf("Reading a 16-bit PNG image (Image may be darker than expected!)\n");
        for (int y = 0; y < im.height; y++) {
            png_bytep srcPtr = row_pointers[y];
            for (int x = 0; x < im.width; x++) {
                for (int c = 0; c < im.channels; c++) {
                    unsigned short val = srcPtr[0]*256 + srcPtr[1];
                    im(x, y)[c] = LDR16toHDR(val);
                    srcPtr += 2;
                }
            }
        }
    }

    // clean up
    for (int y = 0; y < im.height; y++) {
        delete[] row_pointers[y];
    }
    delete[] row_pointers;

    png_destroy_read_struct(&png_ptr, &info_ptr, NULL);

    return im;
}

void save(Window im, string filename) {
    png_structp png_ptr;
    png_infop info_ptr;
    png_bytep *row_pointers;
    png_byte color_type;

    assert(im.frames == 1, "Can't save a multi-frame PNG image\n");
    assert(im.channels > 0 && im.channels < 5,
           "Imagestack can't write PNG files that have other than 1, 2, 3, or 4 channels\n");

    png_byte color_types[4] = {PNG_COLOR_TYPE_GRAY, PNG_COLOR_TYPE_GRAY_ALPHA,
                               PNG_COLOR_TYPE_RGB,  PNG_COLOR_TYPE_RGB_ALPHA
                              };
    color_type = color_types[im.channels - 1];

    // open file
    FILE *f = fopen(filename.c_str(), "wb");
    assert(f, "[write_png_file] File %s could not be opened for writing\n", filename.c_str());

    // initialize stuff
    png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
    assert(png_ptr, "[write_png_file] png_create_write_struct failed\n");

    info_ptr = png_create_info_struct(png_ptr);
    assert(info_ptr, "[write_png_file] png_create_info_struct failed\n");

    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during init_io\n");

    png_init_io(png_ptr, f);

    // write header
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during writing header\n");

    png_set_IHDR(png_ptr, info_ptr, im.width, im.height,
                 8, color_type, PNG_INTERLACE_NONE,
                 PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);

    png_write_info(png_ptr, info_ptr);

    // convert the floats to bytes
    row_pointers = new png_bytep[im.height];
    for (int y = 0; y < im.height; y++) {
	row_pointers[y] = new png_byte[png_get_rowbytes(png_ptr, info_ptr)];
        png_bytep dstPtr = row_pointers[y];
        for (int x = 0; x < im.width; x++) {
            for (int c = 0; c < im.channels; c++) {
                *dstPtr++ = (png_byte)(HDRtoLDR(im(x, y)[c]));
            }
        }
    }

    // write data
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during writing bytes");

    png_write_image(png_ptr, row_pointers);

    // finish write
    assert(!setjmp(png_jmpbuf(png_ptr)), "[write_png_file] Error during end of write");

    png_write_end(png_ptr, NULL);

    // clean up
    for (int y = 0; y < im.height; y++) {
        delete[] row_pointers[y];
    }
    delete[] row_pointers;

    fclose(f);

    png_destroy_write_struct(&png_ptr, &info_ptr);
}

}

#endif
#include "footer.h"
<|endoftext|>


<commit_before>//class to define object model based on list of 3D edges
//edges are directed: when moves from pt1 to pt2 want black on the right
#pragma once

#include <opencv2/core.hpp>

#include <iostream>
#include <vector>

namespace thymio_tracker
{

enum CameraType {NexusCam,
		  EmbeddedCam,
		  UndefinedCam
};

struct IntrinsicCalibration {
    cv::Size imageSize;
    cv::Mat cameraMatrix;
    cv::Mat distCoeffs;

    IntrinsicCalibration(const std::string& calibrationFile)
    {
    	init(calibrationFile);
    }
    IntrinsicCalibration(CameraType _camType = UndefinedCam)
    {
	    switch (_camType)
	    {
	        case NexusCam:
	            init("../data/calibration/nexus_camera_calib.xml");
	            break;
	        case EmbeddedCam:
	            init("../data/calibration/embedded_camera_calib.xml");
	            break;
	        default:
	            imageSize = cv::Size(640,480);

	            cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
     			cameraMatrix.at<double>(0,0) = 800.0;
     			cameraMatrix.at<double>(1,1) = 600.0;
     			cameraMatrix.at<double>(2,2) = 1.0;

     			distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
	            break;
	    }
    }

    void init(const std::string& calibrationFile)
    {
    	cv::FileStorage fs;
	    fs.open(calibrationFile, cv::FileStorage::READ);

	    if(!fs.isOpened())
	        throw std::runtime_error("Calibration file not found!");

	    imageSize.width = (int) fs["image_width"];
	    imageSize.height = (int) fs["image_height"];
	    fs["camera_matrix"] >> cameraMatrix;
	    fs["distortion_coefficients"] >> distCoeffs;

	    fs.release();
    }

} ;

struct DetectionGH {
    cv::Point2f position;//blob/feature position
    int id;//estimated id from GH
    float nbVotes;//nb votes for id
    int discriminativePower;//nb votes best - nb votes second best
    DetectionGH(cv::Point2f _p, int _id, int _nv, int _d): position(_p), id(_id), nbVotes(_nv), discriminativePower(_d) {};
} ;

//get the calibration from an open file
void readCalibrationFromFileStorage(cv::FileStorage &fs, IntrinsicCalibration &calib);
void writeCalibrationToFileStorage(IntrinsicCalibration &calib,cv::FileStorage &fs);

//get camera intrisic matrix in case of resize of image
void resizeCameraMatrix(cv::Mat &_cameraMatrix, const cv::Size& sFrom, const cv::Size& sTo);
void rescaleCalibration(IntrinsicCalibration &calibration, const cv::Size& sTo);

//pixel to meter transfo and inverse
cv::Point2f toPixels(const cv::Mat& _cameraMatrix, const cv::Point2f& _m);
cv::Point2f toMeters(const cv::Mat& _cameraMatrix, const cv::Point2f& _x);
cv::Point2f toMeters(const cv::Mat& _cameraMatrix, const cv::Point2i& _x);
//project to image plane
cv::Point2f toImagePlane(const cv::Point3f& _x);
//get jacobian of point projection with respect to camera position
cv::Mat ProjectZ1_Jac_Dp(const cv::Point3f& mvLastDistCam);
//get x,y coordinates of Point3f
cv::Point2f Pointxy(const cv::Point3f& _m);

//check direction triangle
bool testDirectionBasis(cv::Point2f basis1,cv::Point2f basis2);
bool testDirectionGroup(cv::Point2f v1,cv::Point2f v2,cv::Point2f v3);

}
<commit_msg>Added missing include<commit_after>//class to define object model based on list of 3D edges
//edges are directed: when moves from pt1 to pt2 want black on the right
#pragma once

#include <opencv2/core.hpp>

#include <iostream>
#include <vector>
#include <stdexcept>

namespace thymio_tracker
{

enum CameraType {NexusCam,
		  EmbeddedCam,
		  UndefinedCam
};

struct IntrinsicCalibration {
    cv::Size imageSize;
    cv::Mat cameraMatrix;
    cv::Mat distCoeffs;

    IntrinsicCalibration(const std::string& calibrationFile)
    {
    	init(calibrationFile);
    }
    IntrinsicCalibration(CameraType _camType = UndefinedCam)
    {
	    switch (_camType)
	    {
	        case NexusCam:
	            init("../data/calibration/nexus_camera_calib.xml");
	            break;
	        case EmbeddedCam:
	            init("../data/calibration/embedded_camera_calib.xml");
	            break;
	        default:
	            imageSize = cv::Size(640,480);

	            cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
     			cameraMatrix.at<double>(0,0) = 800.0;
     			cameraMatrix.at<double>(1,1) = 600.0;
     			cameraMatrix.at<double>(2,2) = 1.0;

     			distCoeffs = cv::Mat::zeros(5, 1, CV_64F);
	            break;
	    }
    }

    void init(const std::string& calibrationFile)
    {
    	cv::FileStorage fs;
	    fs.open(calibrationFile, cv::FileStorage::READ);

	    if(!fs.isOpened())
	        throw std::runtime_error("Calibration file not found!");

	    imageSize.width = (int) fs["image_width"];
	    imageSize.height = (int) fs["image_height"];
	    fs["camera_matrix"] >> cameraMatrix;
	    fs["distortion_coefficients"] >> distCoeffs;

	    fs.release();
    }

} ;

struct DetectionGH {
    cv::Point2f position;//blob/feature position
    int id;//estimated id from GH
    float nbVotes;//nb votes for id
    int discriminativePower;//nb votes best - nb votes second best
    DetectionGH(cv::Point2f _p, int _id, int _nv, int _d): position(_p), id(_id), nbVotes(_nv), discriminativePower(_d) {};
} ;

//get the calibration from an open file
void readCalibrationFromFileStorage(cv::FileStorage &fs, IntrinsicCalibration &calib);
void writeCalibrationToFileStorage(IntrinsicCalibration &calib,cv::FileStorage &fs);

//get camera intrisic matrix in case of resize of image
void resizeCameraMatrix(cv::Mat &_cameraMatrix, const cv::Size& sFrom, const cv::Size& sTo);
void rescaleCalibration(IntrinsicCalibration &calibration, const cv::Size& sTo);

//pixel to meter transfo and inverse
cv::Point2f toPixels(const cv::Mat& _cameraMatrix, const cv::Point2f& _m);
cv::Point2f toMeters(const cv::Mat& _cameraMatrix, const cv::Point2f& _x);
cv::Point2f toMeters(const cv::Mat& _cameraMatrix, const cv::Point2i& _x);
//project to image plane
cv::Point2f toImagePlane(const cv::Point3f& _x);
//get jacobian of point projection with respect to camera position
cv::Mat ProjectZ1_Jac_Dp(const cv::Point3f& mvLastDistCam);
//get x,y coordinates of Point3f
cv::Point2f Pointxy(const cv::Point3f& _m);

//check direction triangle
bool testDirectionBasis(cv::Point2f basis1,cv::Point2f basis2);
bool testDirectionGroup(cv::Point2f v1,cv::Point2f v2,cv::Point2f v3);

}
<|endoftext|>


<commit_before>#ifndef KEYBOARD_SIGNAL_UTILS_HPP
#define KEYBOARD_SIGNAL_UTILS_HPP

#include <iostream>
#include <boost/signals2.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

using namespace std;
namespace visualization_utils {

  // control keys
  //65505 Shift(left)   65506 Shift(right)
  //65507 Control(left)   65508 Control(right)
  //65507 Control(left)   65508 Control(right)
  #define SHIFT_L 65505
  #define SHIFT_R 65506
  #define CNTRL_L 65507
  #define CNTRL_R 65508
  #define ALT_L   65513
  #define ALT_R   65514
  #define BKSPACE 65288
  #define LFTARRW 65361
  #define UPARRW  65362
  #define RGTARRW 65363
  #define DWNARRW 65364

  // A global keyboard signal with the keyval integer and a boolean
  // indicating if it's a key-press or a key-release.
  typedef boost::signals2::signal<void(int,bool)> KeyboardSignal;
  typedef boost::shared_ptr<KeyboardSignal> KeyboardSignalRef;

    // should this be interface class KeyboardSignalHandler?
    class KeyboardSignalHandler
    {
      public:
        KeyboardSignalHandler(KeyboardSignalRef signalRef, boost::function<void(int,bool)> func):_user_callback(func)
        {
          _connection =  signalRef->connect(boost::bind(&KeyboardSignalHandler::callback,this,_1,_2));
        };

        ~KeyboardSignalHandler(){
          _connection.disconnect();
        }

        void callback(int keyval, bool is_pressed)
        {
          /*if(is_pressed)
          {
            cout << "KeyPress Event:  Keyval: " << keyval << endl;
          }
          else {
            cout << "KeyRelease Event: Keyval: " << keyval << endl;
          }*/
          if(!_user_callback.empty())
             _user_callback(keyval,is_pressed);
          else
             cerr << "ERROR in visualization_utils::KeyboardSignalHandler:  user callback function reference is empty!" << endl;
        }

        private:
        boost::signals2::connection _connection;
        boost::function<void (int,bool)> _user_callback;

    };

} // end namespace

#endif //KEYBOARD_SIGNAL_UTILS_HPP
<commit_msg>adding function keys and esc to keyboard mapping<commit_after>#ifndef KEYBOARD_SIGNAL_UTILS_HPP
#define KEYBOARD_SIGNAL_UTILS_HPP

#include <iostream>
#include <boost/signals2.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

using namespace std;
namespace visualization_utils {

  // control keys
  //65505 Shift(left)   65506 Shift(right)
  //65507 Control(left)   65508 Control(right)
  //65507 Control(left)   65508 Control(right)
  #define SHIFT_L 65505
  #define SHIFT_R 65506
  #define CNTRL_L 65507
  #define CNTRL_R 65508
  #define ALT_L   65513
  #define ALT_R   65514
  #define BKSPACE 65288
  #define LFTARRW 65361
  #define UPARRW  65362
  #define RGTARRW 65363
  #define DWNARRW 65364
  #define F1 65470
  #define F2 65471
  #define F3 65472
  #define F4 65473
  #define F5 65474
  #define F6 65475
  #define F7 65476
  #define F8 65477
  #define F9 65478
  #define F10 65479
  #define F11 65480
  #define F12 65481
  #define ESC 65307

  // A global keyboard signal with the keyval integer and a boolean
  // indicating if it's a key-press or a key-release.
  typedef boost::signals2::signal<void(int,bool)> KeyboardSignal;
  typedef boost::shared_ptr<KeyboardSignal> KeyboardSignalRef;

    // should this be interface class KeyboardSignalHandler?
    class KeyboardSignalHandler
    {
      public:
        KeyboardSignalHandler(KeyboardSignalRef signalRef, boost::function<void(int,bool)> func):_user_callback(func)
        {
          _connection =  signalRef->connect(boost::bind(&KeyboardSignalHandler::callback,this,_1,_2));
        };

        ~KeyboardSignalHandler(){
          _connection.disconnect();
        }

        void callback(int keyval, bool is_pressed)
        {
          /*if(is_pressed)
          {
            cout << "KeyPress Event:  Keyval: " << keyval << endl;
          }
          else {
            cout << "KeyRelease Event: Keyval: " << keyval << endl;
          }*/
          if(!_user_callback.empty())
             _user_callback(keyval,is_pressed);
          else
             cerr << "ERROR in visualization_utils::KeyboardSignalHandler:  user callback function reference is empty!" << endl;
        }

        private:
        boost::signals2::connection _connection;
        boost::function<void (int,bool)> _user_callback;

    };

} // end namespace

#endif //KEYBOARD_SIGNAL_UTILS_HPP
<|endoftext|>


<commit_before>/*
Copyright (c) 2017, Vadim Malyshev, lboss75@gmail.com
All rights reserved
*/

#include "stdafx.h"
#include "private/udp_transport.h"
#include "messages/dht_route_messages.h"
#include "private/dht_session.h"
#include "logger.h"
#include "dht_network_client.h"
#include "dht_network_client_p.h"

vds::dht::network::udp_transport::udp_transport(){
}

vds::dht::network::udp_transport::~udp_transport() {
}

void vds::dht::network::udp_transport::start(
  const service_provider * sp,
  const std::shared_ptr<certificate> & node_cert,
  const std::shared_ptr<asymmetric_private_key> & node_key,
  uint16_t port) {
  this->send_thread_ = std::make_shared<thread_apartment>(sp);
  this->sp_ = sp;
  this->this_node_id_ = node_cert->fingerprint(hash::sha256());
  this->node_cert_ = node_cert;
  this->node_key_ = node_key;

  try {
    auto [reader, writer] = this->server_.start(sp, network_address::any_ip6(port));
    this->reader_ = reader;
    this->writer_ = writer;
  }
  catch (...) {
    auto[reader, writer] = this->server_.start(sp, network_address::any_ip4(port));
    this->reader_ = reader;
    this->writer_ = writer;
  }

      this->continue_read().detach();
}

void vds::dht::network::udp_transport::stop() {
  this->server_.stop();
}

vds::async_task<void>
vds::dht::network::udp_transport::write_async( const udp_datagram& datagram) {
  auto result = std::make_shared<vds::async_result<void>>();
  this->send_thread_->schedule([result, this, datagram]() {
    try{
      this->writer_->write_async(datagram).get();
    }
    catch (...){
      result->set_exception(std::current_exception());
      return;
    }
    result->set_value();
  });

  return result->get_future();

  //  std::unique_lock<std::debug_mutex> lock(this->write_mutex_);
  //  while(this->write_in_progress_) {
  //    this->write_cond_.wait(*reinterpret_cast<std::unique_lock<std::mutex> *>(&lock));
  //  }
  //  this->write_in_progress_ = true;
  //#ifdef _DEBUG
  //#ifndef _WIN32
  //  this->owner_id_ = syscall(SYS_gettid);
  //#else
  //  this->owner_id_ = GetCurrentThreadId();
  //#endif
  //#endif//_DEBUG
  //co_await this->writer_->write_async(sp, datagram);
}

vds::async_task<void> vds::dht::network::udp_transport::try_handshake(
                                                                  const std::string& address) {

  resizable_data_buffer out_message;
  out_message.add((uint8_t)protocol_message_type_t::HandshakeBroadcast);
  out_message.add(MAGIC_LABEL >> 24);
  out_message.add(MAGIC_LABEL >> 16);
  out_message.add(MAGIC_LABEL >> 8);
  out_message.add(MAGIC_LABEL);
  out_message.add(PROTOCOL_VERSION);

  binary_serializer bs;
  bs << this->node_cert_->der();

  out_message += bs.move_data();

  co_await this->write_async(udp_datagram(
    network_address::parse(this->server_.address().family(), address),
    out_message.move_data(),
    false));
}

vds::async_task<void> vds::dht::network::udp_transport::on_timer() {
  std::list<std::shared_ptr<dht_session>> sessions;

  this->sessions_mutex_.lock_shared();
  for(auto & p : this->sessions_) {
    if (p.second.session_) {
      sessions.push_back(p.second.session_);
    }
  }
  this->sessions_mutex_.unlock_shared();

  for(auto & s : sessions) {
    co_await s->on_timer(this->shared_from_this());
  }
}

void vds::dht::network::udp_transport::get_session_statistics(session_statistic& session_statistic) {
  session_statistic.output_size_ = this->send_thread_->size();

  std::shared_lock<std::shared_mutex> lock(this->sessions_mutex_);
  for (const auto& p : this->sessions_) {
    const auto & session = p.second;
    if (session.session_) {
      session_statistic.items_.push_back(session.session_->get_statistic());
    }
  }
}

vds::async_task<void> vds::dht::network::udp_transport::continue_read(
  ) {
  for (;;) {
    udp_datagram datagram;
    try {
      datagram = co_await this->reader_->read_async();
    }
    catch(const std::system_error & ex) {
      continue;
    }

    if (this->sp_->get_shutdown_event().is_shuting_down()) {
      co_return;
    }

    this->sessions_mutex_.lock();
    auto & session_info = this->sessions_[datagram.address()];
    this->sessions_mutex_.unlock();

    session_info.session_mutex_.lock();
    if (session_info.blocked_) {
      if ((std::chrono::steady_clock::now() - session_info.update_time_) > std::chrono::minutes(1)) {
        session_info.blocked_ = false;
      }
      else {
        session_info.session_mutex_.unlock();
        if (*datagram.data() != (uint8_t)protocol_message_type_t::Failed) {
          uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
          try {
            co_await this->write_async(udp_datagram(datagram.address(),
              const_data_buffer(out_message, sizeof(out_message))));
          }
          catch (...) {
          }
        }
        continue;
      }
    }

    switch ((protocol_message_type_t)datagram.data()[0]) {
    case protocol_message_type_t::HandshakeBroadcast:
    case protocol_message_type_t::Handshake: {

      if (session_info.session_) {
        session_info.session_mutex_.unlock();
        continue;
      }

      if (
        (uint8_t)(MAGIC_LABEL >> 24) == datagram.data()[1]
        && (uint8_t)(MAGIC_LABEL >> 16) == datagram.data()[2]
        && (uint8_t)(MAGIC_LABEL >> 8) == datagram.data()[3]
        && (uint8_t)(MAGIC_LABEL) == datagram.data()[4]
        && PROTOCOL_VERSION == datagram.data()[5]) {
        binary_deserializer bd(datagram.data() + 6, datagram.data_size() - 6);
        const_data_buffer partner_node_cert_der;
        bd >> partner_node_cert_der;
        auto partner_node_cert = certificate::parse_der(partner_node_cert_der);
        auto partner_node_id = partner_node_cert.fingerprint(hash::sha256());
        if (partner_node_id == this->this_node_id_) {
          session_info.session_mutex_.unlock();
          break;
        }

        if (!session_info.session_key_ || (std::chrono::steady_clock::now() - session_info.update_time_) > std::chrono::minutes(10)) {
          session_info.update_time_ = std::chrono::steady_clock::now();
          session_info.session_key_.resize(32);
          crypto_service::rand_bytes(session_info.session_key_.data(), session_info.session_key_.size());
        }

        session_info.session_ = std::make_shared<dht_session>(
          this->sp_,
          datagram.address(),
          this->this_node_id_,
          partner_node_id,
          session_info.session_key_);

        this->sp_->get<logger>()->debug(ThisModule, "Add session %s", datagram.address().to_string().c_str());
        (*this->sp_->get<client>())->add_session(session_info.session_, 0);

        resizable_data_buffer out_message;
        out_message.add(static_cast<uint8_t>(protocol_message_type_t::Welcome));
        out_message.add(MAGIC_LABEL >> 24);
        out_message.add(MAGIC_LABEL >> 16);
        out_message.add(MAGIC_LABEL >> 8);
        out_message.add(MAGIC_LABEL);

        binary_serializer bs;
        bs
          << this->node_cert_->der()
          << partner_node_cert.public_key().encrypt(session_info.session_key_);

        session_info.session_mutex_.unlock();

        out_message += bs.move_data();
        co_await this->write_async(udp_datagram(datagram.address(), out_message.move_data(), false));
        co_await (*this->sp_->get<client>())->send_neighbors(
          message_create<messages::dht_find_node_response>(
            std::list<messages::dht_find_node_response::target_node>({
          messages::dht_find_node_response::target_node(partner_node_id, datagram.address().to_string(), 0) })));

        co_await this->sp_->get<imessage_map>()->on_new_session(
          partner_node_id);
      }
      else {
        session_info.session_mutex_.unlock();
        throw std::runtime_error("Invalid protocol");
      }
      break;
    }
    case protocol_message_type_t::Welcome: {
      if (datagram.data_size() > 5
        && (uint8_t)(MAGIC_LABEL >> 24) == datagram.data()[1]
        && (uint8_t)(MAGIC_LABEL >> 16) == datagram.data()[2]
        && (uint8_t)(MAGIC_LABEL >> 8) == datagram.data()[3]
        && (uint8_t)(MAGIC_LABEL) == datagram.data()[4]) {

        const_data_buffer cert_buffer;
        const_data_buffer key_buffer;
        binary_deserializer bd(datagram.data() + 5, datagram.data_size() - 5);
        bd
          >> cert_buffer
          >> key_buffer;

        auto cert = certificate::parse_der(cert_buffer);
        auto key = this->node_key_->decrypt(key_buffer);

        auto partner_id = cert.fingerprint(hash::sha256());

        //TODO: validate cert
        auto session = std::make_shared<dht_session>(
          this->sp_,
          datagram.address(),
          this->this_node_id_,
          partner_id,
          key);

        session_info.session_ = session;
        session_info.session_mutex_.unlock();

        this->sp_->get<logger>()->debug(ThisModule, "Add session %s", datagram.address().to_string().c_str());
        (*this->sp_->get<client>())->add_session(session, 0);

        co_await this->sp_->get<imessage_map>()->on_new_session(
          partner_id);
      }
      else {
        session_info.session_mutex_.unlock();
        throw std::runtime_error("Invalid protocol");
      }

      break;
    }
    case protocol_message_type_t::Failed: {
      session_info.blocked_ = true;
      (*this->sp_->get<client>())->remove_session(session_info.session_);
      session_info.session_.reset();
      session_info.update_time_ = std::chrono::steady_clock::now();
      session_info.session_mutex_.unlock();
      break;
    }
    default: {
      if (session_info.session_) {
        try {
          auto session = session_info.session_;
          session_info.session_mutex_.unlock();

          bool failed = false;
          try {
            co_await session->process_datagram(
              this->shared_from_this(),
              const_data_buffer(datagram.data(), datagram.data_size()));
          }
          catch (const std::exception & ex) {
            logger::get(this->sp_)->debug(ThisModule, "%s at process message from %s",
              ex.what(),
              datagram.address().to_string().c_str());
            failed = true;
          }

          if(failed) {
            session_info.session_mutex_.lock();
            session_info.blocked_ = true;
            (*this->sp_->get<client>())->remove_session(session_info.session_);
            session_info.session_.reset();
            session_info.update_time_ = std::chrono::steady_clock::now();
            session_info.session_mutex_.unlock();

            uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
            co_await this->write_async(udp_datagram(datagram.address(),
              const_data_buffer(out_message, sizeof(out_message))));
          }
        }
        catch (...) {
          session_info.session_mutex_.lock();
          session_info.blocked_ = true;
          (*this->sp_->get<client>())->remove_session(session_info.session_);
          session_info.session_.reset();
          session_info.update_time_ = std::chrono::steady_clock::now();
          session_info.session_mutex_.unlock();
        }
      }
      else {
        session_info.blocked_ = true;
        (*this->sp_->get<client>())->remove_session(session_info.session_);
        session_info.session_.reset();
        session_info.update_time_ = std::chrono::steady_clock::now();
        session_info.session_mutex_.unlock();

        uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
        try {
          co_await this->write_async(udp_datagram(datagram.address(),
            const_data_buffer(out_message, sizeof(out_message))));
        }
        catch (...) {
        }
      }
      break;
    }
    }
  }
}

<commit_msg>change UDP MTU size<commit_after>/*
Copyright (c) 2017, Vadim Malyshev, lboss75@gmail.com
All rights reserved
*/

#include "stdafx.h"
#include "private/udp_transport.h"
#include "messages/dht_route_messages.h"
#include "private/dht_session.h"
#include "logger.h"
#include "dht_network_client.h"
#include "dht_network_client_p.h"

vds::dht::network::udp_transport::udp_transport(){
}

vds::dht::network::udp_transport::~udp_transport() {
}

void vds::dht::network::udp_transport::start(
  const service_provider * sp,
  const std::shared_ptr<certificate> & node_cert,
  const std::shared_ptr<asymmetric_private_key> & node_key,
  uint16_t port) {
  this->send_thread_ = std::make_shared<thread_apartment>(sp);
  this->sp_ = sp;
  this->this_node_id_ = node_cert->fingerprint(hash::sha256());
  this->node_cert_ = node_cert;
  this->node_key_ = node_key;

  try {
    auto [reader, writer] = this->server_.start(sp, network_address::any_ip6(port));
    this->reader_ = reader;
    this->writer_ = writer;
  }
  catch (...) {
    auto[reader, writer] = this->server_.start(sp, network_address::any_ip4(port));
    this->reader_ = reader;
    this->writer_ = writer;
  }

      this->continue_read().detach();
}

void vds::dht::network::udp_transport::stop() {
  this->server_.stop();
}

vds::async_task<void>
vds::dht::network::udp_transport::write_async( const udp_datagram& datagram) {
  auto result = std::make_shared<vds::async_result<void>>();
  this->send_thread_->schedule([result, this, datagram]() {
    try{
      this->writer_->write_async(datagram).get();
    }
    catch (...){
      result->set_exception(std::current_exception());
      return;
    }
    result->set_value();
  });

  return result->get_future();

  //  std::unique_lock<std::debug_mutex> lock(this->write_mutex_);
  //  while(this->write_in_progress_) {
  //    this->write_cond_.wait(*reinterpret_cast<std::unique_lock<std::mutex> *>(&lock));
  //  }
  //  this->write_in_progress_ = true;
  //#ifdef _DEBUG
  //#ifndef _WIN32
  //  this->owner_id_ = syscall(SYS_gettid);
  //#else
  //  this->owner_id_ = GetCurrentThreadId();
  //#endif
  //#endif//_DEBUG
  //co_await this->writer_->write_async(sp, datagram);
}

vds::async_task<void> vds::dht::network::udp_transport::try_handshake(
                                                                  const std::string& address) {

  resizable_data_buffer out_message;
  out_message.add((uint8_t)protocol_message_type_t::HandshakeBroadcast);
  out_message.add(MAGIC_LABEL >> 24);
  out_message.add(MAGIC_LABEL >> 16);
  out_message.add(MAGIC_LABEL >> 8);
  out_message.add(MAGIC_LABEL);
  out_message.add(PROTOCOL_VERSION);

  binary_serializer bs;
  bs << this->node_cert_->der();

  out_message += bs.move_data();

  co_await this->write_async(udp_datagram(
    network_address::parse(this->server_.address().family(), address),
    out_message.move_data(),
    false));
}

vds::async_task<void> vds::dht::network::udp_transport::on_timer() {
  std::list<std::shared_ptr<dht_session>> sessions;

  this->sessions_mutex_.lock_shared();
  for(auto & p : this->sessions_) {
    if (p.second.session_) {
      sessions.push_back(p.second.session_);
    }
  }
  this->sessions_mutex_.unlock_shared();

  for(auto & s : sessions) {
    co_await s->on_timer(this->shared_from_this());
  }
}

void vds::dht::network::udp_transport::get_session_statistics(session_statistic& session_statistic) {
  session_statistic.output_size_ = this->send_thread_->size();

  std::shared_lock<std::shared_mutex> lock(this->sessions_mutex_);
  for (const auto& p : this->sessions_) {
    const auto & session = p.second;
    if (session.session_) {
      session_statistic.items_.push_back(session.session_->get_statistic());
    }
  }
}

vds::async_task<void> vds::dht::network::udp_transport::continue_read(
  ) {
  for (;;) {
    udp_datagram datagram;
    try {
      datagram = co_await this->reader_->read_async();
    }
    catch(const std::system_error & ex) {
      continue;
    }

    if (this->sp_->get_shutdown_event().is_shuting_down()) {
      co_return;
    }

    this->sessions_mutex_.lock();
    auto & session_info = this->sessions_[datagram.address()];
    this->sessions_mutex_.unlock();

    session_info.session_mutex_.lock();
    if (session_info.blocked_) {
      if ((std::chrono::steady_clock::now() - session_info.update_time_) > std::chrono::minutes(1)) {
        session_info.blocked_ = false;
      }
      else {
        session_info.session_mutex_.unlock();
        if (*datagram.data() != (uint8_t)protocol_message_type_t::Failed
          && *datagram.data() != (uint8_t)protocol_message_type_t::Handshake
          && *datagram.data() != (uint8_t)protocol_message_type_t::HandshakeBroadcast) {
          uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
          try {
            co_await this->write_async(udp_datagram(datagram.address(),
              const_data_buffer(out_message, sizeof(out_message))));
          }
          catch (...) {
          }
        }
        continue;
      }
    }

    switch ((protocol_message_type_t)datagram.data()[0]) {
    case protocol_message_type_t::HandshakeBroadcast:
    case protocol_message_type_t::Handshake: {

      if (session_info.session_) {
        session_info.session_mutex_.unlock();
        continue;
      }

      if (
        (uint8_t)(MAGIC_LABEL >> 24) == datagram.data()[1]
        && (uint8_t)(MAGIC_LABEL >> 16) == datagram.data()[2]
        && (uint8_t)(MAGIC_LABEL >> 8) == datagram.data()[3]
        && (uint8_t)(MAGIC_LABEL) == datagram.data()[4]
        && PROTOCOL_VERSION == datagram.data()[5]) {
        binary_deserializer bd(datagram.data() + 6, datagram.data_size() - 6);
        const_data_buffer partner_node_cert_der;
        bd >> partner_node_cert_der;
        auto partner_node_cert = certificate::parse_der(partner_node_cert_der);
        auto partner_node_id = partner_node_cert.fingerprint(hash::sha256());
        if (partner_node_id == this->this_node_id_) {
          session_info.session_mutex_.unlock();
          break;
        }

        if (!session_info.session_key_ || (std::chrono::steady_clock::now() - session_info.update_time_) > std::chrono::minutes(10)) {
          session_info.update_time_ = std::chrono::steady_clock::now();
          session_info.session_key_.resize(32);
          crypto_service::rand_bytes(session_info.session_key_.data(), session_info.session_key_.size());
        }

        session_info.session_ = std::make_shared<dht_session>(
          this->sp_,
          datagram.address(),
          this->this_node_id_,
          partner_node_id,
          session_info.session_key_);

        this->sp_->get<logger>()->debug(ThisModule, "Add session %s", datagram.address().to_string().c_str());
        (*this->sp_->get<client>())->add_session(session_info.session_, 0);

        resizable_data_buffer out_message;
        out_message.add(static_cast<uint8_t>(protocol_message_type_t::Welcome));
        out_message.add(MAGIC_LABEL >> 24);
        out_message.add(MAGIC_LABEL >> 16);
        out_message.add(MAGIC_LABEL >> 8);
        out_message.add(MAGIC_LABEL);

        binary_serializer bs;
        bs
          << this->node_cert_->der()
          << partner_node_cert.public_key().encrypt(session_info.session_key_);

        session_info.session_mutex_.unlock();

        out_message += bs.move_data();
        co_await this->write_async(udp_datagram(datagram.address(), out_message.move_data(), false));
        co_await (*this->sp_->get<client>())->send_neighbors(
          message_create<messages::dht_find_node_response>(
            std::list<messages::dht_find_node_response::target_node>({
          messages::dht_find_node_response::target_node(partner_node_id, datagram.address().to_string(), 0) })));

        co_await this->sp_->get<imessage_map>()->on_new_session(
          partner_node_id);
      }
      else {
        session_info.session_mutex_.unlock();
        throw std::runtime_error("Invalid protocol");
      }
      break;
    }
    case protocol_message_type_t::Welcome: {
      if (datagram.data_size() > 5
        && (uint8_t)(MAGIC_LABEL >> 24) == datagram.data()[1]
        && (uint8_t)(MAGIC_LABEL >> 16) == datagram.data()[2]
        && (uint8_t)(MAGIC_LABEL >> 8) == datagram.data()[3]
        && (uint8_t)(MAGIC_LABEL) == datagram.data()[4]) {

        const_data_buffer cert_buffer;
        const_data_buffer key_buffer;
        binary_deserializer bd(datagram.data() + 5, datagram.data_size() - 5);
        bd
          >> cert_buffer
          >> key_buffer;

        auto cert = certificate::parse_der(cert_buffer);
        auto key = this->node_key_->decrypt(key_buffer);

        auto partner_id = cert.fingerprint(hash::sha256());

        //TODO: validate cert
        auto session = std::make_shared<dht_session>(
          this->sp_,
          datagram.address(),
          this->this_node_id_,
          partner_id,
          key);

        session_info.session_ = session;
        session_info.session_mutex_.unlock();

        this->sp_->get<logger>()->debug(ThisModule, "Add session %s", datagram.address().to_string().c_str());
        (*this->sp_->get<client>())->add_session(session, 0);

        co_await this->sp_->get<imessage_map>()->on_new_session(
          partner_id);
      }
      else {
        session_info.session_mutex_.unlock();
        throw std::runtime_error("Invalid protocol");
      }

      break;
    }
    case protocol_message_type_t::Failed: {
      session_info.blocked_ = true;
      (*this->sp_->get<client>())->remove_session(session_info.session_);
      session_info.session_.reset();
      session_info.update_time_ = std::chrono::steady_clock::now();
      session_info.session_mutex_.unlock();
      break;
    }
    default: {
      if (session_info.session_) {
        try {
          auto session = session_info.session_;
          session_info.session_mutex_.unlock();

          bool failed = false;
          try {
            co_await session->process_datagram(
              this->shared_from_this(),
              const_data_buffer(datagram.data(), datagram.data_size()));
          }
          catch (const std::exception & ex) {
            logger::get(this->sp_)->debug(ThisModule, "%s at process message from %s",
              ex.what(),
              datagram.address().to_string().c_str());
            failed = true;
          }

          if(failed) {
            session_info.session_mutex_.lock();
            session_info.blocked_ = true;
            (*this->sp_->get<client>())->remove_session(session_info.session_);
            session_info.session_.reset();
            session_info.update_time_ = std::chrono::steady_clock::now();
            session_info.session_mutex_.unlock();

            uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
            co_await this->write_async(udp_datagram(datagram.address(),
              const_data_buffer(out_message, sizeof(out_message))));
          }
        }
        catch (...) {
          session_info.session_mutex_.lock();
          session_info.blocked_ = true;
          (*this->sp_->get<client>())->remove_session(session_info.session_);
          session_info.session_.reset();
          session_info.update_time_ = std::chrono::steady_clock::now();
          session_info.session_mutex_.unlock();
        }
      }
      else {
        session_info.blocked_ = true;
        (*this->sp_->get<client>())->remove_session(session_info.session_);
        session_info.session_.reset();
        session_info.update_time_ = std::chrono::steady_clock::now();
        session_info.session_mutex_.unlock();

        uint8_t out_message[] = { (uint8_t)protocol_message_type_t::Failed };
        try {
          co_await this->write_async(udp_datagram(datagram.address(),
            const_data_buffer(out_message, sizeof(out_message))));
        }
        catch (...) {
        }
      }
      break;
    }
    }
  }
}

<|endoftext|>