text:
/*****************************************************************************/
/*  Copyright (c) 2015, Alessandro Pieropan                                  */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include "../include/pose_estimation.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/math/constants/constants.hpp>
#include "../include/utilities.h"
#include <eigen3/Eigen/Geometry>
#include <algorithm>
#include <stdexcept>
#include <iostream>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace fato {

void getPoseRansac(const std::vector<cv::Point3f>& model_points,
                   const std::vector<cv::Point2f>& tracked_points,
                   const cv::Mat& camera_model, int iterations, float distance,
                   vector<int>& inliers, Mat& rotation, Mat& translation) {
  if (model_points.size() > 4) {
    Mat rotation_vect;
    solvePnPRansac(model_points, tracked_points, camera_model,
                   Mat::zeros(1, 8, CV_32F), rotation_vect, translation, false,
                   iterations, distance, model_points.size(), inliers, CV_P3P);

    try {
      Rodrigues(rotation_vect, rotation);
      rotation.convertTo(rotation, CV_32FC1);
    } catch (cv::Exception& e) {
      cout << "Error estimating ransac rotation: " << e.what() << endl;
    }

  } else {
    rotation = Mat(3, 3, CV_32FC1, 0.0f);
    translation = Mat(1, 3, CV_32FC1, 0.0f);
  }
}

void getMatrices(const std::vector<cv::Point2f>& prev_pts,
                 const std::vector<float>& prev_depth,
                 const std::vector<cv::Point2f>& next_pts, float nodal_x,
                 float nodal_y, float focal_x, float focal_y,
                 Eigen::MatrixXf& A, Eigen::VectorXf& b) {
  int valid_points = prev_pts.size();

  A = Eigen::MatrixXf(valid_points * 2, 6);
  b = Eigen::VectorXf(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < valid_points; ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }
}

void getPoseFromFlow(const std::vector<cv::Point2f>& prev_pts,
                     const std::vector<float>& prev_depth,
                     const std::vector<cv::Point2f>& next_pts, float nodal_x,
                     float nodal_y, float focal_x, float focal_y,
                     vector<float>& translation, vector<float>& rotation) {
  int valid_points = prev_pts.size();

  Eigen::MatrixXf A(valid_points * 2, 6);
  Eigen::VectorXf b(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < prev_pts.size(); ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);
  // solving least square to find the 6 unknown parameters: tx,ty, tz, wx, wy,
  // wz
  Eigen::VectorXf Y = (A.transpose() * A).ldlt().solve(A.transpose() * b);

  //  cout << "LQ: " << fixed << setprecision(3) << Y[0] << " " << Y[1] << " "
  //  << Y[2] << " "
  //                 << Y[3] << " " << Y[4] << " " << Y[5] << endl;

  translation[0] = Y[0];
  translation[1] = Y[1];
  translation[2] = Y[2];

  rotation[0] = Y[3];
  rotation[1] = Y[4];
  rotation[2] = Y[5];
}

Eigen::VectorXf getPoseFromFlowRobust(const std::vector<cv::Point2f>& prev_pts,
                           const std::vector<float>& prev_depth,
                           const std::vector<cv::Point2f>& next_pts,
                           float nodal_x, float nodal_y, float focal_x,
                           float focal_y, int num_iters,
                           std::vector<float>& translation,
                           std::vector<float>& rotation,
                           std::vector<int>& outliers) {
  Eigen::MatrixXf X;
  Eigen::VectorXf y;

  getMatrices(prev_pts, prev_depth, next_pts, nodal_x, nodal_y, focal_x,
              focal_y, X, y);

  // during tracking possible to initialize with the previous estimation???
  Eigen::VectorXf beta = (X.transpose() * X).ldlt().solve(X.transpose() * y);

  Eigen::VectorXf W;
  Eigen::MatrixXf XW(X.rows(), X.cols());

  for (auto i = 0; i < num_iters; ++i) {
    // cout << "iter " << i;// << endl;

    Eigen::VectorXf residuals = ((X * beta) - y);
    residuals = residuals.array().abs();

    // cout << residuals << endl;

    std::vector<float> res_vector(residuals.rows(), 0);

    for (int j = 0; j < residuals.rows(); ++j) {
      res_vector.at(j) = residuals(j);
    }

    sort(res_vector.begin(), res_vector.end());

    float residual_scale;
    if (res_vector.size() % 2 != 0)
      residual_scale = res_vector.at(res_vector.size() / 2);
    else
      residual_scale = res_vector.at(res_vector.size() / 2) +
                       res_vector.at(res_vector.size() / 2 + 1) / 2.0f;

    // avoid division by 0
    residual_scale = max(residual_scale, 0.0001f);

    residual_scale *=
        6.9460;  // constant defined in the IRLS and bisquare distance

    W = residuals / residual_scale;

    for (auto j = 0; j < W.rows(); ++j) {
      if (W(j) > 1) {
        W(j) = 0;
      } else {
        auto tmp = 1 - W(j) * W(j);
        W(j) = tmp * tmp;
      }
    }

    for (auto j = 0; j < XW.rows(); ++j) {
      for (auto k = 0; k < XW.cols(); ++k) {
        XW(j, k) = W(j) * X(j, k);
      }
    }

    beta = (XW.transpose() * X).ldlt().solve(XW.transpose() * y);
  }

  // cout << "\n here" << endl;

  int num_points = W.rows() / 2;

  for (auto j = 0; j < num_points; ++j) {
    int id = 2 * j;
    if (W(id) == 0 || W(id + 1) == 0) {
      outliers.push_back(j);
    }
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);

  translation[0] = beta[0];
  translation[1] = beta[1];
  translation[2] = beta[2];

  rotation[0] = beta[3];
  rotation[1] = beta[4];
  rotation[2] = beta[5];

  return beta;
}

void getPose2D(const std::vector<cv::Point2f*>& model_points,
               const std::vector<cv::Point2f*>& tracked_points, float& scale,
               float& angle) {
  vector<double> angles;
  vector<float> scales;

  angles.reserve(model_points.size() * 2);
  scales.reserve(model_points.size() * 2);

  const double pi = boost::math::constants::pi<double>();

  for (size_t i = 0; i < model_points.size(); ++i) {
    for (size_t j = 0; j < model_points.size(); j++) {
      // computing angle
      Point2f a = *tracked_points.at(i) - *tracked_points.at(j);
      Point2f b = *model_points.at(i) - *model_points.at(j);
      double val = atan2(a.y, a.x) - atan2(b.y, b.x);

      if (abs(val) > pi) {
        int sign = (val < 0) ? -1 : 1;
        val = val - sign * 2 * pi;
      }
      angles.push_back(val);

      if (i == j) continue;

      // computing scale
      auto tracked_dis =
          getDistance(tracked_points.at(i), tracked_points.at(j));
      auto model_dis = getDistance(model_points.at(i), model_points.at(j));
      if (model_dis != 0) scales.push_back(tracked_dis / model_dis);
    }
  }

  sort(angles.begin(), angles.end());
  sort(scales.begin(), scales.end());

  auto angles_size = angles.size();
  auto scale_size = scales.size();

  if (angles_size == 0)
    angle = 0;

  else if (angles_size % 2 == 0)
    angle = (angles[angles_size / 2 - 1] + angles[angles_size / 2]) / 2;
  else
    angle = angles[angles_size / 2];

  if (scale_size == 0)
    scale = 1;
  else if (scale_size % 2 == 0)
    scale = (scales[scale_size / 2 - 1] + scales[scale_size / 2]) / 2;
  else
    scale = scales[scale_size / 2];
}

// TODO: implement ransac approach for better SVD estimation
Mat getRigidTransform(Mat& a, Mat& b) {
  int numRows = a.rows;

  vector<float> srcM = {0, 0, 0};
  vector<float> dstM = {0, 0, 0};

  // compute centroid of the two sets of points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      srcM[j] += a.at<float>(i, j);
      dstM[j] += b.at<float>(i, j);
    }
  }

  for (int i = 0; i < 3; i++) {
    srcM[i] = srcM[i] / static_cast<float>(numRows);
    dstM[i] = dstM[i] / static_cast<float>(numRows);
  }

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  // cout << "H\n" << H << endl;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

Mat getRigidTransform(Mat& a, Mat& b, vector<float>& cA, vector<float>& cB) {
  int numRows = a.rows;

  vector<float>& srcM = cA;
  vector<float>& dstM = cB;

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

void rotateBBox(const vector<Point3f>& bBox, const Mat& rotation,
                vector<Point3f>& updatedBBox) {
  Mat a(4, 3, CV_32FC1);
  Mat b, b_T, a_T;

  for (size_t i = 0; i < 4; i++) {
    a.at<float>(i, 0) = bBox.at(i).x;
    a.at<float>(i, 1) = bBox.at(i).y;
    a.at<float>(i, 2) = bBox.at(i).z;
  }

  transpose(a, a_T);

  b_T = rotation * a_T;
  transpose(b_T, b);

  for (size_t i = 0; i < 4; i++) {
    updatedBBox.push_back(
        Point3f(b.at<float>(i, 0), b.at<float>(i, 1), b.at<float>(i, 2)));
  }
}

void rotatePoint(const Point3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point.x;
  a.at<float>(1) = point.y;
  a.at<float>(2) = point.z;
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation, Vec3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint[0] = b.at<float>(0);
  updatedPoint[1] = b.at<float>(1);
  updatedPoint[2] = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotationVecToMat(const Mat& vec, Mat& mat) {}

/**************************************************************/
/*           POSE CLASS                                       */
/**************************************************************/

Pose::Pose() { pose_ = Matrix4d(4, 4); }

Pose::Pose(Mat& r_mat, Mat& t_vect) {
  pose_ = Matrix4d(4, 4);

  if (t_vect.cols != 3 && t_vect.rows != 3) {
    cout << t_vect.cols << " " << t_vect.rows << endl;
    throw std::runtime_error(
        "Pose: bad translation vector format, 3x1 accepted");
  }

  Mat tmp_rot, tmp_t;
  r_mat.convertTo(tmp_rot, CV_64FC1);
  t_vect.convertTo(tmp_t, CV_64FC1);

  if (tmp_rot.cols == 3 && tmp_rot.rows == 3) {
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = tmp_rot.at<double>(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else if (r_mat.cols == 3 && r_mat.cols == 1) {
    Eigen::Matrix3d rot_view;
    rot_view =
        Eigen::AngleAxisd(tmp_rot.at<double>(2), Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(1), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(0), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else {
    throw std::runtime_error(
        "Pose: bad rotation matrix format, 3x3 or 1x3 accepted");
  }

  init_rot = r_mat;
  init_tr = t_vect;
}

Pose::Pose(Matrix4d& pose) {
  pose_ = Matrix4d(4, 4);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      pose_(i, j) = pose(i, j);
    }
  }
}

Pose::Pose(std::vector<double>& beta) {
  if (beta.size() != 6) {
    throw std::runtime_error("Pose: bad parameters size, should be 6");
  }

  pose_ = Matrix4d(4, 4);

  Eigen::Matrix3d rot_view;
  rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      pose_(i, j) = rot_view(i, j);
    }
    pose_(3, i) = 0;
  }
  pose_(3, 3) = 1;

  pose_(0, 3) = beta.at(0);
  pose_(1, 3) = beta.at(1);
  pose_(2, 3) = beta.at(2);

  init_beta_ = beta;
}

Pose::Pose(VectorXf &beta)
{
    if (beta.rows() != 6) {
      throw std::runtime_error("Pose: bad parameters size, should be 6");
    }

    init_beta_.resize(6,0);

    for(auto i = 0; i < 6; ++i)
        init_beta_[i] = static_cast<double>(beta[i]);

    pose_ = Matrix4d(4, 4);

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(init_beta_.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(init_beta_.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(init_beta_.at(3), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;

    pose_(0, 3) = init_beta_.at(0);
    pose_(1, 3) = init_beta_.at(1);
    pose_(2, 3) = init_beta_.at(2);
}

std::pair<cv::Mat, cv::Mat> Pose::toCV() const {
  cv::Mat rotation = cv::Mat(3, 3, CV_64FC1, 0.0f);
  cv::Mat translation = cv::Mat(1, 3, CV_64FC1, 0.0f);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      rotation.at<double>(i, j) = pose_(i, j);
    }
    translation.at<double>(i) = pose_(i, 3);
  }

  return pair<cv::Mat, cv::Mat>(rotation, translation);
}

std::pair<Eigen::Matrix3d, Eigen::Vector3d> Pose::toEigen() const
{
    Eigen::Matrix3d rotation_temp;
    Eigen::Vector3d translation_vect;
    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
          rotation_temp(i,j) = pose_(i,j);
        }
        translation_vect(i) = pose_(i,3);
    }

    return pair<Eigen::Matrix3d, Eigen::Vector3d>(rotation_temp, translation_vect);
}

glm::mat4 Pose::toGL() const
{

    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
          Rx_180(1, 1) = -1.0;
          Rx_180(2, 2) = -1.0;
    auto pose = Rx_180 * pose_;

    glm::mat4 pose_glm;

    for(auto i = 0; i < 4; ++i)
    {
        for(auto j = 0; j < 4; ++j)
        {
          pose_glm[j][i] = pose(i,j);
        }
    }

    return pose_glm;
}

void Pose::transform(Eigen::Matrix4d& transform) { pose_ = transform * pose_; }

void Pose::transform(std::vector<double> &beta)
{

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

    Eigen::Matrix4d transform;

    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
            transform(i,j) = rot_view(i,j);
        }
        transform(i,3) = beta[i];
        transform(3,i) = 0;
    }
    transform(3,3) = 1;

//    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
//    Rx_180(1, 1) = -1.0;
//    Rx_180(2, 2) = -1.0;

//    transform = Rx_180 * transform;

    pose_ = transform * pose_;
}

vector<double> Pose::getBeta() {
  Eigen::Matrix3d tmp_mat(3, 3);

  for (auto i = 0; i < 3; ++i) {
    for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = pose_(i, j);
  }

  Eigen::Vector3d angles = tmp_mat.eulerAngles(2, 1, 0);

  vector<double> tmp = {pose_(0, 3), pose_(1, 3), pose_(2, 3),
                        angles(2),   angles(1),   angles(0)};

  return tmp;
}

vector<double> Pose::translation() const
{
    return vector<double>{pose_(0, 3), pose_(1, 3), pose_(2, 3)};
}

Eigen::Quaternionf Pose::rotation() const
{
    Eigen::Matrix3f tmp_mat(3, 3);

    for (auto i = 0; i < 3; ++i) {
      for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = (float)pose_(i, j);
    }

    return Quaternionf(tmp_mat);
}

string Pose::str() const {
  stringstream ss;
  ss << fixed << setprecision(3);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      ss << pose_(i, j) << " ";
    }
    ss << "\n";
  }

  return ss.str();
}

}  // end namespace

max_stars_repo_name: clickcao/fato

max_issues_repo_name: clickcao/fato

max_forks_repo_name: clickcao/fato


text:
#include "mainprocessor.h"
#define ARMA_64BIT_WORD
// #define ARMA_NO_DEBUG
#include <armadillo>
//using namespace arma;

#include <QTimer>
#include <itkImageFileWriter.h>
#include <iostream>

MainProcessor::MainProcessor(QCoreApplication* a, QObject *parent) :
                QObject(parent), app(a)
{
    begin = std::chrono::high_resolution_clock::now();
    nifti2Pgm = new Nifti2Pgm();
    edisonWrapper = new EdisonWrapper();
    connect(edisonWrapper, SIGNAL(allProcessesFinished()), this, SLOT(enhanceContrast()));
}

MainProcessor::~MainProcessor()
{
    delete  nifti2Pgm;
    delete edisonWrapper;
    app->quit();
}

void MainProcessor::runProcesses(QFileInfoList imageFiles,
                                 QString edisonPath, QString outDir)
{

    this->imageFiles = imageFiles;

    if(!edisonWrapper->setProgramPath(edisonPath)) {
        std::cout << "Wrong path to Edison " << edisonPath.toStdString() << std::endl;
        return;
    }
    this->outputDir = outDir;

    for(int i = 0; i < 2; i++) {
        nifti2Pgm->setNiftiFile(imageFiles.at(i).absoluteFilePath(), i);
    }

    QList<arma::fcube> cubes = nifti2Pgm->getCubesList();
    FL_org = cubes.at(0);
    nifti2Pgm->normalizeFlair();
    nifti2Pgm->binarizeLB();
    nifti2Pgm->createOutputCubes();
    QString pgmFileName = nifti2Pgm->writePGM(outputDir);

    //edisonWrapper->setParameters(parameters);
    edisonWrapper->setImages(imageFiles);
    edisonWrapper->setOutputPaths(outputDir);
    edisonWrapper->createOutputNames();
    edisonWrapper->createEdisonFile(pgmFileName);
    edisonWrapper->launchStage();
    // enhanceContrast() is launched by signal from edisonWrapper.
}

void MainProcessor::enhanceContrast()
{
    QList<arma::fcube> cubes = nifti2Pgm->getCubesList();
    arma::fcube* FL = &(cubes[0]);
    arma::fcube* LB = &(cubes[1]);
    arma::fmat* im2edison = nifti2Pgm->getIm2edison();
    //arma::fmat* im2pgm = nifti2Pgm->getIm2pgm();
    QString gradientFileName = edisonWrapper->getGradientFile();
    QFileInfo gradientFile(gradientFileName);
    if(!gradientFile.exists()) {
        std::cout << "Gradient file " << gradientFileName.toStdString()
                  << " does not exist" << std::endl;
        return;
    }

    arma::fmat gradientMap;
    gradientMap.load(gradientFileName.toStdString(), arma::raw_ascii);

    FL_en = arma::zeros<arma::fcube>(arma::size(*FL));
//    FL_en = *FL;
    uint bins_g = 1024;
    uint bins_i = 1024;
    arma::fvec gradientMapVec = arma::vectorise(gradientMap);
    arma::fvec im2edisonVec = arma::vectorise(*im2edison);
    arma::umat N = hist3(&gradientMapVec, &im2edisonVec, bins_g, bins_i);
    N.col(0) = arma::zeros<arma::uvec>(bins_g);
    arma::umat NC = cumsum(cumsum(N, 0), 1);

    float max_iwm =im2edison->max();
    arma::fvec i_l = arma::linspace<arma::fvec>(0, max_iwm, bins_i);
    arma::fvec g_l = arma::linspace<arma::fvec>(0, 1, bins_g);
    arma::fvec p = arma::zeros<arma::fvec>(bins_g);
    for(arma::uword gi = 0; gi < bins_g; gi++) {
        unsigned long long sz = sum(NC.col(gi));
        if(sz == 0)
            p(gi) = 0;
        else {
            float sm = sum(NC.col(gi) % g_l);
            p(gi) = sm/static_cast<float>(sz);
        }
    }

    arma::fvec P = cumsum(p % i_l);

    for(arma::uword its = 0; its < FL->n_slices; its++) {
        for(arma::uword i=0; i < FL->n_rows; i++) {
            for(arma::uword j=0; j < FL->n_cols; j ++) {
//                ulong ind = static_cast<ulong>(roundf((*im2pgm)(i + its * FL->n_rows, j) * bins_g));
                ulong ind = static_cast<ulong>(roundf((*im2edison)(i + its * FL->n_rows, j) * bins_g));
                if(ind == 0)
                    FL_en(i, j, its) = 0;
                else
                    FL_en(i, j, its) = (P(ind - 1));
            }
        }
    }
    QPair<float, float> minmax = nifti2Pgm->getMinmaxFlair();
    float maxFL = minmax.second;
    float minFL = minmax.first;
    float minFL_en = FL_en.min();
    float maxFL_en = FL_en.max();
    FL_en = (FL_en - minFL_en) / (maxFL_en - minFL_en)*(maxFL-minFL) + minFL;
    arma::uvec ind = find((*LB) < 0.5);
    FL_en(ind) = FL_org(ind);

    typedef itk::Image< float, 3 > ITKImageType;
    typename ITKImageType::Pointer inputImage;

    std::memcpy(nifti2Pgm->getFlairImage().GetPointer()->GetBufferPointer(),
                FL_en.memptr(), FL_en.n_elem*sizeof(float));

    using ImageType = itk::Image<float, 3>;
    using WriterType = itk::ImageFileWriter<ImageType>;
    WriterType::Pointer writer = WriterType::New();
    QString suffix = imageFiles.at(0).completeSuffix();
    enhancedFileInfo.setFile(QDir(outputDir),
                       imageFiles.at(0).baseName() + "-enhanced." + suffix);
    writer->SetFileName(enhancedFileInfo.absoluteFilePath().toStdString());
    writer->SetInput(nifti2Pgm->getFlairImage().GetPointer());
    writer->Update();
    endMessages();
}

arma::umat MainProcessor::hist3(arma::fvec *X, arma::fvec *Y, uint n_binsX, uint n_binsY)
{
    arma::umat histogram;
    if(X->n_elem != Y->n_elem) {
        std::cout << "Vectors with different number of elements" << std::endl;
        return histogram;
    }

    histogram.zeros(n_binsX, n_binsY);
    arma::uword num_elems = X->n_elem;
    float minX = X->min();
    float maxX = X->max();
    float minY = Y->min();
    float maxY = Y->max();
    float lengthX = maxX - minX;
    float lengthY = maxY - minY;
    uint indexX, indexY;
    for(arma::uword i = 0; i < num_elems; i++) {
        indexX = static_cast<uint>(truncf((X->at(i) - minX) * n_binsX / lengthX));
        indexY = static_cast<uint>(truncf((Y->at(i) - minY) * n_binsY / lengthY));
        if(indexX == n_binsX)
            --indexX;
        if(indexY == n_binsY)
            --indexY;
        histogram(indexX, indexY) += 1;
    }

    return histogram;
}

void MainProcessor::endMessages()
{
    auto end = std::chrono::high_resolution_clock::now();
    std::cout << std::endl << "Images generated:" << std::endl;
    std::cout << enhancedFileInfo.absoluteFilePath().toStdString() << std::endl;
    std::cout << std::endl << QString::number(std::chrono::duration_cast<std::chrono::nanoseconds>
                 (end - begin).count()/1000000.0).toStdString() << "ms." << std::endl;
    std::cout << "Contrast enhancement finished" << std::endl;
    app->quit();
}

ext: cpp


text:
/* The file is part of Snowman decompiler. */
/* See doc/licenses.asciidoc for the licensing information. */

//
// SmartDec decompiler - SmartDec is a native code to C/C++ decompiler
// Copyright (C) 2015 Alexander Chernov, Katerina Troshina, Yegor Derevenets,
// Alexander Fokin, Sergey Levin, Leonid Tsvetkov
//
// This file is part of SmartDec decompiler.
//
// SmartDec decompiler is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// SmartDec decompiler is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with SmartDec decompiler.  If not, see <http://www.gnu.org/licenses/>.
//

#include "StructureAnalyzer.h"

#include <algorithm>
#include <queue>

#include <boost/unordered_map.hpp>

#include <nc/common/Foreach.h>
#include <nc/common/Range.h>
#include <nc/common/make_unique.h>

#include <nc/core/ir/BasicBlock.h>
#include <nc/core/ir/Jump.h>
#include <nc/core/ir/dflow/Utils.h>
#include <nc/core/ir/misc/ArrayAccess.h>
#include <nc/core/ir/misc/BoundsCheck.h>
#include <nc/core/ir/misc/PatternRecognition.h>

#include "BasicNode.h"
#include "Dfs.h"
#include "Edge.h"
#include "Graph.h"
#include "LoopExplorer.h"
#include "Switch.h"

namespace nc {
namespace core {
namespace ir {
namespace cflow {

void StructureAnalyzer::analyze() {
    analyze(graph_.root());
}

void StructureAnalyzer::analyze(Region *region) {
    bool changed;

    do {
        changed = false;

        /*
         * Classify edges, sort nodes topologically.
         */
        Dfs dfs(region);

        /*
         * Try to reduce various kinds of regions.
         */
        foreach (Node *node, dfs.postordering()) {
            if (reduceCompoundCondition(node)) {
                changed = true;
                break;
            }
        }
        if (changed) {
            continue;
        }

        foreach (Node *node, dfs.postordering()) {
            if (reduceCyclic(node, dfs)) {
                changed = true;
                break;
            }
        }
        if (changed) {
            continue;
        }

        foreach (Node *node, dfs.postordering()) {
            if (reduceBlock(node)) {
                changed = true;
                break;
            }
        }
        if (changed) {
            continue;
        }

        foreach (Node *node, dfs.postordering()) {
            if (reduceConditional(node)) {
                changed = true;
                break;
            }
        }
        if (changed) {
            continue;
        }

        foreach (Node *node, dfs.postordering()) {
            if (reduceSwitch(node) || reduceHopelessConditional(node)) {
                changed = true;
                break;
            }
        }
        if (changed) {
            continue;
        }
    } while (changed);
}

bool StructureAnalyzer::reduceBlock(Node *entry) {
    Node *uniquePredecessor = entry->uniquePredecessor();

    /*
     * Blocks are only needed for reducing ifs.
     * If the unique predecessor cannot be an if condition, stop.
     */
    if (!uniquePredecessor || !uniquePredecessor->isFork() || !uniquePredecessor->isCondition()) {
        return false;
    }

    /* Discover all the nodes in the block. */
    std::vector<Node *> traverse;
    Node *node = entry;
    do {
        traverse.push_back(node);
        node = node->uniqueSuccessor();
    } while (node && node->uniquePredecessor());

    /* Finally, create the subregion. */
    if (traverse.size() > 1) {
        auto subregion = std::make_unique<Region>(Region::BLOCK);
        subregion->setEntry(entry);
        subregion->nodes() = std::move(traverse);
        return insertSubregion(entry->parent(), std::move(subregion));
    }

    return false;
}

bool StructureAnalyzer::reduceConditional(Node *entry) {
    if (!entry->isFork() || !entry->isCondition()) {
        return false;
    }

    Node *left = entry->outEdges()[0]->head();
    Node *right = entry->outEdges()[1]->head();

    /*
     * IF_THEN_ELSE
     */
    if (left->inEdges().size() == 1 && right->inEdges().size() == 1 &&
        left->outEdges().size() <= 1 && right->outEdges().size() <= 1 &&
        (left->outEdges().empty() || right->outEdges().empty() ||
         left->outEdges()[0]->head() == right->outEdges()[0]->head()))
    {
        auto subregion = std::make_unique<Region>(Region::IF_THEN_ELSE);
        subregion->setEntry(entry);
        subregion->nodes().push_back(entry);
        subregion->nodes().push_back(left);
        subregion->nodes().push_back(right);
        return insertSubregion(entry->parent(), std::move(subregion));
    }

    /*
     * IF_THEN
     */
#define REDUCE(left, right)                                                         \
    if (left->inEdges().size() == 1 &&                                              \
        (left->outEdges().empty() ||                                                \
         (left->outEdges().size() == 1 && left->outEdges()[0]->head() == right)))   \
    {                                                                               \
        auto subregion = std::make_unique<Region>(Region::IF_THEN);                 \
        subregion->setEntry(entry);                                                 \
        subregion->nodes().push_back(entry);                                        \
        subregion->nodes().push_back(left);                                         \
        subregion->setExitBasicBlock(right->getEntryBasicBlock());                  \
        return insertSubregion(entry->parent(), std::move(subregion));              \
    }
    REDUCE(left, right)
    REDUCE(right, left)
#undef REDUCE

    return false;
}

bool StructureAnalyzer::reduceHopelessConditional(Node *entry) {
    if (!entry->isFork() || !entry->isCondition()) {
        return false;
    }

    Node *left = entry->outEdges()[0]->head();
    Node *right = entry->outEdges()[1]->head();

    auto subregion = std::make_unique<Region>(Region::IF_THEN_ELSE);
    subregion->setEntry(entry);
    subregion->nodes().push_back(entry);
    subregion->nodes().push_back(left);
    subregion->nodes().push_back(right);
    return insertSubregion(entry->parent(), std::move(subregion));
}

bool StructureAnalyzer::reduceCompoundCondition(Node *entry) {
    if (!entry->isFork() || !entry->isCondition()) {
        return false;
    }

    Node *left = entry->outEdges()[0]->head();
    Node *right = entry->outEdges()[1]->head();

#define REDUCE(left, right)                                                                 \
    if (left->inEdges().size() == 1 && left->isFork() && left->isCondition() &&             \
        ((left->outEdges()[0]->head() == right && left->outEdges()[1]->head() != entry) ||  \
         (left->outEdges()[1]->head() == right && left->outEdges()[0]->head() != entry)))   \
    {                                                                                \
        auto subregion = std::make_unique<Region>(Region::COMPOUND_CONDITION);              \
        subregion->setEntry(entry);                                                         \
        subregion->nodes().push_back(entry);                                                \
        subregion->nodes().push_back(left);                                                 \
        return insertSubregion(entry->parent(), std::move(subregion));                      \
    }
    REDUCE(left, right)
    REDUCE(right, left)
#undef REDUCE

    return false;
}

namespace {

struct LoopDescription {
    Region::RegionKind kind;
    Node *condition;
    Node *bodyEntry;
    Node *exitNode;

    LoopDescription(Region::RegionKind kind, Node *condition, Node *bodyEntry, Node *exitNode):
        kind(kind), condition(condition), bodyEntry(bodyEntry), exitNode(exitNode)
    {}
};

} // anonymous namespace

bool StructureAnalyzer::reduceCyclic(Node *entry, const Dfs &dfs) {
    /*
     * Try to find the nodes constituting the loop.
     */
    LoopExplorer explorer(entry, dfs);

    if (explorer.loopNodes().empty()) {
        return false;
    }

    /*
     * Create the region.
     */
    auto subregion = std::make_unique<Region>(Region::LOOP);
    subregion->setEntry(entry);
    subregion->nodes() = std::move(explorer.loopNodes());

    /*
     * Potential condition nodes, together with the respective loop
     * exit blocks. We need to compute it before doing the structural
     * analysis on the loop subregion, because we need the edges before
     * their redirection by insertSubregion().
     */
    std::vector<LoopDescription> loopDescriptions;

    auto detectLoop = [&](Region::RegionKind kind, Node *condition, Node *bodyEntry, Node *loopExit) {
        if (!nc::contains(subregion->nodes(), loopExit)) {
            assert(nc::contains(subregion->nodes(), bodyEntry));
            loopDescriptions.push_back(LoopDescription(kind, condition, bodyEntry, loopExit));
        }
    };

    /*
     * For a DO_WHILE loop.
     */
    foreach (Edge *edge, entry->inEdges()) {
        if (dfs.getEdgeType(edge) == Dfs::BACK) {
            Node *n = edge->tail();
            if (n->isFork() && n->isCondition()) {
                detectLoop(Region::DO_WHILE, n, n->outEdges()[0]->head(), n->outEdges()[1]->head());
                detectLoop(Region::DO_WHILE, n, n->outEdges()[1]->head(), n->outEdges()[0]->head());
            }
        }
    }

    /*
     * For a WHILE loop.
     */
    if (entry->isFork() && entry->isCondition()) {
        detectLoop(Region::WHILE, entry, entry->outEdges()[0]->head(), entry->outEdges()[1]->head());
        detectLoop(Region::WHILE, entry, entry->outEdges()[1]->head(), entry->outEdges()[0]->head());
    }

    /*
     * Insert the subregion, redirect edges.
     */
    Region *loop = insertSubregion(entry->parent(), std::move(subregion));
    if (!loop) {
        return false;
    }

    /*
     * Remove 'continue' edges. They only make the structural
     * analysis in the loop region harder.
     */
    std::vector<Edge *> continueEdges = entry->inEdges();
    foreach (Edge *edge, continueEdges) {
        edge->setTail(nullptr);
        edge->setHead(nullptr);
    }

    /*
     * Run structural analysis inside the loop region.
     */
    analyze(loop);

    /*
     * Try to find a condition node.
     */
    foreach (const auto &description, loopDescriptions) {
        if (description.condition->parent() == loop) {
            if (description.kind == Region::WHILE) {
                /*
                 * analyze() could put the body entry somewhere
                 * in the middle of some other region, in which
                 * case we will not be able to fall through into
                 * it from the while condition.
                 */
                auto uniqueSuccessor = description.condition->uniqueSuccessor();
                if (!uniqueSuccessor ||
                    description.bodyEntry->getEntryBasicBlock() != uniqueSuccessor->getEntryBasicBlock()) {
                    continue;
                }
            }

            loop->setRegionKind(description.kind);
            loop->setLoopCondition(description.condition);
            loop->setExitBasicBlock(description.exitNode->getEntryBasicBlock());
            break;
        }
    }

    return true;
}

bool StructureAnalyzer::reduceSwitch(Node *entry) {
    /*
     * Do not detect the same switch multiple times.
     */
    if (auto witch = entry->parent()->as<Switch>()) {
        if (witch->switchNode() == entry) {
            return false;
        }
    }

    /*
     * Entry must be a basic block node.
     */
    BasicNode *basicEntry = entry->as<BasicNode>();

    if (!basicEntry) {
        return false;
    }

    /*
     * Entry must end with an unconditional jump using a jump table,
     * and for which we can trace the jump index.
     */
    misc::ArrayAccess arrayAccess;
    std::size_t jumpTableSize;

    if (const Jump *jump = basicEntry->basicBlock()->getJump()) {
        if (jump->isUnconditional() && jump->thenTarget().table()) {
            arrayAccess = misc::recognizeArrayAccess(jump->thenTarget().address(), dataflow_);
            jumpTableSize = jump->thenTarget().table()->size();
        }
    }

    if (!arrayAccess) {
        return false;
    }

    /*
     * Typically, there is a bounds check before the switch:
     *
     * if (x < 10) {
     *     switch (x) {
     *       ...
     *     }
     * }
     */
    BasicNode *boundsCheckNode = nullptr;

    if (Node *node = entry->uniquePredecessor()) {
        if (BasicNode *basicBlockNode = node->as<BasicNode>()) {
            if (const Jump *jump = basicBlockNode->basicBlock()->getJump()) {
                if (auto boundsCheck = misc::recognizeBoundsCheck(jump, entry->getEntryBasicBlock(), dataflow_)) {
                    if (dflow::getFirstCopy(boundsCheck.index(), dataflow_) ==
                        dflow::getFirstCopy(arrayAccess.index(), dataflow_))
                    {
                        boundsCheckNode = basicBlockNode;
                        jumpTableSize = std::min(jumpTableSize, static_cast<std::size_t>(boundsCheck.maxValue() + 1));
                    }
                }
            }
        }
    }

    /*
     * Node getting control if the bounds check fails is either an exit or a default.
     */
    Node *exitOrDefaultBranch = nullptr;

    if (boundsCheckNode) {
        exitOrDefaultBranch = boundsCheckNode->getOtherSuccessor(entry);
    }

    /*
     * Direct successors of entry node and exitOrDefaultBranch are branches of the switch.
     */
    std::vector<Node *> branches;

    foreach (Edge *edge, entry->outEdges()) {
        branches.push_back(edge->head());
    }

    if (exitOrDefaultBranch) {
        branches.push_back(exitOrDefaultBranch);
    }

    std::sort(branches.begin(), branches.end());
    branches.erase(std::unique(branches.begin(), branches.end()), branches.end());

    /*
     * Using BFS, for each branch, compute the nodes reachable solely from this branch.
     */
    boost::unordered_map<Node *, Node *> node2branch;
    std::queue<Node *> queue;

    foreach (Node *node, branches) {
        queue.push(node);
        node2branch[node] = node;
    }

    while (!queue.empty()) {
        foreach (Edge *outEdge, queue.front()->outEdges()) {
            if (!nc::find(node2branch, outEdge->head())) {
                Node *branch = nullptr;

                foreach (Edge *inEdge, outEdge->head()->inEdges()) {
                    if (Node *inBranch = nc::find(node2branch, inEdge->tail())) {
                        if (branch == nullptr) {
                            branch = inBranch;
                        } else if (branch != inBranch) {
                            branch = nullptr;
                            break;
                        }
                    } else {
                        branch = nullptr;
                        break;
                    }
                }

                if (branch != nullptr) {
                    node2branch[outEdge->head()] = branch;
                    queue.push(outEdge->head());
                }
            }
        }

        queue.pop();
    }

    /*
     * Returns how many branches actually join in the given node.
     */
    auto getJoinDegree = [&](const Node *node) -> std::size_t {
        std::vector<Node *> incomingBranches;

        foreach (const Edge *edge, node->inEdges()) {
            incomingBranches.push_back(find(node2branch, edge->tail()));
        }

        std::sort(incomingBranches.begin(), incomingBranches.end());
        incomingBranches.erase(std::unique(incomingBranches.begin(), incomingBranches.end()), incomingBranches.end());

        return incomingBranches.size();
    };

    /*
     * One of the branches can be actually an exit branch, i.e. go outside
     * the switch region.
     */
    Node *exitBranch = nullptr;

    /* More than so many other branches must join in the exit branch. */
    std::size_t exitBranchJoinDegree = 2;

    foreach (Node *branch, branches) {
        auto mergeDegree = getJoinDegree(branch);
        if (mergeDegree > exitBranchJoinDegree) {
            exitBranchJoinDegree = mergeDegree;
            exitBranch = branch;
        }
    }

    /* If bounds check does not lead to an exit, it leads to the default branch. */
    Node *defaultBranch = nullptr;

    if (exitBranch != exitOrDefaultBranch) {
        defaultBranch = exitOrDefaultBranch;
    }

    /*
     * Create the region.
     */
    auto subregion = std::make_unique<Switch>(basicEntry, arrayAccess.index(), jumpTableSize);

    subregion->nodes().push_back(entry);

    if (boundsCheckNode) {
        subregion->nodes().push_back(boundsCheckNode);
        subregion->setBoundsCheckNode(boundsCheckNode);
        subregion->setEntry(boundsCheckNode);
    } else {
        subregion->setEntry(entry);
    }

    if (exitBranch) {
        subregion->setExitBasicBlock(exitBranch->getEntryBasicBlock());
    }

    if (defaultBranch) {
        subregion->setDefaultBasicBlock(defaultBranch->getEntryBasicBlock());
    }

    foreach (const auto &pair, node2branch) {
        assert(pair.second != nullptr);

        if (pair.second != exitBranch) {
            subregion->nodes().push_back(pair.first);
        }
    }

    /*
     * Install this new region, redirect edges.
     */
    return insertSubregion(entry->parent(), std::move(subregion));
}

Region *StructureAnalyzer::insertSubregion(Region *region, std::unique_ptr<Region> subregion) {
    assert(region != nullptr);
    assert(subregion != nullptr);

    if (region->entry() == subregion->entry()) {
        region->setEntry(subregion.get());
    } else if (nc::contains(subregion->nodes(), region->entry())) {
        return nullptr;
    }

    foreach (auto node, subregion->nodes()) {
        node->setParent(subregion.get());
    }

    region->nodes().erase(
        std::remove_if(
            region->nodes().begin(),
            region->nodes().end(),
            [&](Node *n) { return n->parent() == subregion.get(); }),
        region->nodes().end());

    subregion->setParent(region);
    region->nodes().push_back(subregion.get());

    /*
     * Redirect edges properly.
     */

    std::vector<Edge *> edgesToSubregion;
    std::vector<Edge *> edgesFromSubregion;
    std::vector<Edge *> duplicateEdges;

    std::vector<Node *> tails;
    std::vector<Node *> heads;

    foreach (Node *node, subregion->nodes()) {
        foreach (Edge *edge, node->inEdges()) {
            assert(edge->tail()->parent() == region || edge->tail()->parent() == subregion.get());

            if (edge->tail()->parent() == region) {
                if (edge->head() == subregion->entry() && !nc::contains(tails, edge->tail())) {
                    edgesToSubregion.push_back(edge);
                    tails.push_back(edge->tail());
                } else {
                    duplicateEdges.push_back(edge);
                }
            }
        }
        foreach (Edge *edge, node->outEdges()) {
            assert(edge->head()->parent() == region || edge->head()->parent() == subregion.get());

            if (edge->head()->parent() == region) {
                if (!nc::contains(heads, edge->head())) {
                    edgesFromSubregion.push_back(edge);
                    heads.push_back(edge->head());
                } else {
                    duplicateEdges.push_back(edge);
                }
            }
        }
    }

    foreach (Edge *edge, edgesToSubregion) {
        edge->setHead(subregion.get());
    }
    foreach (Edge *edge, edgesFromSubregion) {
        edge->setTail(subregion.get());
    }
    foreach (Edge *edge, duplicateEdges) {
        edge->setTail(nullptr);
        edge->setHead(nullptr);
    }

    return graph_.addNode(std::move(subregion));
}

} // namespace cflow
} // namespace ir
} // namespace core
} // namespace nc

/* vim:set et sts=4 sw=4: */


text:
//
// Copyright (c) 2018-2019 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
// Official repository: https://github.com/boostorg/beast
//

// Test that header file is self-contained.
#include <boost/beast/_experimental/json/basic_parser.hpp>

#include <boost/beast/_experimental/unit_test/suite.hpp>

#include <boost/beast/core/buffers_range.hpp>
#include <boost/beast/core/error.hpp>
#include <boost/beast/core/string.hpp>
#include <boost/asio/buffer.hpp>

namespace boost {
namespace beast {
namespace json {

class basic_parser_test : public beast::unit_test::suite
{
public:
    struct test_parser
        : basic_parser
    {
        test_parser() = default;

        void
        on_document_begin(
            error_code&) override
        {
        }

        void
        on_object_begin(
            error_code&) override
        {
        }

        void
        on_object_end(
            error_code&) override
        {
        }

        void
        on_array_begin(
            error_code&) override
        {
        }

        void
        on_array_end(
            error_code&) override
        {
        }

        void
        on_key_data(
            string_view,
            error_code&) override
        {
        }

        void
        on_key_end(
            string_view,
            error_code&) override
        {
        }

        void
        on_string_data(
            string_view,
            error_code&) override
        {
        }

        void
        on_string_end(
            string_view,
            error_code&) override
        {
        }

        void
        on_number(
            number,
            error_code&) override
        {
        }

        void
        on_bool(
            bool,
            error_code&) override
        {
        }

        void
        on_null(error_code&) override
        {
        }
    };

    void
    good(string_view s)
    {
        error_code ec;
        for(std::size_t i = 0;
            i < s.size() - 1; ++i)
        {
            // write_some with 1 buffer
            {
                test_parser p;
                auto used = p.write_some(
                    net::const_buffer(s.data(), i), ec);
                BEAST_EXPECT(used == i);
                BEAST_EXPECT(! p.is_done());
                if(! BEAST_EXPECTS(! ec, ec.message()))
                    continue;
                used = p.write_some(net::const_buffer(
                    s.data() + i, s.size() - i), ec);
                BEAST_EXPECT(used == s.size() - i);
                if(! BEAST_EXPECTS(! ec, ec.message()))
                    continue;
                p.write({}, ec);
                BEAST_EXPECTS(! ec, ec.message());
                BEAST_EXPECT(p.is_done());
            }
            // write_some with 1 buffer sequence
            {
                test_parser p;
                std::array<
                    net::const_buffer, 2> b;
                b[0] = {s.data(), i};
                b[1] = {s.data()+i, s.size()-i};
                auto used = p.write_some(b, ec);
                BEAST_EXPECT(used = s.size());
                BEAST_EXPECTS(! ec, ec.message());
                p.write({}, ec);
                BEAST_EXPECTS(! ec, ec.message());
            }
            // write with 1 buffer sequence
            {
                test_parser p;
                std::array<
                    net::const_buffer, 2> b;
                b[0] = {s.data(), i};
                b[1] = {s.data()+i, s.size()-i};
                auto used = p.write(b, ec);
                BEAST_EXPECT(used = s.size());
                BEAST_EXPECTS(! ec, ec.message());
            }
            // write with 1 buffer
            {
                test_parser p;
                auto used = p.write(
                    {s.data(), s.size()}, ec);
                BEAST_EXPECT(used = s.size());
                BEAST_EXPECTS(! ec, ec.message());
            }
        }
    }

    void
    bad(string_view s)
    {
        error_code ec;
        test_parser p;
        auto const used = p.write_some(
            boost::asio::const_buffer(
                s.data(), s.size()), ec);
        if(! ec)
        {
            if(p.is_done())
            {
                if(BEAST_EXPECT(used != s.size()))
                    return;
            }
            else
            {
                p.write_eof(ec);
                if(BEAST_EXPECT(ec))
                    return;
            }
        }
        else
        {
            pass();
            return;
        }
        log << "fail: \"" << s << "\"\n";
    }

    void
    testObject()
    {
        good("{}");
        good("{ }");
        good("{ \t }");
        good("{ \"x\" : null }");
        good("{ \"x\" : {} }");
        good("{ \"x\" : { \"y\" : null } }");

        bad ("{");
        bad ("{{}}");
    }

    void
    testArray()
    {
        good("[]");
        good("[ ]");
        good("[ \t ]");
        good("[ \"\" ]");
        good("[ \" \" ]");
        good("[ \"x\" ]");
        good("[ \"x\", \"y\" ]");
        bad ("[");
        bad ("[ \"x\", ]");
    }

    void
    testString()
    {
        good("\""   "x"         "\"");
        good("\""   "xy"        "\"");
        good("\""   "x y"       "\"");

        bad ("\""   "\t"        "\"");
    }

    void
    testNumber()
    {
        good("0");
        good("0.0");
        good("0.10");
        good("0.01");
        good("1");
        good("10");
        good("1.5");
        good("10.5");
        good("10.25");
        good("10.25e0");
        good("1e1");
        good("1e10");
        good("1e+0");
        good("1e+1");
        good("0e+10");
        good("0e-0");
        good("0e-1");
        good("0e-10");
        good("1E+1");
        good("-0");
        good("-1");
        good("-1e1");

        bad ("");
        bad ("-");
        bad ("00");
        bad ("00.");
        bad ("00.0");
        bad ("1a");
        bad (".");
        bad ("1.");
        bad ("1+");
        bad ("0.0+");
        bad ("0.0e+");
        bad ("0.0e-");
        bad ("0.0e0-");
        bad ("0.0e");

    }

    void
    testMonostates()
    {
        good("true");
        good(" true");
        good("true ");
        good("\ttrue");
        good("true\t");
        good("\r\n\t true\r\n\t ");
        bad ("truu");
        bad ("tu");
        bad ("t");

        good("false");
        bad ("fals");
        bad ("fel");
        bad ("f");

        good("null");
        bad ("nul");
        bad ("no");
        bad ("n");
    }

    void run() override
    {
        log <<
            "sizeof(basic_parser) == " <<
            sizeof(basic_parser) << "\n";
        testObject();
        testArray();
        testString();
        testNumber();
        testMonostates();
    }
};

BEAST_DEFINE_TESTSUITE(beast,json,basic_parser);

} // json
} // beast
} // boost

max_stars_repo_name: djarek/BeastLounge

max_issues_repo_name: djarek/BeastLounge

max_forks_repo_name: djarek/BeastLounge


text: #include <boost/mpl/aux_/preprocessed/bcc_pre590/advance_backward.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino