#include <math.h>
#include <limits.h>

#include <cstdint>
#include <map>

// C++11
#include <memory>

#include <boost/make_shared.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include "myutility.h"
#include "MyCalibration.h"

#define INVALID_PIXEL_VALUE 0

extern bool g_DebugFlg;

Eigen::Vector2i g_points[4];
int g_picking_counter = -1;
Eigen::Vector2i g_picked_points[CORRESPOND_POINTS];

<<<<<<< HEAD
void mouseCallback(int event, int x, int y, int flag, void*)
=======
void mouseCallback(int event, int x,int y, int flag, void*)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  static int point = 0;
  std::string desc;
  switch (event)
  {
<<<<<<< HEAD
    case cv::EVENT_LBUTTONUP:
      if (flag & cv::EVENT_FLAG_LBUTTON)
      {
        g_points[point][0] = x;
        g_points[point][1] = y;
        point++;
        if (point == 4)
        {
          point = 0;
        }
      }
      break;
    case cv::EVENT_RBUTTONUP:
      if (g_picking_counter >= 0 && (flag & cv::EVENT_FLAG_RBUTTON))
      {
        g_picked_points[g_picking_counter][0] = x;
        g_picked_points[g_picking_counter][1] = y;
        std::cout << "Picked [" << g_picking_counter << "]: "
                  << "(" << x << ", " << y << ")" << std::endl;
        g_picking_counter++;
        if (g_picking_counter >= CORRESPOND_POINTS)
        {
          g_picking_counter = -1;
        }
      }
      break;
=======
  case cv::EVENT_LBUTTONUP:
    if (flag & cv::EVENT_FLAG_LBUTTON)
    {
      g_points[point][0] = x;
      g_points[point][1] = y;
      point++;
      if(point == 4)
      {
        point = 0;
      }
    }
    break;
  case cv::EVENT_RBUTTONUP:
    if (g_picking_counter >=0 && (flag & cv::EVENT_FLAG_RBUTTON))
    {
      g_picked_points[g_picking_counter][0] = x;
      g_picked_points[g_picking_counter][1] = y;
      std::cout << "Picked [" << g_picking_counter << "]: "
        << "(" << x << ", " << y  << ")" << std::endl;
      g_picking_counter++;
      if (g_picking_counter >= CORRESPOND_POINTS)
      {
        g_picking_counter = -1;
      }
    }
    break;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  }
  return;
}

<<<<<<< HEAD
int MyCalibration::initialize(pcl::visualization::CloudViewer& viewer, uint8_t* depth_frame_region,
                              uint8_t* color_frame_region)
{
  // correct_mirroring << -1,0,0,0,1,0,0,0,1;
  {  // Setting streams
=======
int MyCalibration::initialize(
    pcl::visualization::CloudViewer& viewer,
    uint8_t *depth_frame_region,
    uint8_t *color_frame_region)
{
  //correct_mirroring << -1,0,0,0,1,0,0,0,1;
  { // Setting streams
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    if (depth_frame_region != NULL)
    {
      depth_frame = depth_frame_region;
    }
    else
    {
      std::cout << "Failed to set pointer of depth frame." << std::endl;
      return -1;
    }
    if (color_frame_region != NULL)
    {
      color_frame = color_frame_region;
    }
    else
    {
      std::cout << "Failed to set pointer of color frame." << std::endl;
      return -1;
    }
  }
  cloud->width = CAMERA_RESOLUTION_X;
<<<<<<< HEAD
  cloud->height = CAMERA_RESOLUTION_Y;
=======
  cloud->height= CAMERA_RESOLUTION_Y;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  cloud->is_dense = false;
  cloud->points.resize(cloud->height * cloud->width);
  this->viewer = &viewer;

  int i;
  g_picking_counter = 0;
<<<<<<< HEAD
  for (i = 0; i < CORRESPOND_POINTS; i++)
=======
  for (i=0; i<CORRESPOND_POINTS; i++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  {
    g_picked_points[i] = Eigen::Vector2i::Zero();
  }
  return 0;
}

int MyCalibration::extractPlanePoints()
{
  int i, j;
<<<<<<< HEAD
  std::vector< Eigen::Vector3f > all_points;
=======
  std::vector<Eigen::Vector3f> all_points;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7

  // Sort points
  int min_cross;
  int tmp_cross;
  int min_cross_num;
  Eigen::Vector2i tmp_vec[3];
<<<<<<< HEAD
  for (i = 0; i < 4; i++)
  {
    min_cross_num = i;
    min_cross = INT_MAX;
    for (j = i; j < 4; j++)
=======
  for (i = 0; i<4; i++)
  {
    min_cross_num = i;
      min_cross = INT_MAX;
    for (j = i; j<4; j++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    {
      if (i == 0)
      {
        tmp_vec[0] = g_points[j];
        tmp_vec[1] = g_points[i];
      }
      else
      {
<<<<<<< HEAD
        tmp_vec[0] = g_points[j] - g_points[i - 1];
        tmp_vec[1] = g_points[i] - g_points[i - 1];
=======
        tmp_vec[0] = g_points[j] - g_points[i-1];
        tmp_vec[1] = g_points[i] - g_points[i-1];
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      }

      tmp_cross = tmp_vec[0][0] * tmp_vec[1][1] - tmp_vec[0][1] * tmp_vec[1][0];
      if (tmp_cross < min_cross)
      {
        min_cross = tmp_cross;
        min_cross_num = j;
      }
    }
    tmp_vec[3] = g_points[i];
    g_points[i] = g_points[min_cross_num];
    g_points[min_cross_num] = tmp_vec[3];
  }

  // Detect closed region
  tmp_vec[0] = Eigen::Vector2i::Zero(2);
  for (i = 0; i < 4; i++)
  {
    tmp_vec[0] += g_points[i] / 4;
  }

<<<<<<< HEAD
  // Setting points
  int check;
  bool check_flg;
  std::vector< char > points_attribute;
=======

  // Setting points
  int check;
  bool check_flg;
  std::vector<char> points_attribute;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  Eigen::Vector3f tmp_vec3[1];

  points_on_plane.clear();
  all_points.clear();
<<<<<<< HEAD
  all_points.resize(CAMERA_RESOLUTION_X * CAMERA_RESOLUTION_Y, Eigen::Vector3f(0, 0, 0));
=======
  all_points.resize(CAMERA_RESOLUTION_X * CAMERA_RESOLUTION_Y, Eigen::Vector3f(0,0,0));
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  other_points.clear();
  points_attribute.clear();
  points_attribute.resize(CAMERA_RESOLUTION_X * CAMERA_RESOLUTION_Y, (char)0x00);

  // Get static point cloud
<<<<<<< HEAD
  for (i = 0; i < CAMERA_RESOLUTION_Y; i++)
  {
    for (j = 0; j < CAMERA_RESOLUTION_X; j++)
    {
      float* p;
      p = (float*)&points->data[(i * CAMERA_RESOLUTION_X + j) * sizeof(float) * 4];
      all_points[i * CAMERA_RESOLUTION_X + j][0] = *p * 1000;
      p++;
      all_points[i * CAMERA_RESOLUTION_X + j][1] = *p * 1000;
      p++;
      all_points[i * CAMERA_RESOLUTION_X + j][2] = *p * 1000;
    }
  }

  std::vector< Eigen::Vector2i > loop_stack;
  loop_stack.clear();
  loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1]));
  while (!loop_stack.empty())
  {  // Detecting points on plane : 0x01
    tmp_vec[0] = loop_stack.back();
    loop_stack.pop_back();
    if (points_attribute[tmp_vec[0][0] + tmp_vec[0][1] * CAMERA_RESOLUTION_X] & 0x01)
    {
      continue;
    }
    for (i = 0; i < 4; i++)
=======
  for (i=0; i<CAMERA_RESOLUTION_Y; i++)
  {
    for (j=0; j<CAMERA_RESOLUTION_X; j++)
    {
      float *p;
      p = (float*)&points->data[(i*CAMERA_RESOLUTION_X+j)*sizeof(float)*4];
      all_points[i*CAMERA_RESOLUTION_X+j][0] = *p*1000; p++;
      all_points[i*CAMERA_RESOLUTION_X+j][1] = *p*1000; p++;
      all_points[i*CAMERA_RESOLUTION_X+j][2] = *p*1000;
    }
  }

  std::vector<Eigen::Vector2i> loop_stack;
  loop_stack.clear();
  loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1]));
  while (!loop_stack.empty())
  { // Detecting points on plane : 0x01
    tmp_vec[0] = loop_stack.back();
    loop_stack.pop_back();
    if (points_attribute[tmp_vec[0][0]+tmp_vec[0][1]*CAMERA_RESOLUTION_X]&0x01)
    {
      continue;
    }
    for (i = 0; i<4; i++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    {
      check = 0;
      check_flg = true;
      tmp_vec[1] = tmp_vec[0] - g_points[i];
      if (i == 3)
      {
        tmp_vec[2] = g_points[0] - g_points[i];
      }
      else
      {
<<<<<<< HEAD
        tmp_vec[2] = g_points[i + 1] - g_points[i];
      }
      check = (tmp_vec[2][0] * tmp_vec[1][1] - tmp_vec[2][1] * tmp_vec[1][0]);
=======
        tmp_vec[2] = g_points[i+1] - g_points[i];
      }
      check = (tmp_vec[2][0]*tmp_vec[1][1] - tmp_vec[2][1] * tmp_vec[1][0]);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      if (check > 0)
      {
        check_flg = false;
        break;
      }
    }
    if (check_flg)
    {
      // This point is on the plane. (because user chooses a region of a plane)
<<<<<<< HEAD
      points_attribute[tmp_vec[0][0] + tmp_vec[0][1] * CAMERA_RESOLUTION_X] |= 0x01;
      if (!(points_attribute[tmp_vec[0][0] + 1 + tmp_vec[0][1] * CAMERA_RESOLUTION_X] & 0x01))
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0] + 1, tmp_vec[0][1]));
      }
      if (!(points_attribute[tmp_vec[0][0] - 1 + tmp_vec[0][1] * CAMERA_RESOLUTION_X] & 0x01))
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0] - 1, tmp_vec[0][1]));
      }
      if (!(points_attribute[tmp_vec[0][0] + (tmp_vec[0][1] + 1) * CAMERA_RESOLUTION_X] & 0x01))
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1] + 1));
      }
      if (!(points_attribute[tmp_vec[0][0] + (tmp_vec[0][1] - 1) * CAMERA_RESOLUTION_X] & 0x01))
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1] - 1));
=======
      points_attribute[tmp_vec[0][0]+tmp_vec[0][1]*CAMERA_RESOLUTION_X] |= 0x01;
      if (!(points_attribute[tmp_vec[0][0]+1+tmp_vec[0][1]*CAMERA_RESOLUTION_X]&0x01) )
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0]+1, tmp_vec[0][1]));
      }
      if (!(points_attribute[tmp_vec[0][0]-1+tmp_vec[0][1]*CAMERA_RESOLUTION_X]&0x01) )
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0]-1, tmp_vec[0][1]));
      }
      if (!(points_attribute[tmp_vec[0][0]+(tmp_vec[0][1]+1)*CAMERA_RESOLUTION_X]&0x01) )
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1]+1));
      }
      if (!(points_attribute[tmp_vec[0][0]+(tmp_vec[0][1]-1)*CAMERA_RESOLUTION_X]&0x01) )
      {
        loop_stack.push_back(Eigen::Vector2i(tmp_vec[0][0], tmp_vec[0][1]-1));
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      }
    }
  }

  const int radius = 3;
  int k;
  int now;
<<<<<<< HEAD
  for (k = 0; k < CORRESPOND_POINTS; k++)
  {  // Detecting points around picked points : 0x02
    unsigned int _cnt = 0;
    picked_points[k] = Eigen::Vector3f::Zero();
    for (i = g_picked_points[k][1] - radius; i <= g_picked_points[k][1] + radius; i++)
    {
      for (j = -sqrt(pow(radius, 2.0) - pow((i - g_picked_points[k][1]), 2.0)) + g_picked_points[k][0];
           j <= sqrt(pow(radius, 2.0) - pow((i - g_picked_points[k][1]), 2.0)) + g_picked_points[k][0]; j++)
      {
        if (i >= 0 && i < CAMERA_RESOLUTION_Y && j >= 0 && j < CAMERA_RESOLUTION_X)
        {  // picked_points_around
          now = i * CAMERA_RESOLUTION_X + j;
=======
  for (k=0; k<CORRESPOND_POINTS; k++)
  { // Detecting points around picked points : 0x02
    unsigned int _cnt = 0;
    picked_points[k] = Eigen::Vector3f::Zero();
    for (i=g_picked_points[k][1]-radius; i<=g_picked_points[k][1]+radius; i++)
    {
      for (j=-sqrt(pow(radius,2.0)-pow((i-g_picked_points[k][1]),2.0))+g_picked_points[k][0];
           j<=sqrt(pow(radius,2.0)-pow((i-g_picked_points[k][1]),2.0))+g_picked_points[k][0]; j++)
      {
        if(i>=0 && i<CAMERA_RESOLUTION_Y && j>=0 && j<CAMERA_RESOLUTION_X)
        { // picked_points_around
          now = i*CAMERA_RESOLUTION_X+j;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
          points_attribute[now] |= 0x02;
          picked_points[k] += all_points[now];
          _cnt++;
        }
      }
    }
<<<<<<< HEAD
    picked_points[k] = picked_points[k] / (double)_cnt;
  }

  for (i = 0; i < CAMERA_RESOLUTION_Y; i++)
  {  // Add to vector as attribute
    for (j = 0; j < CAMERA_RESOLUTION_X; j++)
    {
      now = (i * CAMERA_RESOLUTION_X + j);
=======
    picked_points[k] = picked_points[k]/(double)_cnt;
  }

  for (i=0; i<CAMERA_RESOLUTION_Y; i++)
  { // Add to vector as attribute
    for (j=0; j<CAMERA_RESOLUTION_X; j++)
    {
      now = (i*CAMERA_RESOLUTION_X+j);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      if (points_attribute[now] == 0x00)
      {
        other_points.push_back(&all_points[now]);
      }
      if (points_attribute[now] & 0x01)
      {
        points_on_plane.push_back(&all_points[now]);
<<<<<<< HEAD
        if ((i + j) % 2 == 0)
        {
          image.at< uchar >(i, j) = 255;
=======
        if ((i+j)%2==0)
        {
          image.at<uchar>(i,j) = 255;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        }
      }
      if (points_attribute[now] & 0x02)
      {
        picked_points_around.push_back(&all_points[now]);
      }
    }
  }

  std::cout << "Extracted number of points: " << points_on_plane.size() << std::endl;

  cv::imshow("Depth image", image);

  return 0;
}

int MyCalibration::viewPoints()
{
  int i;
<<<<<<< HEAD
  std::vector< Eigen::Vector3f* >::iterator it;
  Eigen::Vector3f* ptmp;
=======
  std::vector<Eigen::Vector3f*>::iterator it;
  Eigen::Vector3f *ptmp;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  Eigen::Vector3f tmp;

  cloud->clear();
  pcl::PointXYZRGB point;

<<<<<<< HEAD
  for (it = other_points.begin(); it != other_points.end(); it++)
=======
  for (it = other_points.begin();
      it != other_points.end(); it++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  {
    ptmp = *it;
    tmp = *ptmp;
    point.x = tmp[0] / 1000.0;
    point.y = tmp[1] / 1000.0;
    point.z = tmp[2] / 1000.0;
    point.r = 255;
    point.g = tmp[2] / 5;
    point.b = 255;
    if (point.z < 10.0)
    {
      cloud->push_back(point);
    }
  }
<<<<<<< HEAD
  for (it = points_on_plane.begin(); it != points_on_plane.end(); it++)
=======
  for (it = points_on_plane.begin();
      it != points_on_plane.end(); it++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  {
    ptmp = *it;
    tmp = *ptmp;
    point.x = tmp[0] / 1000.0;
    point.y = tmp[1] / 1000.0;
    point.z = tmp[2] / 1000.0;
    point.r = 255;
    point.g = 255;
    point.b = tmp[2] / 5;
    cloud->push_back(point);
  }
#ifdef _DEBUG
<<<<<<< HEAD
  for (it = picked_points_around.begin(); it != picked_points_around.end(); it++)
  {  // Draw around of picked points
=======
  for (it = picked_points_around.begin();
      it != picked_points_around.end(); it++)
  { // Draw around of picked points
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    ptmp = *it;
    tmp = *ptmp;
    point.x = tmp[0] / 1000.0;
    point.y = tmp[1] / 1000.0;
    point.z = tmp[2] / 1000.0;
    point.r = tmp[2] / 5;
    point.g = 255;
    point.b = 255;
    cloud->push_back(point);
  }
<<<<<<< HEAD
#endif  // _DEBUG
  for (i = 0; i < CORRESPOND_POINTS; i++)
  {  // Draw picked points
=======
#endif // _DEBUG
  for (i=0; i<CORRESPOND_POINTS; i++)
  { // Draw picked points
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    point.x = picked_points[i][0] / 1000.0;
    point.y = picked_points[i][1] / 1000.0;
    point.z = picked_points[i][2] / 1000.0;
    point.r = picked_points[i][2] / 5;
    point.g = picked_points[i][2] / 5;
    point.b = 255;
    cloud->push_back(point);
  }
  viewer->showCloud(cloud);
  return 0;
}

inline double _E(double phi_x, double phi_y, double phi_z, double t_x, double t_y, double t_z,
<<<<<<< HEAD
                 const Eigen::Vector3f* X_w, const Eigen::Vector3f* X_c)
=======
    const Eigen::Vector3f* X_w,
    const Eigen::Vector3f* X_c)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  int i;
  double ret;
  Eigen::Matrix3d rot;
<<<<<<< HEAD
  rot = Eigen::AngleAxisd(phi_z, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());
  ret = 0.0;
  for (i = 0; i < CORRESPOND_POINTS; i++)
  {
    Eigen::Vector3d tmp;
    tmp = X_w[i].cast< double >() - (rot * (X_c[i].cast< double >()) + Eigen::Vector3d(t_x, t_y, t_z));
    ret += pow(tmp.norm(), 2.0);
=======
  rot = Eigen::AngleAxisd(phi_z, Eigen::Vector3d::UnitZ())
    * Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());
  ret = 0.0;
  for (i=0; i < CORRESPOND_POINTS; i++)
  {
    Eigen::Vector3d tmp;
    tmp = X_w[i].cast<double>()-(rot*(X_c[i].cast<double>())+Eigen::Vector3d(t_x,t_y,t_z));
    ret += pow(tmp.norm(),2.0);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  }
  return ret;
}

<<<<<<< HEAD
inline int find_corner(const cv::Mat& image, const cv::Size pattern_size, std::vector< cv::Point2f >& corners,
                       bool& pattern_found)
{
  cv::Mat gray(image.rows, image.cols, CV_8UC1);
  switch (image.type())
  {
    case CV_8UC1:
      gray = image;
      break;
    case CV_8UC3:
      cv::cvtColor(image, gray, CV_BGR2GRAY);
      break;
    default:
      std::cerr << "Unexpected image type." << std::endl;
      return -1;
  }
  pattern_found =
      cv::findChessboardCorners(gray, pattern_size, corners,
                                cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK);
  if (pattern_found)
  {
    cv::cornerSubPix(gray, corners, cv::Size(15, 15), cv::Size(-1, -1),
                     cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 15, 0.01));
=======
inline int find_corner(const cv::Mat& image, const cv::Size pattern_size,
    std::vector<cv::Point2f>& corners, bool& pattern_found)
{
  cv::Mat gray(image.rows, image.cols, CV_8UC1);
  switch(image.type())
  {
  case CV_8UC1:
    gray = image;
    break;
  case CV_8UC3:
    cv::cvtColor(image, gray, CV_BGR2GRAY);
    break;
  default:
    std::cerr << "Unexpected image type." << std::endl;
    return -1;
  }
  pattern_found = cv::findChessboardCorners(gray, pattern_size, corners,
      cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK);
  if (pattern_found)
  {
    cv::cornerSubPix(gray, corners, cv::Size(15,15), cv::Size(-1,-1),
        cv::TermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 15, 0.01));
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  }
  return 0;
}

pcl::PointXYZRGB _s, _ex, _ey, _ez;
void viewerDrawAxis(pcl::visualization::PCLVisualizer& viewer)
{
  viewer.removeShape("axis-x");
  viewer.removeShape("axis-y");
  viewer.removeShape("axis-z");
<<<<<<< HEAD
  viewer.addLine< pcl::PointXYZRGB >(_s, _ex, 255, 0, 0, "axis-x");
  viewer.addLine< pcl::PointXYZRGB >(_s, _ey, 0, 255, 0, "axis-y");
  viewer.addLine< pcl::PointXYZRGB >(_s, _ez, 0, 0, 255, "axis-z");
=======
  viewer.addLine<pcl::PointXYZRGB>(_s,_ex, 255, 0, 0, "axis-x");
  viewer.addLine<pcl::PointXYZRGB>(_s,_ey, 0, 255, 0, "axis-y");
  viewer.addLine<pcl::PointXYZRGB>(_s,_ez, 0, 0, 255, "axis-z");
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  return;
}

extern double d_roll, d_pitch, d_yaw;
extern double d_tx, d_ty, d_tz;
<<<<<<< HEAD
int MyCalibration::calcurateExtrinsicParameters(double convertion_th, int convertion_method, const float pattern_width)
=======
int MyCalibration::calcurateExtrinsicParameters(double convertion_th,
    int convertion_method,
    const float pattern_width)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
{
  int i, j, k;
  double t_x, t_y, t_z;
  double phi_x, phi_y, phi_z;
  Eigen::Vector3d avg_vec;
  Eigen::Matrix3d cov_mat;

  Eigen::Vector3d tmp;

  avg_vec = Eigen::Vector3d::Zero();
  cov_mat = Eigen::Matrix3d::Zero();

  Eigen::Vector3d ax[3];
  Eigen::Vector3d ax_cam[3];
  ax_cam[0] = Eigen::Vector3d::UnitX();
  ax_cam[1] = Eigen::Vector3d::UnitY();
  ax_cam[2] = Eigen::Vector3d::UnitZ();

<<<<<<< HEAD
  {  // calcuration of average
    for (i = 0; i < 3; i++)
    {
      for (k = 0; k < points_on_plane.size(); k++)
      {
        tmp = points_on_plane[k]->cast< double >();
=======
  { // calcuration of average
    for (i=0; i < 3; i++)
    {
      for (k=0; k < points_on_plane.size(); k++)
      {
        tmp = points_on_plane[k]->cast<double>();
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        avg_vec[i] += tmp[i] / points_on_plane.size();
      }
    }

    // calcuration of covariance
<<<<<<< HEAD
    for (i = 0; i < 3; i++)
    {
      for (j = 0; j < 3; j++)
      {
        for (k = 0; k < points_on_plane.size(); k++)
        {
          tmp = points_on_plane[k]->cast< double >();
          cov_mat(i, j) += ((tmp[i] - avg_vec[i]) * (tmp[j] - avg_vec[j])) / points_on_plane.size();
=======
    for (i=0; i < 3; i++)
    {
      for (j=0; j < 3; j++)
      {
        for (k=0; k < points_on_plane.size(); k++)
        {
          tmp = points_on_plane[k]->cast<double>();
          cov_mat(i,j) += ((tmp[i] - avg_vec[i])
              * (tmp[j] - avg_vec[j])) / points_on_plane.size();
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        }
      }
    }
  }

<<<<<<< HEAD
  {  // Calcurate Z-axis
    Eigen::EigenSolver< Eigen::Matrix3d > solver(cov_mat);
    double eigen_min = std::numeric_limits< double >::infinity();
    for (i = 0; i < 3; i++)
=======
  { // Calcurate Z-axis
    Eigen::EigenSolver<Eigen::Matrix3d> solver(cov_mat);
    double eigen_min = std::numeric_limits<double>::infinity();
    for (i=0; i<3; i++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    {
      if (solver.eigenvalues()(i).real() < eigen_min)
      {
        j = i;
        eigen_min = solver.eigenvalues()(i).real();
      }
    }
<<<<<<< HEAD
    Eigen::Matrix3cd axis_world;
    axis_world = solver.eigenvectors();

    ax[2][0] = axis_world(0, j).real();
    ax[2][1] = axis_world(1, j).real();
    ax[2][2] = axis_world(2, j).real();
=======
    Eigen::Matrix3cd  axis_world;
    axis_world = solver.eigenvectors();

    ax[2][0] = axis_world(0,j).real();
    ax[2][1] = axis_world(1,j).real();
    ax[2][2] = axis_world(2,j).real();
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    if (ax[2].dot(ax_cam[2]) > 0)
    {
      ax[2] = -ax[2];
    }
<<<<<<< HEAD
    ax[2] = ax[2] / ax[2].norm();
  }

  // Estimate other 4 parameters
  {  // Set world points
    for (int x = 0; x < 5; x++)
    {
      for (int y = 0; y < 6; y++)
      {
        world_points[x * 6 + y] = Eigen::Vector3f(x * pattern_width, y * pattern_width, 0.0);
=======
    ax[2] = ax[2]/ax[2].norm();
  }

  // Estimate other 4 parameters
  { // Set world points
    for (int x=0; x < 5; x++)
    {
      for (int y=0; y < 6; y++)
      {
        world_points[x*6+y] = Eigen::Vector3f(x*pattern_width,y*pattern_width,0.0);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      }
    }
  }

<<<<<<< HEAD
  // For Debug ===
  if (g_DebugFlg)
  {
    Eigen::Matrix3d _debug_mat = Eigen::Matrix3d::Zero();
    _debug_mat = Eigen::AngleAxisd(d_yaw / 180 * M_PI, Eigen::Vector3d::UnitZ()) *
                 Eigen::AngleAxisd(d_pitch / 180 * M_PI, Eigen::Vector3d::UnitY()) *
                 Eigen::AngleAxisd(d_roll / 180 * M_PI, Eigen::Vector3d::UnitX());

    Eigen::Vector3d _debug_vec = Eigen::Vector3d(d_tx, d_ty, d_tz);

    for (i = 0; i < CORRESPOND_POINTS; i++)
    {
      picked_points[i] = _debug_mat.cast< float >().inverse() * (world_points[i] - _debug_vec.cast< float >());
      std::cout << "i: " << i << "\t" << picked_points[i][0] << ", " << picked_points[i][1] << ", "
                << picked_points[i][2] << std::endl;
    }
    ax[2] = _debug_mat.inverse() * Eigen::Vector3d::UnitZ();
  }
  // === For Debug

  double n;
  phi_x = atan2(ax[2](1), ax[2](2));
  phi_y = atan2(-ax[2](0), sqrt(pow(ax[2][1], 2.0) + pow(ax[2][2], 2.0)));
  Eigen::Matrix3d rot_rp;
  rot_rp = Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());
  Eigen::Vector3d rotatedRP_point;

  t_z = 0.0;
  for (i = 0; i < CORRESPOND_POINTS; i++)
  {
    rotatedRP_point = rot_rp * picked_points[i].cast< double >();
=======
// For Debug ===
  if (g_DebugFlg)
  {
    Eigen::Matrix3d _debug_mat = Eigen::Matrix3d::Zero();
    _debug_mat = Eigen::AngleAxisd(d_yaw / 180 * M_PI, Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(d_pitch / 180 * M_PI, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(d_roll / 180 * M_PI, Eigen::Vector3d::UnitX());

    Eigen::Vector3d _debug_vec = Eigen::Vector3d(d_tx,d_ty,d_tz);

    for (i=0; i < CORRESPOND_POINTS; i++)
    {
      picked_points[i] = _debug_mat.cast<float>().inverse()
        * (world_points[i]-_debug_vec.cast<float>());
      std::cout << "i: " << i << "\t" <<
        picked_points[i][0] << ", " <<
        picked_points[i][1] << ", " <<
        picked_points[i][2] << std::endl;
    }
    ax[2] = _debug_mat.inverse() * Eigen::Vector3d::UnitZ();
  }
// === For Debug

  double n;
  phi_x = atan2(ax[2](1), ax[2](2));
  phi_y = atan2(-ax[2](0), sqrt(pow(ax[2][1],2.0)+pow(ax[2][2],2.0)));
  Eigen::Matrix3d rot_rp;
  rot_rp = Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY())
    * Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());
  Eigen::Vector3d rotatedRP_point;

  t_z = 0.0;
  for (i=0; i<CORRESPOND_POINTS; i++)
  {
    rotatedRP_point = rot_rp*picked_points[i].cast<double>();
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    // t_z
    t_z += (world_points[i][2] - rotatedRP_point[2]) / CORRESPOND_POINTS;
  }

  /* Convertion calcuration */
  phi_z = 0.0;
<<<<<<< HEAD
  t_x = 0.0;
  t_y = 0.0;
=======
  t_x   = 0.0;
  t_y   = 0.0;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  Eigen::Vector3d grad_E = Eigen::Vector3d::Ones();

  unsigned long long int cnt;
  // Steepest descent method
  if (convertion_method == 1)
  {
    const double step = 0.00000002;
    const double grad_th = convertion_th;
    cnt = 0;
    while (grad_E.norm() > grad_th)
    {
      Eigen::Matrix3d rot;
<<<<<<< HEAD
      rot = Eigen::AngleAxisd(phi_z, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());

      grad_E = Eigen::Vector3d::Zero();
      for (i = 0; i < CORRESPOND_POINTS; i++)
      {
        Eigen::Vector3d rotated_point = rot * (picked_points[i].cast< double >());
        Eigen::Vector3d world_point = world_points[i].cast< double >();
        grad_E[0] += 2.0 * ((world_point[0] - t_x) * rotated_point[1] - (world_point[1] - t_y) * rotated_point[0]);
=======
      rot = Eigen::AngleAxisd(phi_z, Eigen::Vector3d::UnitZ())
        * Eigen::AngleAxisd(phi_y, Eigen::Vector3d::UnitY())
        * Eigen::AngleAxisd(phi_x, Eigen::Vector3d::UnitX());

      grad_E = Eigen::Vector3d::Zero();
      for (i=0; i<CORRESPOND_POINTS; i++)
      {
        Eigen::Vector3d rotated_point = rot * (picked_points[i].cast<double>());
        Eigen::Vector3d world_point = world_points[i].cast<double>();
        grad_E[0] += 2.0 * ((world_point[0]-t_x)*rotated_point[1]
            - (world_point[1]-t_y)*rotated_point[0]);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        grad_E[1] -= 2.0 * (world_point[0] - (rotated_point[0] + t_x));
        grad_E[2] -= 2.0 * (world_point[1] - (rotated_point[1] + t_y));
      }
      phi_z -= step * grad_E[0];
<<<<<<< HEAD
      t_x -= step * grad_E[1];
      t_y -= step * grad_E[2];
      if (phi_z > M_PI)
      {
        phi_z -= 2.0 * M_PI;
      }
      if (phi_z <= -M_PI)
      {
        phi_z += 2.0 * M_PI;
=======
      t_x   -= step * grad_E[1];
      t_y   -= step * grad_E[2];
      if (phi_z > M_PI)
      {
        phi_z -= 2.0*M_PI;
      }
      if (phi_z <= -M_PI)
      {
        phi_z += 2.0*M_PI;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      }
      cnt++;

      std::cout << "k: " << cnt << "---" << std::endl
<<<<<<< HEAD
                << "phi_z: " << phi_z << "\t"
                << "t_x: " << t_x << "\t"
                << "t_y: " << t_y << std::endl
                << "E: " << _E(phi_x, phi_y, phi_z, t_x, t_y, t_z, world_points, picked_points) << "\t"
                << "grad_E: " << grad_E[0] << "\t" << grad_E[1] << "\t" << grad_E[2] << std::endl
                << "|grad_E|: " << grad_E.norm() << std::endl
                << std::endl;
=======
        << "phi_z: " << phi_z << "\t"
        << "t_x: " << t_x << "\t"
        << "t_y: " << t_y << std::endl
        << "E: " << _E(phi_x, phi_y, phi_z, t_x, t_y, t_z, world_points, picked_points) << "\t"
        << "grad_E: " << grad_E[0] << "\t" << grad_E[1] << "\t" << grad_E[2] << std::endl
        << "|grad_E|: " << grad_E.norm() << std::endl
        << std::endl;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    }
  }
  // Perturbation method
  else if (convertion_method == 2)
  {
<<<<<<< HEAD
    double E = _E(phi_x, phi_y, phi_z, t_x, t_y, t_z, world_points, picked_points);
=======
    double E = _E(phi_x,phi_y,phi_z,t_x,t_y,t_z,world_points,picked_points);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    const double _th = convertion_th;
    const double alpha = 0.001;
    const double beta = 0.01;
    const double gamma = 0.01;
    double E_prev;
    cnt = 0;
    do
    {
      E_prev = E;

      double min;
<<<<<<< HEAD
      min = std::numeric_limits< double >::infinity();
      for (i = 0; i < 6; i++)
      {
        switch (i)
        {
          case 0:
            E = _E(phi_x, phi_y, phi_z + alpha, t_x, t_y, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
          case 1:
            E = _E(phi_x, phi_y, phi_z - alpha, t_x, t_y, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
          case 2:
            E = _E(phi_x, phi_y, phi_z, t_x + beta, t_y, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
          case 3:
            E = _E(phi_x, phi_y, phi_z, t_x - beta, t_y, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
          case 4:
            E = _E(phi_x, phi_y, phi_z, t_x, t_y + gamma, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
          case 5:
            E = _E(phi_x, phi_y, phi_z, t_x, t_y - gamma, t_z, world_points, picked_points);
            if (min > E)
            {
              min = E;
              j = i;
            }
            break;
        }
        E = min;
      }
      switch (j)
      {
        case 0:
          phi_z += alpha;
          if (phi_z > M_PI)
          {
            phi_z -= 2.0 * M_PI;
          }
          break;
        case 1:
          phi_z -= alpha;
          if (phi_z < -M_PI)
          {
            phi_z += 2.0 * M_PI;
          }
          break;
        case 2:
          t_x += beta;
          break;
        case 3:
          t_x -= beta;
          break;
        case 4:
          t_y += gamma;
          break;
        case 5:
          t_y -= gamma;
          break;
=======
      min = std::numeric_limits<double>::infinity();
      for (i=0; i<6; i++)
      {
        switch(i)
        {
        case 0:
          E = _E(phi_x,phi_y,phi_z+alpha,t_x,t_y,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        case 1:
          E = _E(phi_x,phi_y,phi_z-alpha,t_x,t_y,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        case 2:
          E = _E(phi_x,phi_y,phi_z,t_x+beta,t_y,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        case 3:
          E = _E(phi_x,phi_y,phi_z,t_x-beta,t_y,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        case 4:
          E = _E(phi_x,phi_y,phi_z,t_x,t_y+gamma,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        case 5:
          E = _E(phi_x,phi_y,phi_z,t_x,t_y-gamma,t_z,world_points,picked_points);
          if (min > E)
          {
            min = E;
            j = i;
          }
          break;
        }
        E = min;
      }
      switch(j)
      {
      case 0:
        phi_z += alpha;
        if (phi_z > M_PI)
        {
          phi_z -= 2.0 * M_PI;
        }
        break;
      case 1:
        phi_z -= alpha;
        if (phi_z < -M_PI)
        {
          phi_z += 2.0 * M_PI;
        }
        break;
      case 2:
        t_x += beta;
        break;
      case 3:
        t_x -= beta;
        break;
      case 4:
        t_y += gamma;
        break;
      case 5:
        t_y -= gamma;
        break;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
      }
      cnt++;
      if (cnt % 1000 == 0)
      {
        std::cout << "delta_E " << cnt << "-> " << E_prev - E << std::endl;
      }
    } while (E_prev - E > _th);
  }
  else
  {
    std::cerr << "Error::Requrered unexpected method." << std::endl;
    return 1;
  }

  std::cout << "Tried " << cnt << " times for convertion calcuration of extrinsic parameters" << std::endl;
<<<<<<< HEAD
  std::cout << "rpy[deg]: " << phi_x * 180.0 / M_PI << ", " << phi_y * 180.0 / M_PI << ", " << phi_z * 180.0 / M_PI
            << std::endl;

  std::cout << "t[mm]: " << t_x << ", " << t_y << ", " << t_z << std::endl;

  {  // Draw axis on cloud viewer for certification
    Eigen::Matrix3d rotation = Eigen::Matrix3d::Zero();
    rotation =
        Eigen::AngleAxisd(phi_z, ax_cam[2]) * Eigen::AngleAxisd(phi_y, ax_cam[1]) * Eigen::AngleAxisd(phi_x, ax_cam[0]);
    Eigen::Vector3d translation = Eigen::Vector3d(t_x, t_y, t_z);
    tmp = Eigen::Vector3d::Zero();
    // tmp = rotation * tmp + translation;
    tmp = rotation.inverse() * (tmp - translation);
=======
  std::cout << "rpy[deg]: " <<
    phi_x*180.0/M_PI << ", " <<
    phi_y*180.0/M_PI << ", " <<
    phi_z*180.0/M_PI << std::endl;

  std::cout << "t[mm]: " <<
    t_x << ", " <<
    t_y << ", " <<
    t_z << std::endl;

  { // Draw axis on cloud viewer for certification
    Eigen::Matrix3d rotation = Eigen::Matrix3d::Zero();
    rotation = Eigen::AngleAxisd(phi_z, ax_cam[2])
      * Eigen::AngleAxisd(phi_y, ax_cam[1])
      * Eigen::AngleAxisd(phi_x, ax_cam[0]);
    Eigen::Vector3d translation = Eigen::Vector3d(t_x,t_y,t_z);
    tmp = Eigen::Vector3d::Zero();
    //tmp = rotation * tmp + translation;
    tmp = rotation.inverse() * ( tmp - translation );
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    _s.x = (tmp[0]) / 1000.0;
    _s.y = (tmp[1]) / 1000.0;
    _s.z = (tmp[2]) / 1000.0;
    _s.r = 0;
    _s.g = 0;
    _s.b = 0;
    // axis 1
<<<<<<< HEAD
    tmp = Eigen::Vector3d::UnitX() * 50;
=======
    tmp = Eigen::Vector3d::UnitX()*50;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    tmp = rotation.inverse() * (tmp - translation);
    _ex.x = (tmp[0]) / 1000.0;
    _ex.y = (tmp[1]) / 1000.0;
    _ex.z = (tmp[2]) / 1000.0;
    _ex.r = 255;
    _ex.g = 0;
    _ex.b = 0;
    // axis 2
<<<<<<< HEAD
    tmp = Eigen::Vector3d::UnitY() * 50;
=======
    tmp = Eigen::Vector3d::UnitY()*50;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    tmp = rotation.inverse() * (tmp - translation);
    _ey.x = (tmp[0]) / 1000.0;
    _ey.y = (tmp[1]) / 1000.0;
    _ey.z = (tmp[2]) / 1000.0;
    _ey.r = 0;
    _ey.g = 255;
    _ey.b = 0;
    // axis 3
<<<<<<< HEAD
    tmp = Eigen::Vector3d::UnitZ() * 50;
=======
    tmp = Eigen::Vector3d::UnitZ()*50;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    tmp = rotation.inverse() * (tmp - translation);
    _ez.x = (tmp[0]) / 1000.0;
    _ez.y = (tmp[1]) / 1000.0;
    _ez.z = (tmp[2]) / 1000.0;
    _ez.r = 0;
    _ez.g = 0;
    _ez.b = 255;
    viewer->runOnVisualizationThread(viewerDrawAxis);
    viewer->showCloud(cloud);
  }

  return 0;
}

int MyCalibration::startPickingPoints()
{
  g_picking_counter = 0;
  return 0;
}

int MyCalibration::pickPointsAutomatically(int pattern_rows, int pattern_cols)
{
<<<<<<< HEAD
  int i, j;
  int key;
  cv::Mat image_chess(CAMERA_RESOLUTION_Y, CAMERA_RESOLUTION_X, CV_8UC3);

  // openni::VideoFrameRef frame;
  bool pattern_found;
  std::vector< cv::Point2f > corners;
  cv::namedWindow("Detecting points");
  for (i = 0; i < image.rows; i++)
  {
    for (j = 0; j < image.cols; j++)
    {
      image_chess.at< cv::Vec3b >(i, j) = cv::Vec3b(255, 255, 255);
=======
  int i,j;
  int key;
  cv::Mat image_chess(CAMERA_RESOLUTION_Y, CAMERA_RESOLUTION_X, CV_8UC3);

  //openni::VideoFrameRef frame;
  bool pattern_found;
  std::vector<cv::Point2f> corners;
  cv::namedWindow("Detecting points");
  for (i=0; i<image.rows; i++)
  {
    for (j=0; j<image.cols; j++)
    {
      image_chess.at<cv::Vec3b>(i,j) = cv::Vec3b(255,255,255);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    }
  }

  if (color_frame != NULL)
<<<<<<< HEAD
  {  // Detect corners with color image.
    image_chess = cv::Mat(CAMERA_RESOLUTION_Y, CAMERA_RESOLUTION_X, CV_8UC3, color_frame);
    cv::cvtColor(image_chess, image_chess, CV_BGR2RGB);
  }
  else
  {                      // Detect corners with depth image.
    if (key == 1048681)  // key == 'i' ?
    {                    // Clean image
      for (i = 0; i < CAMERA_RESOLUTION_Y; i++)
      {
        for (j = 0; j < CAMERA_RESOLUTION_X; j++)
        {
          image.at< cv::Vec3b >(i, j) = cv::Vec3b(255, 255, 255);
=======
  { // Detect corners with color image.
    image_chess = cv::Mat(CAMERA_RESOLUTION_Y,CAMERA_RESOLUTION_X,CV_8UC3,color_frame);
    cv::cvtColor(image_chess,image_chess,CV_BGR2RGB);
  }
  else
  { // Detect corners with depth image.
    if (key == 1048681) // key == 'i' ?
    { // Clean image
      for (i=0; i<CAMERA_RESOLUTION_Y; i++)
      {
        for (j=0; j<CAMERA_RESOLUTION_X; j++)
        {
          image.at<cv::Vec3b>(i,j) = cv::Vec3b(255,255,255);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        }
      }
    }
    // Fill with brack at invalid pixel
<<<<<<< HEAD
    float* dp;
    // dp = (openni::DepthPixel*)depth_frame;
    for (i = 0; i < image.rows; i++)
    {
      for (j = 0; j < image.cols; j++)
      {
        int now = (i * CAMERA_RESOLUTION_X + j);
        if (*(float*)(dp + now) == INVALID_PIXEL_VALUE)
        {
          image.at< cv::Vec3b >(i, j) = cv::Vec3b(0, 0, 0);
=======
    float *dp;
    //dp = (openni::DepthPixel*)depth_frame;
    for (i=0; i<image.rows; i++)
    {
      for (j=0; j<image.cols; j++)
      {
        int now = (i*CAMERA_RESOLUTION_X+j);
        if (*(float*)(dp+now) == INVALID_PIXEL_VALUE)
        {
          image.at<cv::Vec3b>(i,j) = cv::Vec3b(0,0,0);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
        }
      }
    }
  }
<<<<<<< HEAD
  find_corner(image_chess, cv::Size(pattern_cols, pattern_rows), corners, pattern_found);
  cv::drawChessboardCorners(image_chess, cv::Size(pattern_cols, pattern_rows), (cv::Mat)corners, pattern_found);
  cv::imshow("Detecting points", image_chess);
  if (!pattern_found)
=======
  find_corner(image_chess, cv::Size(pattern_cols,pattern_rows), corners, pattern_found);
  cv::drawChessboardCorners( image_chess, cv::Size(pattern_cols,pattern_rows),
      (cv::Mat)corners, pattern_found);
  cv::imshow("Detecting points", image_chess);
  if(!pattern_found)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  {
    int key = cv::waitKey(10);
    return key;
  }

  std::cout << "Detected corners" << std::endl;
  key = cv::waitKey(0);
  cv::destroyWindow("Detecting points");

<<<<<<< HEAD
  g_points[0][0] = (int)corners[0].x;
  g_points[0][1] = (int)corners[0].y;
  g_points[1][0] = (int)corners[(pattern_rows - 1) * pattern_cols].x;
  g_points[1][1] = (int)corners[(pattern_rows - 1) * pattern_cols].y;
  g_points[2][0] = (int)corners[pattern_cols - 1].x;
  g_points[2][1] = (int)corners[pattern_cols - 1].y;
  g_points[3][0] = (int)corners[pattern_rows * pattern_cols - 1].x;
  g_points[3][1] = (int)corners[pattern_rows * pattern_cols - 1].y;

  for (i = 0; i < pattern_rows; i++)
  {
    for (j = 0; j < pattern_cols; j++)
    {
      g_picked_points[i * pattern_cols + j][0] = (int)corners[i * pattern_cols + j].x;
      g_picked_points[i * pattern_cols + j][1] = (int)corners[i * pattern_cols + j].y;
=======
  g_points[0][0] = (int)corners[ 0].x;
  g_points[0][1] = (int)corners[ 0].y;
  g_points[1][0] = (int)corners[(pattern_rows-1)*pattern_cols].x;
  g_points[1][1] = (int)corners[(pattern_rows-1)*pattern_cols].y;
  g_points[2][0] = (int)corners[pattern_cols-1].x;
  g_points[2][1] = (int)corners[pattern_cols-1].y;
  g_points[3][0] = (int)corners[pattern_rows*pattern_cols-1].x;
  g_points[3][1] = (int)corners[pattern_rows*pattern_cols-1].y;

  for(i=0; i<pattern_rows; i++)
  {
    for(j=0; j<pattern_cols; j++)
    {
      g_picked_points[i*pattern_cols+j][0] = (int)corners[i*pattern_cols+j].x;
      g_picked_points[i*pattern_cols+j][1] = (int)corners[i*pattern_cols+j].y;
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
    }
  }

  return 0;
}

void MyCalibration::getDepthFrameCallback(const sensor_msgs::Image::ConstPtr& frame)
{
  int i, j;
<<<<<<< HEAD
  uint8_t* p;
  for (i = 0, p = depth_frame; i < frame->height * frame->step; i++, p++)
=======
  uint8_t *p;
  for (i=0, p = depth_frame; i < frame->height*frame->step; i++, p++)
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  {
    *p = frame->data[i];
  }
  cv::Mat image_tmp = cv::Mat(frame->height, frame->width, CV_16UC1, depth_frame);
<<<<<<< HEAD
  image_tmp.convertTo(image, CV_8UC1, 255.0 / 10000.0);
  // cv::imshow("Depth image", image);
=======
  image_tmp.convertTo(image, CV_8UC1, 255.0/10000.0);
  //cv::imshow("Depth image", image);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  return;
}

void MyCalibration::getColorFrameCallback(const sensor_msgs::Image::ConstPtr& frame)
{
  int i;
<<<<<<< HEAD
  uint8_t* p;
  for (i = 0, p = color_frame; i < frame->height * frame->step; i++, p++)
  {
    *p = frame->data[i];
  }
  // cv::Mat image_tmp = cv::Mat(frame->height, frame->width, CV_8UC3, color_frame);
  // cv::cvtColor(image_tmp,image_tmp,CV_BGR2RGB);
  // cv::imshow("Color image", image_tmp);
  // cv::waitKey(10);
=======
  uint8_t *p;
  for (i=0, p = color_frame; i < frame->height*frame->step; i++, p++)
  {
    *p = frame->data[i];
  }
  //cv::Mat image_tmp = cv::Mat(frame->height, frame->width, CV_8UC3, color_frame);
  //cv::cvtColor(image_tmp,image_tmp,CV_BGR2RGB);
  //cv::imshow("Color image", image_tmp);
  //cv::waitKey(10);
>>>>>>> 51ecc3540900cfe208d8c2ca1ecaf2184d407ca7
  return;
}

void MyCalibration::getPointsCallback(const sensor_msgs::PointCloud2::ConstPtr& points)
{
  this->points = points;
  return;
}


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

#include "xregRayCastSurRenderOCL.h"

#include <boost/compute/utility/source.hpp>
#include <boost/compute/types/struct.hpp>

#include "xregAssert.h"

//////////////////////////////////////////////////////////////////////

BOOST_COMPUTE_ADAPT_STRUCT(xreg::RayCasterSurRenderOCL::RayCastSurRenderArgs, RayCastSurRenderArgs,
                           (thresh,
                            pad1,
                            num_backtracking_steps,
                            ambient_reflection_ratio,
                            diffuse_reflection_ratio,
                            specular_reflection_ratio,
                            alpha_shininess))

//////////////////////////////////////////////////////////////////////

namespace  // un-named
{

const char* kRAY_CASTING_SUR_RENDER_OPENCL_SRC = BOOST_COMPUTE_STRINGIZE_SOURCE(

__kernel void xregSurRenderKernel1(const RayCastArgs args,
                                   const RayCastSurRenderArgs sur_render_args,
                                   __global const float4* det_pts,
                                   image3d_t vol_tex,
                                   __global const float16* cam_to_itk_phys_xforms,
                                   __global float8* dst_step_vecs_and_intersect_pts_wrt_itk_idx,
                                   __global const ulong* cam_model_for_proj,
                                   __global const float4* cam_focal_pts)
{
  const ulong idx = get_global_id(0);

  const ulong num_rays = args.num_projs * args.num_det_pts;

  if (idx < num_rays)
  {
    const ulong proj_idx   = idx / args.num_det_pts;
    const ulong det_pt_idx = idx - (proj_idx * args.num_det_pts);
    const ulong cam_idx    = cam_model_for_proj[proj_idx];

    const float4 focal_pt_wrt_cam = cam_focal_pts[cam_idx];

    const sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_LINEAR;

    const float4 tex_coords_off = (float4) (0.5f, 0.5f, 0.5f, 0);

    const float16 xform_cam_to_itk_idx = xregFrm4x4Composition(args.itk_phys_pt_to_itk_idx_xform,
                                                               cam_to_itk_phys_xforms[proj_idx]);

    const float4 pinhole_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, focal_pt_wrt_cam);

    const float4 cur_det_pt_wrt_cam = det_pts[(cam_idx * args.num_det_pts) + det_pt_idx];

    const float4 pinhole_to_det_wrt_itk_idx = xregFrm4x4XformFloat4Pt(xform_cam_to_itk_idx, cur_det_pt_wrt_cam)
                                                  - pinhole_wrt_itk_idx;

    // check intersection with the image/volume boundary
    // these pointer were passed as float4's since boost::compute does not have a float3
    float2 t = xregRayRectIntersect(xregFloat4HmgToFloat3(args.img_aabb_min),
                                    xregFloat4HmgToFloat3(args.img_aabb_max),
                                    xregFloat4HmgToFloat3(pinhole_wrt_itk_idx),
                                    xregFloat4HmgToFloat3(pinhole_to_det_wrt_itk_idx));
    // t.x indicates the first intersection with the volume along the source to detector ray,
    // t.y indicates the exit of the volume along the source to detector ray
    // t.x, t.y are in [0,inf)

    const float4 start_pt_wrt_itk_idx = pinhole_wrt_itk_idx + (t.x * pinhole_to_det_wrt_itk_idx);

    const float pinhole_to_det_len_wrt_itk_idx = xregFloat4HmgNorm(pinhole_to_det_wrt_itk_idx);
    const float intersect_len_wrt_itk_idx = (t.y - t.x) * pinhole_to_det_len_wrt_itk_idx;

    const float4 focal_pt_to_det_wrt_cam = cur_det_pt_wrt_cam - focal_pt_wrt_cam;
    // NOTE: since we are transforming a vector with a scaling transform (points to indices),
    //       the output vector may not have the same norm, thus we take the norm
    const float step_len_wrt_itk_idx = xregFloat4HmgNorm(
                      xregFrm4x4XformFloat4Vec(xform_cam_to_itk_idx,
                         (focal_pt_to_det_wrt_cam / xregFloat4HmgNorm(focal_pt_to_det_wrt_cam)) * args.step_size));

    const ulong num_steps = (ulong)(intersect_len_wrt_itk_idx / step_len_wrt_itk_idx);

    float4 cur_cont_vol_idx = (float4) (start_pt_wrt_itk_idx.x, start_pt_wrt_itk_idx.y, start_pt_wrt_itk_idx.z, 0);

    // "/ pinhole_to_det_len_wrt_itk_idx" makes pinhole_to_det_wrt_itk_idx a unit vector
    const float scale_to_step = step_len_wrt_itk_idx / pinhole_to_det_len_wrt_itk_idx;

    const float4 step_vec_wrt_itk_idx = (float4) (pinhole_to_det_wrt_itk_idx.x * scale_to_step, pinhole_to_det_wrt_itk_idx.y * scale_to_step, pinhole_to_det_wrt_itk_idx.z * scale_to_step, 0);

    // The first three elements will store the step direction in ITK index space
    dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s0 = step_vec_wrt_itk_idx.x;
    dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s1 = step_vec_wrt_itk_idx.y;
    dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s2 = step_vec_wrt_itk_idx.z;

    // The fourth element will indicate whether this ray intersected the surface.
    dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s3 = 0;  //  0 -> no intersection found

    for (ulong step_idx = 0; step_idx <= num_steps; ++step_idx, cur_cont_vol_idx += step_vec_wrt_itk_idx)
    {
      if (read_imagef(vol_tex, sampler, cur_cont_vol_idx + tex_coords_off).x >= sur_render_args.thresh)
      {
        // The 5th, 6th, and 7th elements will store the initial point of intersection, before any backtracking
        dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s4 = cur_cont_vol_idx.x;
        dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s5 = cur_cont_vol_idx.y;
        dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s6 = cur_cont_vol_idx.z;
        dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s7 = 0;

        dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s3 = 1;  // 1 -> intersection found
        break;
      }
    }
  }
}

__kernel void xregSurRenderKernel2(const RayCastArgs args,
                                   const RayCastSurRenderArgs sur_render_args,
                                   image3d_t vol_tex,
                                   __global const float8* dst_step_vecs_and_intersect_pts_wrt_itk_idx,
                                   __global float* dst_intensities)
{
  const ulong idx = get_global_id(0);

  const ulong num_rays = args.num_projs * args.num_det_pts;

  if (idx < num_rays)
  {
    if (dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s3 > 1.0e-6)
    {
      const sampler_t sampler = CLK_NORMALIZED_COORDS_FALSE | CLK_ADDRESS_CLAMP | CLK_FILTER_LINEAR;

      const float4 tex_coords_off = (float4) (0.5f, 0.5f, 0.5f, 0);

      float3 light_src_vec = (float3) (dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s0, dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s1, dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s2);
      // right now, this light source vector points from the light source, it needs to be negated, and it is when normalizing a few lines below

      // This vector will always point towards the pinhole.
      float4 step_vec_wrt_itk_idx = (float4) (light_src_vec.x, light_src_vec.y, light_src_vec.z, 0);
      step_vec_wrt_itk_idx *= 0.5f;

      // normalize the direction vector to the light source, and negate to get the right orientation
      light_src_vec /= -xregFloat3Norm(light_src_vec);

      float4 cur_cont_vol_idx = (float4) (dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s4, dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s5, dst_step_vecs_and_intersect_pts_wrt_itk_idx[idx].s6, 0);

      cur_cont_vol_idx += tex_coords_off;

      // we know that we should start out backtracking.
      cur_cont_vol_idx -= step_vec_wrt_itk_idx;

      for (ulong backtrack_idx = 0; backtrack_idx < sur_render_args.num_backtracking_steps; ++backtrack_idx)
      {
        step_vec_wrt_itk_idx *= 0.5f;

        cur_cont_vol_idx += (read_imagef(vol_tex, sampler, cur_cont_vol_idx).x >= sur_render_args.thresh) ?
                                  step_vec_wrt_itk_idx : -step_vec_wrt_itk_idx;
      }

      // compute lighting/shading according to the phong model

      // approximate the derivative at this point with finite differencing adjacent indices
      // NOTE: these are wrt image (index) axes.

      float3 tmp_vec;

      // Gradient in X Direction:
      tmp_vec.x = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x + 1, cur_cont_vol_idx.y, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x -
                  read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x - 1, cur_cont_vol_idx.y, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x;

      // Gradient in Y Direction:
      tmp_vec.y = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y + 1, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x -
                  read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y - 1, cur_cont_vol_idx.z, cur_cont_vol_idx.w)).x;

      // Gradient in Z Direction:
      tmp_vec.z = read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y, cur_cont_vol_idx.z + 1, cur_cont_vol_idx.w)).x -
                  read_imagef(vol_tex, sampler, (float4) (cur_cont_vol_idx.x, cur_cont_vol_idx.y, cur_cont_vol_idx.z - 1, cur_cont_vol_idx.w)).x;

      tmp_vec /= (-0.5f * xregFloat3Norm(tmp_vec));

      float val = sur_render_args.ambient_reflection_ratio;

      // Treating the pinhole as the light source

      float tmp_dot = xregFloat3Inner(light_src_vec, tmp_vec);
      const int diffuse_valid = tmp_dot > 1.0e-6;

      val += diffuse_valid ? (sur_render_args.diffuse_reflection_ratio * tmp_dot) : 0;

      tmp_vec *= 2 * tmp_dot;
      tmp_vec -= light_src_vec;

      tmp_dot = xregFloat3Inner(light_src_vec, tmp_vec);

      val += (tmp_dot > 1.0e-6) ?
              (sur_render_args.specular_reflection_ratio * pow(tmp_dot, sur_render_args.alpha_shininess)) : 0;

      dst_intensities[idx] += val;
    }
  }
}

);

//////////////////////////////////////////////////////////////////////

}  // un-named

xreg::RayCasterSurRenderOCL::RayCasterSurRenderOCL()
  : RayCasterOCL(), step_vecs_and_intersect_pts_wrt_itk_idx_dev_(ctx_)
{ }

xreg::RayCasterSurRenderOCL::RayCasterSurRenderOCL(const boost::compute::device& dev)
  : RayCasterOCL(dev), step_vecs_and_intersect_pts_wrt_itk_idx_dev_(ctx_)
{ }

xreg::RayCasterSurRenderOCL::RayCasterSurRenderOCL(const boost::compute::context& ctx,
                                                   const boost::compute::command_queue& queue)
  : RayCasterOCL(ctx, queue), step_vecs_and_intersect_pts_wrt_itk_idx_dev_(ctx_)
{ }

void xreg::RayCasterSurRenderOCL::allocate_resources()
{
  namespace bc = boost::compute;

  RayCasterOCL::allocate_resources();

  // Build the surface rendering ray casting program
  std::stringstream ss;
  ss << RayCastBaseOCLStr()
     << boost::compute::type_definition<RayCasterSurRenderOCL::RayCastSurRenderArgs>()
     << kRAY_CASTING_SUR_RENDER_OPENCL_SRC;

  bc::program prog = bc::program::create_with_source(ss.str(), ctx_);
  prog.build();

  dev_kernel1_ = prog.create_kernel("xregSurRenderKernel1");

  dev_kernel2_ = prog.create_kernel("xregSurRenderKernel2");

  step_vecs_and_intersect_pts_wrt_itk_idx_dev_.resize(this->camera_models_[0].num_det_rows *
                                                      this->camera_models_[0].num_det_cols *
                                                      this->num_projs_,
                                                      cmd_queue_);
}

void xreg::RayCasterSurRenderOCL::compute(const size_type vol_idx)
{
  xregASSERT(this->resources_allocated_);

  compute_helper_pre_kernels(vol_idx);

  RayCastSurRenderArgs sur_args;

  sur_args.thresh = render_thresh();

  sur_args.num_backtracking_steps = num_backtracking_steps();

  sur_args.ambient_reflection_ratio =
                  surface_render_params().ambient_reflection_ratio;

  sur_args.diffuse_reflection_ratio =
                  surface_render_params().diffuse_reflection_ratio;

  sur_args.specular_reflection_ratio =
                  surface_render_params().specular_reflection_ratio;

  sur_args.alpha_shininess = surface_render_params().alpha_shininess;

  // setup kernel arguments and launch

  dev_kernel1_.set_arg(0, sizeof(ray_cast_kernel_args_), &ray_cast_kernel_args_);

  dev_kernel1_.set_arg(1, sizeof(sur_args), &sur_args);

  dev_kernel1_.set_arg(2, det_pts_dev_);

  dev_kernel1_.set_arg(3, vol_texs_dev_[vol_idx]);

  dev_kernel1_.set_arg(4, cam_to_itk_phys_xforms_dev_);

  dev_kernel1_.set_arg(5, step_vecs_and_intersect_pts_wrt_itk_idx_dev_);

  dev_kernel1_.set_arg(6, cam_model_for_proj_dev_);

  dev_kernel1_.set_arg(7, focal_pts_dev_);

  std::size_t global_work_size = ray_cast_kernel_args_.num_det_pts * this->num_projs_;
  //std::size_t local_work_size = 512;  passing null lets open CL pick a local size

  cmd_queue_.enqueue_nd_range_kernel(dev_kernel1_,
                                     1, // dim
                                     0, // null offset -> start at 0
                                     &global_work_size,
                                     0  // passing null lets open CL pick a local size
                                    ).wait();

  dev_kernel2_.set_arg(0, sizeof(ray_cast_kernel_args_), &ray_cast_kernel_args_);

  dev_kernel2_.set_arg(1, sizeof(sur_args), &sur_args);

  dev_kernel2_.set_arg(2, vol_texs_dev_[vol_idx]);

  dev_kernel2_.set_arg(3, step_vecs_and_intersect_pts_wrt_itk_idx_dev_);

  dev_kernel2_.set_arg(4, *proj_pixels_dev_to_use_);

  cmd_queue_.enqueue_nd_range_kernel(dev_kernel2_,
                                     1, // dim
                                     0, // null offset -> start at 0
                                     &global_work_size,
                                     0  // passing null lets open CL pick a local size
                                    ).wait();

  compute_helper_post_kernels(vol_idx);
}


// Copyright (C) 2006-2009 Dmitry Bufistov and Andrey Parfenov

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_CYCLE_RATIO_HOWARD_HPP
#define BOOST_GRAPH_CYCLE_RATIO_HOWARD_HPP

#include <vector>
#include <list>
#include <algorithm>
#include <limits>

#include <boost/bind.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/concept_check.hpp>
#include <boost/pending/queue.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>

/** @file howard_cycle_ratio.hpp
 * @brief The implementation of the maximum/minimum cycle ratio/mean algorithm.
 * @author Dmitry Bufistov
 * @author Andrey Parfenov
 */

namespace boost {

  /**
   * The mcr_float is like numeric_limits, but only for floating point types
   * and only defines infinity() and epsilon(). This class is primarily used
   * to encapsulate a less-precise epsilon than natively supported by the
   * floating point type.
   */
  template <typename Float = double> struct mcr_float {
    typedef Float value_type;

    static Float infinity()
    { return std::numeric_limits<value_type>::infinity(); }

    static Float epsilon()
    { return Float(-0.005); }
  };

  namespace detail {

    template <typename FloatTraits> struct
    min_comparator_props {
      typedef std::greater<typename FloatTraits::value_type> comparator;
      static const int multiplier = 1;
    };

    template <typename FloatTraits> struct
    max_comparator_props {
      typedef std::less<typename FloatTraits::value_type> comparator;
      static const int multiplier = -1;
    };

    template <typename FloatTraits, typename ComparatorProps>
    struct float_wrapper {
      typedef typename FloatTraits::value_type value_type;
      typedef ComparatorProps comparator_props_t;
      typedef typename ComparatorProps::comparator comparator;

      static value_type infinity()
      { return FloatTraits::infinity() * ComparatorProps::multiplier; }

      static value_type epsilon()
      { return FloatTraits::epsilon() * ComparatorProps::multiplier; }

    };

    /*! @class mcr_howard
     * @brief Calculates optimum (maximum/minimum) cycle ratio of a directed graph.
     * Uses  Howard's iteration policy algorithm. </br>(It is described in the paper
     * "Experimental Analysis of the Fastest Optimum Cycle Ratio and Mean Algorithm"
     * by Ali Dasdan).
     */
    template <typename FloatTraits,
              typename Graph, typename VertexIndexMap,
              typename EdgeWeight1, typename EdgeWeight2>
    class mcr_howard
    {
    public:
      typedef typename FloatTraits::value_type float_t;
      typedef typename FloatTraits::comparator_props_t cmp_props_t;
      typedef typename FloatTraits::comparator comparator_t;
      typedef enum{ my_white = 0, my_black } my_color_type;
      typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;
      typedef typename graph_traits<Graph>::edge_descriptor edge_t;
      typedef typename graph_traits<Graph>::vertices_size_type vn_t;
      typedef std::vector<float_t> vp_t;
      typedef typename boost::iterator_property_map<
        typename vp_t::iterator, VertexIndexMap
      > distance_map_t; //V -> float_t

      typedef typename std::vector<edge_t> ve_t;
      typedef std::vector<my_color_type> vcol_t;
      typedef typename ::boost::iterator_property_map<
        typename ve_t::iterator, VertexIndexMap
      > policy_t; //Vertex -> Edge
      typedef typename ::boost::iterator_property_map<
        typename vcol_t::iterator, VertexIndexMap
      > color_map_t;

      typedef typename std::list<vertex_t> pinel_t;// The in_edges list of the policy graph
      typedef typename std::vector<pinel_t> inedges1_t;
      typedef typename ::boost::iterator_property_map<
        typename inedges1_t::iterator, VertexIndexMap
      > inedges_t;
      typedef typename std::vector<edge_t> critical_cycle_t;

      //Bad  vertex flag. If true, then the vertex is "bad".
      // Vertex is "bad" if its out_degree is equal to zero.
      typedef typename boost::iterator_property_map<
        std::vector<int>::iterator, VertexIndexMap
      > badv_t;

      /*!
       * Constructor
       * \param g = (V, E) - a directed multigraph.
       * \param vim  Vertex Index Map. Read property Map: V -> [0, num_vertices(g)).
       * \param ewm  edge weight map. Read property map: E -> R
       * \param ew2m  edge weight map. Read property map: E -> R+
       * \param infty A big enough value to guaranty that there exist a cycle with
       *  better ratio.
       * \param cmp The compare operator for float_ts.
       */
      mcr_howard(const Graph &g, VertexIndexMap vim,
                  EdgeWeight1 ewm, EdgeWeight2 ew2m) :
        m_g(g), m_vim(vim), m_ew1m(ewm), m_ew2m(ew2m),
        m_bound(mcr_bound()),
        m_cr(m_bound),
        m_V(num_vertices(m_g)),
        m_dis(m_V, 0), m_dm(m_dis.begin(), m_vim),
        m_policyc(m_V), m_policy(m_policyc.begin(), m_vim),
        m_inelc(m_V), m_inel(m_inelc.begin(), m_vim),
        m_badvc(m_V, false), m_badv(m_badvc.begin(), m_vim),
        m_colcv(m_V),
        m_col_bfs(m_V)
      { }

      /*!
       * \return maximum/minimum_{for all cycles C}
       *         [sum_{e in C} w1(e)] / [sum_{e in C} w2(e)],
       * or FloatTraits::infinity() if graph has no cycles.
       */
      float_t ocr_howard()
      {
        construct_policy_graph();
        int k = 0;
        float_t mcr = 0;
        do
          {
            mcr = policy_mcr();
            ++k;
          }
        while (try_improve_policy(mcr) && k < 100); //To avoid infinite loop

        const float_t eps_ =  -0.00000001 * cmp_props_t::multiplier;
        if (m_cmp(mcr, m_bound + eps_))
          {
            return FloatTraits::infinity();
          }
        else
          {
            return  mcr;
          }
      }
      virtual ~mcr_howard() {}

    protected:
      virtual void store_critical_edge(edge_t ed, critical_cycle_t &cc) {}
      virtual void store_critical_cycle(critical_cycle_t &cc) {}

    private:
      /*!
       * \return lower/upper bound for the maximal/minimal cycle ratio
       */
      float_t mcr_bound()
      {
        typename  graph_traits<Graph>::vertex_iterator  vi, vie;
        typename  graph_traits<Graph>::out_edge_iterator  oei, oeie;
        float_t cz = (std::numeric_limits<float_t>::max)(); //Closest to zero value
        float_t s = 0;
        const float_t eps_ = std::numeric_limits<float_t>::epsilon();
        for (tie(vi, vie) = vertices(m_g); vi != vie; ++vi)
          {
            for (tie(oei, oeie) = out_edges(*vi, m_g); oei != oeie; ++oei)
              {
                s += std::abs(m_ew1m[*oei]);
                float_t a = std::abs(m_ew2m[*oei]);
                if ( a > eps_ && a < cz)
                {
                  cz = a;
                }
              }
          }
        return  cmp_props_t::multiplier * (s / cz);
      }

      /*!
       *  Constructs an arbitrary policy graph.
       */
      void construct_policy_graph()
      {
        m_sink = graph_traits<Graph>().null_vertex();
        typename  graph_traits<Graph>::vertex_iterator  vi, vie;
        typename  graph_traits<Graph>::out_edge_iterator  oei, oeie;
        for ( tie(vi, vie) = vertices(m_g); vi != vie; ++vi )
          {
            tie(oei, oeie) = out_edges(*vi, m_g);
            typename graph_traits<Graph>::out_edge_iterator mei =
              std::max_element(oei, oeie,
                               bind(m_cmp,
                                    bind(&EdgeWeight1::operator[], m_ew1m, _1),
                                    bind(&EdgeWeight1::operator[], m_ew1m, _2)
                                    )
                               );
            if (mei == oeie)
              {
                if (m_sink == graph_traits<Graph>().null_vertex())
                  {
                    m_sink = *vi;
                  }
                m_badv[*vi] = true;
                m_inel[m_sink].push_back(*vi);
              }
            else
              {
                m_inel[target(*mei, m_g)].push_back(*vi);
                m_policy[*vi] = *mei;
              }
          }
      }
      /*! Sets the distance value for all vertices "v" such that there is
       * a path from "v" to "sv". It does "inverse" breadth first visit of the policy
       * graph, starting from the vertex "sv".
       */
      void mcr_bfv(vertex_t sv, float_t cr, color_map_t c)
      {
        boost::queue<vertex_t> Q;
        c[sv] = my_black;
        Q.push(sv);
        while (!Q.empty())
          {
            vertex_t v = Q.top(); Q.pop();
            for (typename pinel_t::const_iterator itr = m_inel[v].begin();
                 itr != m_inel[v].end(); ++itr)
              //For all in_edges of the policy graph
              {
                if (*itr != sv)
                  {
                    if (m_badv[*itr])
                      {
                        m_dm[*itr] = m_dm[v] + m_bound - cr;
                      }
                    else
                      {
                        m_dm[*itr] = m_dm[v] + m_ew1m[m_policy[*itr]] -
                          m_ew2m[m_policy[*itr]] * cr;
                      }
                    c[*itr] = my_black;
                    Q.push(*itr);
                  }
              }
          }
      }

      /*!
       * \param sv an arbitrary (undiscovered) vertex of the policy graph.
       * \return a vertex in the policy graph that belongs to a cycle.
       * Performs a depth first visit until a cycle edge is found.
       */
      vertex_t find_cycle_vertex(vertex_t sv)
      {
        vertex_t gv = sv;
        std::fill(m_colcv.begin(), m_colcv.end(), my_white);
        color_map_t cm(m_colcv.begin(), m_vim);
        do
          {
            cm[gv] = my_black;
            if (! m_badv[gv])
              {
                gv = target(m_policy[gv], m_g);
              }
            else
              {
                gv = m_sink;
              }
          }
        while (cm[gv] != my_black);
        return gv;
      }

      /*!
       * \param sv - vertex that belongs to a cycle in the policy graph.
       */
      float_t cycle_ratio(vertex_t sv)
      {
        if (sv == m_sink) return m_bound;
        std::pair<float_t, float_t> sums_(float_t(0), float_t(0));
        vertex_t v = sv;
        critical_cycle_t cc;
        do
          {
            store_critical_edge(m_policy[v], cc);
            sums_.first += m_ew1m[m_policy[v]];
            sums_.second += m_ew2m[m_policy[v]];
            v = target(m_policy[v], m_g);
          }
        while (v != sv);
        float_t cr = sums_.first / sums_.second;
        if ( m_cmp(m_cr, cr) )
          {
            m_cr = cr;
            store_critical_cycle(cc);
          }
        return cr;
      }

      /*!
       *  Finds the optimal cycle ratio of the policy graph
       */
      float_t policy_mcr()
      {
        std::fill(m_col_bfs.begin(), m_col_bfs.end(), my_white);
        color_map_t vcm_ = color_map_t(m_col_bfs.begin(), m_vim);
        typename graph_traits<Graph>::vertex_iterator uv_itr, vie;
        tie(uv_itr, vie) = vertices(m_g);
        float_t mcr = m_bound;
        while ( (uv_itr = std::find_if(uv_itr, vie,
                                       bind(std::equal_to<my_color_type>(),
                                            my_white,
                                            bind(&color_map_t::operator[], vcm_, _1)
                                            )
                                       )
                 ) != vie )
          ///While there are undiscovered vertices
          {
            vertex_t gv = find_cycle_vertex(*uv_itr);
            float_t cr = cycle_ratio(gv) ;
            mcr_bfv(gv, cr, vcm_);
            if ( m_cmp(mcr, cr) )  mcr = cr;
            ++uv_itr;
          }
        return mcr;
      }

      /*!
       * Changes the edge m_policy[s] to the new_edge.
       */
      void improve_policy(vertex_t s, edge_t new_edge)
      {
        vertex_t t = target(m_policy[s], m_g);
        typename property_traits<VertexIndexMap>::value_type ti = m_vim[t];
        m_inelc[ti].erase( std::find(m_inelc[ti].begin(), m_inelc[ti].end(), s));
        m_policy[s] = new_edge;
        t = target(new_edge, m_g);
        m_inel[t].push_back(s); ///Maintain in_edge list
      }

      /*!
       * A negative cycle detector.
       */
      bool try_improve_policy(float_t cr)
      {
        bool improved = false;
        typename  graph_traits<Graph>::vertex_iterator  vi, vie;
        typename  graph_traits<Graph>::out_edge_iterator  oei, oeie;
        const float_t eps_ =  FloatTraits::epsilon();
        for (tie(vi, vie) = vertices(m_g); vi != vie; ++vi)
          {
            if (!m_badv[*vi])
              {
                for (tie(oei, oeie) = out_edges(*vi, m_g); oei != oeie; ++oei)
                  {
                    vertex_t t = target(*oei, m_g);
                    //Current distance from *vi to some vertex
                    float_t dis_ = m_ew1m[*oei] - m_ew2m[*oei] * cr + m_dm[t];
                    if ( m_cmp(m_dm[*vi] + eps_, dis_) )
                      {
                        improve_policy(*vi, *oei);
                        m_dm[*vi] = dis_;
                        improved = true;
                      }
                  }
              }
            else
              {
                float_t dis_ = m_bound - cr + m_dm[m_sink];
                if ( m_cmp(m_dm[*vi] + eps_, dis_) )
                  {
                    m_dm[*vi] = dis_;
                  }
              }
          }
        return improved;
      }
    private:
      const Graph &m_g;
      VertexIndexMap m_vim;
      EdgeWeight1 m_ew1m;
      EdgeWeight2 m_ew2m;
      comparator_t m_cmp;
      float_t m_bound; //> The lower/upper bound to the maximal/minimal cycle ratio
      float_t m_cr; //>The best cycle ratio that has been found so far

      vn_t m_V; //>The number of the vertices in the graph
      vp_t m_dis; //>Container for the distance map
      distance_map_t m_dm; //>Distance map

      ve_t m_policyc; //>Container for the policy graph
      policy_t m_policy; //>The interface for the policy graph

      inedges1_t m_inelc; //>Container fot in edges list
      inedges_t m_inel; //>Policy graph, input edges list

      std::vector<int> m_badvc;
      badv_t m_badv; //Marks "bad" vertices

      vcol_t m_colcv, m_col_bfs; //Color maps
      vertex_t m_sink; //To convert any graph to "good"
    };

    /*! \class mcr_howard1
  * \brief Finds optimum cycle raio and a critical cycle
     */
    template <typename FloatTraits,
              typename Graph, typename VertexIndexMap,
              typename EdgeWeight1, typename EdgeWeight2>
    class mcr_howard1  : public
    mcr_howard<FloatTraits, Graph, VertexIndexMap,
               EdgeWeight1, EdgeWeight2>
    {
    public:
      typedef mcr_howard<FloatTraits, Graph, VertexIndexMap,
        EdgeWeight1, EdgeWeight2> inhr_t;
      mcr_howard1(const Graph &g, VertexIndexMap vim,
        EdgeWeight1 ewm, EdgeWeight2 ew2m) :
        inhr_t(g, vim, ewm, ew2m)
      { }

      void get_critical_cycle(typename inhr_t::critical_cycle_t &cc)
      { return cc.swap(m_cc); }

    protected:
      void store_critical_edge(typename inhr_t::edge_t ed,
        typename inhr_t::critical_cycle_t &cc)
      { cc.push_back(ed); }

      void store_critical_cycle(typename inhr_t::critical_cycle_t &cc)
      { m_cc.swap(cc); }

    private:
      typename inhr_t::critical_cycle_t m_cc; //Critical cycle
    };

    /*!
     * \param g a directed multigraph.
     * \param vim Vertex Index Map. A map V->[0, num_vertices(g))
     * \param ewm Edge weight1 map.
     * \param ew2m Edge weight2 map.
     * \param pcc  pointer to the critical edges list.
     * \return Optimum cycle ratio of g or FloatTraits::infinity() if g has no cycles.
     */
    template <typename FT,
              typename TG, typename TVIM,
              typename TEW1, typename TEW2,
              typename EV>
    typename FT::value_type
 optimum_cycle_ratio(const TG &g, TVIM vim, TEW1 ewm, TEW2 ew2m, EV* pcc)
    {
      typedef typename graph_traits<TG>::directed_category DirCat;
      BOOST_STATIC_ASSERT((is_convertible<DirCat*, directed_tag*>::value == true));
      function_requires< IncidenceGraphConcept<TG> >();
      function_requires< VertexListGraphConcept<TG> >();
      typedef typename graph_traits<TG>::vertex_descriptor Vertex;
      function_requires< ReadablePropertyMapConcept<TVIM, Vertex> >();
      typedef typename graph_traits<TG>::edge_descriptor Edge;
      function_requires< ReadablePropertyMapConcept<TEW1, Edge> >();
      function_requires< ReadablePropertyMapConcept<TEW2, Edge> >();

      if(pcc == 0) {
          return detail::mcr_howard<FT,TG, TVIM, TEW1, TEW2>(
            g, vim, ewm, ew2m
          ).ocr_howard();
      }

      detail::mcr_howard1<FT, TG, TVIM, TEW1, TEW2> obj(g, vim, ewm, ew2m);
      double ocr = obj.ocr_howard();
      obj.get_critical_cycle(*pcc);
      return ocr;
    }
  } // namespace detail

// Algorithms
// Maximum Cycle Ratio

template <
    typename FloatTraits,
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeight1Map,
    typename EdgeWeight2Map>
inline typename FloatTraits::value_type
maximum_cycle_ratio(const Graph &g, VertexIndexMap vim, EdgeWeight1Map ew1m,
                    EdgeWeight2Map ew2m,
                    std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0,
                    FloatTraits = FloatTraits())
{
    typedef detail::float_wrapper<
        FloatTraits, detail::max_comparator_props<FloatTraits>
    > Traits;
    return detail::optimum_cycle_ratio<Traits>(g, vim, ew1m, ew2m, pcc);
}

template <
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeight1Map,
    typename EdgeWeight2Map>
inline double
maximum_cycle_ratio(const Graph &g, VertexIndexMap vim,
                    EdgeWeight1Map ew1m, EdgeWeight2Map ew2m,
                    std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0)
{ return maximum_cycle_ratio(g, vim, ew1m, ew2m, pcc, mcr_float<>()); }

// Minimum Cycle Ratio

template <
    typename FloatTraits,
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeight1Map,
    typename EdgeWeight2Map>
typename FloatTraits::value_type
minimum_cycle_ratio(const Graph &g, VertexIndexMap vim,
                    EdgeWeight1Map ew1m, EdgeWeight2Map ew2m,
                    std::vector<typename graph_traits<Graph>::edge_descriptor> *pcc = 0,
                    FloatTraits = FloatTraits())
{
    typedef detail::float_wrapper<
        FloatTraits, detail::min_comparator_props<FloatTraits>
    > Traits;
    return detail::optimum_cycle_ratio<Traits>(g, vim, ew1m, ew2m, pcc);
}

template <
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeight1Map,
    typename EdgeWeight2Map>
inline double
minimum_cycle_ratio(const Graph &g, VertexIndexMap vim,
                    EdgeWeight1Map ew1m, EdgeWeight2Map ew2m,
                    std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0)
{ return minimum_cycle_ratio(g, vim, ew1m, ew2m, pcc, mcr_float<>()); }

// Maximum Cycle Mean

template <
    typename FloatTraits,
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeightMap,
    typename EdgeIndexMap>
inline typename FloatTraits::value_type
maximum_cycle_mean(const Graph &g, VertexIndexMap vim,
                   EdgeWeightMap ewm, EdgeIndexMap eim,
                   std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0,
                   FloatTraits ft = FloatTraits())
{
    typedef typename remove_const<
        typename property_traits<EdgeWeightMap>::value_type
    >::type Weight;
    typename std::vector<Weight> ed_w2(boost::num_edges(g), 1);
    return maximum_cycle_ratio(g, vim, ewm,
                               make_iterator_property_map(ed_w2.begin(), eim),
                               pcc, ft);
}

template <
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeightMap,
    typename EdgeIndexMap>
inline double
maximum_cycle_mean(const Graph& g, VertexIndexMap vim,
                   EdgeWeightMap ewm, EdgeIndexMap eim,
                   std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0)
{ return maximum_cycle_mean(g, vim, ewm, eim, pcc, mcr_float<>()); }

// Minimum Cycle Mean

template <
    typename FloatTraits,
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeightMap,
    typename EdgeIndexMap>
inline typename FloatTraits::value_type
minimum_cycle_mean(const Graph &g, VertexIndexMap vim,
                   EdgeWeightMap ewm, EdgeIndexMap eim,
                   std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0,
                   FloatTraits ft = FloatTraits())
{
    typedef typename remove_const<
        typename property_traits<EdgeWeightMap>::value_type
    >::type Weight;
    typename std::vector<Weight> ed_w2(boost::num_edges(g), 1);
    return minimum_cycle_ratio(g, vim, ewm,
                               make_iterator_property_map(ed_w2.begin(), eim),
                               pcc, ft);
}

template <
    typename Graph,
    typename VertexIndexMap,
    typename EdgeWeightMap,
    typename EdgeIndexMap>
inline double
minimum_cycle_mean(const Graph &g, VertexIndexMap vim,
                   EdgeWeightMap ewm, EdgeIndexMap eim,
                   std::vector<typename graph_traits<Graph>::edge_descriptor>* pcc = 0)
{ return minimum_cycle_mean(g, vim, ewm, eim, pcc, mcr_float<>()); }

} //namespace boost

#endif