// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/vision/transformation/align_point_clouds.h"

#include <glog/logging.h>
#include <Eigen/Dense>

// Use Besel-McKay registration to align point clouds to a model. We use SVD
// decomposition because it is better at finding the absolute minimum error for
// rotation as compared to a true ICP that Besl-McKay describe, which is only
// guaranteed to find a local minima.
namespace theia {

void AlignPointCloudsICP(const int num_points,
                         const double left[],
                         const double right[],
                         double rotation[3 * 3],
                         double translation[3]) {
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > left_points(
      left, 3, num_points);
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > right_points(
      right, 3, num_points);

  Eigen::Vector3d left_centroid = left_points.rowwise().mean();
  Eigen::Vector3d right_centroid = right_points.rowwise().mean();

  // Calculate cross correlation matrix based on the points shifted about the
  // centroid.
  Eigen::Matrix3d cross_correlation = Eigen::Matrix3d::Zero();
  for (int i = 0; i < num_points; i++) {
    cross_correlation += (left_points.col(i) - left_centroid) *
                         (right_points.col(i) - right_centroid).transpose();
  }

  // Compute SVD decomposition of the cross correlation. This is excessive for 3
  // dimensions, but it is more stable than ICP.  TODO(cmsweeney): Evaluate the
  // performance more (SVD method runs in roughly 0.15ms on some tests
  // performed) to determine whether the closed form solution (see Horn's
  // quaternion method for analytic computation of rotation using quaternions)
  // is preferred.
  Eigen::JacobiSVD<Eigen::Matrix3d>
      svd(cross_correlation, Eigen::ComputeFullU | Eigen::ComputeFullV);
  // Use the memory allocated from the rotation array passed in to specify the
  // memory space for the rotation matrix calculated. This is both memory
  // efficient and prevents any memory scope errors (same concept used for
  // translation below).
  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation_mat(
      rotation);
  rotation_mat = svd.matrixV() * (svd.matrixU().transpose());

  // Rotation is solved for, but the optimal translation is the difference of
  // the right centroid and the rotated left centroid.
  Eigen::Vector3d rotated_left_centroid = rotation_mat * left_centroid;
  Eigen::Map<Eigen::RowVector3d> translation_vec(translation);
  translation_vec = right_centroid - rotated_left_centroid;
}

void AlignPointCloudsUmeyama(const int num_points,
                             const double left[],
                             const double right[],
                             double rotation[3 * 3],
                             double translation[3],
                             double* scale) {
  // First, compute the centroids.
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > left_points(
      left, 3, num_points);
  Eigen::Map<const Eigen::Matrix<double, 3, Eigen::Dynamic> > right_points(
      right, 3, num_points);

  // Get the transformation via Umeyama's least squares algorithm. This returns
  // a matrix of the form:
  //    [ s * R  t]
  //    [   0    1]
  // from which we can extract the scale, rotation, and translation.
  const Eigen::Matrix4d transformation_mat =
      Eigen::umeyama(left_points, right_points, true);

  Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation_mat(
      rotation);
  rotation_mat = transformation_mat.topLeftCorner<3, 3>();
  *scale = pow(rotation_mat.determinant(), 1.0 / 3.0);
  rotation_mat /= *scale;
  Eigen::Map<Eigen::Vector3d> translation_vec(translation);
  translation_vec = transformation_mat.topRightCorner<3, 1>();
}

}  // namespace theia


/*
Copyright (c) 2016 University of Helsinki

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 <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/algorithm/string.hpp>

using namespace cv;
using namespace std;

#define DEFAULT_FRAMERATE 5
#define DEFAULT_METHOD 2
#define DEFAULT_CROP_OUT 40
#define DEFAULT_CROP_IN 0
#define DEFAULT_FOV 170

// ----------------------------------------------------------------------

void help(char** av) {
  cout << "Usage:" << endl << av[0]
       << " [options] videofile"
       << endl << endl
       << "Arguments:" << endl
       << "  videofile : full path to video file" << endl
       << endl
       << "Options:" << endl
       << "  [--todisk|--todisk=X]  : "
       << "save output video to disk as \"output.avi\" or as \"X\""
       << endl
       << "  [--fps=X]              : "
       << "set framerate to X, default is " << DEFAULT_FRAMERATE
       << endl
       << "  [--method=X]           : "
       << "method to use, default is " << DEFAULT_METHOD
       << endl
       << "  [--crop_out=X]         : "
       << "crop X pixels from output, default is " << DEFAULT_CROP_OUT
       << endl
       << "  [--crop_in=X]          : "
       << "crop X pixels from input, default is " << DEFAULT_CROP_IN
       << endl
       << "  [--fov=X]              : "
       << "set FOV to X, default is " << DEFAULT_FOV << " (only method 1)"
       << endl;
}

// ----------------------------------------------------------------------
// Method 1, adapted from:
// https://github.com/kscottz/dewarp/blob/master/fisheye/defish.py

void buildMap(Mat &map_x, Mat &map_y, Size src_size, Size dst_size,
              double hfovd=170.0, double vfovd=170.0) {

  // Build the fisheye mapping
  double vfov = (vfovd/180.0)*CV_PI;
  double hfov = (hfovd/180.0)*CV_PI;
  double vstart = ((180.0-vfovd)/180.00)*CV_PI/2.0;
  double hstart = ((180.0-hfovd)/180.00)*CV_PI/2.0;
  size_t count = 0;

  // need to scale to changed range from our
  // smaller cirlce traced by the fov
  double xmax = sin(CV_PI/2.0)*cos(vstart);
  double xmin = sin(CV_PI/2.0)*cos(vstart+vfov);
  double xscale = xmax-xmin;
  double xoff = xscale/2.0;
  double zmax = cos(hstart);
  double zmin = cos(hfov+hstart);
  double zscale = zmax-zmin;
  double zoff = zscale/2.0;

  // Fill in the map, this is slow but
  // we could probably speed it up
  // since we only calc it once, whatever
  for (int y=0; y<dst_size.height; y++)
    for (int x=0; x<dst_size.width; x++) {
      count++;
      double phi = vstart+(vfov*((float(x)/float(dst_size.width))));
      double theta = hstart+(hfov*((float(y)/float(dst_size.height))));
      double xp = ((sin(theta)*cos(phi))+xoff)/zscale;
      double zp = ((cos(theta))+zoff)/zscale;
      double xS = src_size.width-(xp*src_size.width);
      double yS = src_size.height-(zp*src_size.height);
      map_x.at<float>(y,x) = floor(xS +0.5);
      map_y.at<float>(y,x) = floor(yS +0.5);
    }

  return;
}

// ----------------------------------------------------------------------
// Method 2, adapted from: http://paulbourke.net/dome/fish2/

void fish2sphere(Mat &map_x, Mat &map_y, Size src_size, Size dst_size) {

  Point2f pfish;
  float theta,phi,r;
  Point3f psph;

  float FOV = 3.141592654; // FOV of the fisheye, eg: 180 degrees
  float width = src_size.width;
  float height = src_size.height;

  for (int y=0; y<dst_size.height; y++)
    for (int x=0; x<dst_size.width; x++) {

      // Polar angles
      theta = 3.14159265 * (x / width - 0.5); // -pi/2 to pi/2
      phi = 3.14159265 * (y / height - 0.5);	// -pi/2 to pi/2

      // Vector in 3D space
      psph.x = cos(phi) * sin(theta);
      psph.y = cos(phi) * cos(theta);
      psph.z = sin(phi);

      // Calculate fisheye angle and radius
      theta = atan2(psph.z,psph.x);
      phi = atan2(sqrt(psph.x*psph.x+psph.z*psph.z),psph.y);
      r = width * phi / FOV;

      // Pixel in fisheye space
      map_x.at<float>(y,x) = floor(0.5 * width + r * cos(theta) + 0.5);
      map_y.at<float>(y,x) = floor(0.5 * width + r * sin(theta) + 0.5);
    }

  return;
}

// ----------------------------------------------------------------------

int main(int ac, char** av) {

  if (ac <= 1) {
    help(av);
    return 1;
  }

  bool write_video = false, imgmode = false;
  string outputfn = "output.avi", imgfn = "";
  VideoCapture capture;
  size_t framerate = DEFAULT_FRAMERATE, method = DEFAULT_METHOD;
  size_t crop_out = DEFAULT_CROP_OUT, crop_in = DEFAULT_CROP_IN;
  size_t fov = DEFAULT_FOV;

  for (int i=1; i<ac; i++) {
    string arg(av[i]);

    if (boost::starts_with(arg, "--todisk")) {
      write_video = true;
      if (arg.size()>9)
	outputfn = arg.substr(9);
      continue;
    } else if (boost::starts_with(arg, "--fps=") && arg.size()>6) {
      framerate = atoi(arg.substr(6).c_str());
      continue;
    } else if (boost::starts_with(arg, "--method=") && arg.size()>9) {
      method = atoi(arg.substr(9).c_str());
      continue;
    } else if (boost::starts_with(arg, "--imgmode")) {
      imgmode = true;
      continue;
    } else if (boost::starts_with(arg, "--crop_in=") && arg.size()>10) {
      crop_in = atoi(arg.substr(10).c_str());
      continue;
    } else if (boost::starts_with(arg, "--crop_out=") && arg.size()>11) {
      crop_out = atoi(arg.substr(11).c_str());
      continue;
    } else if (boost::starts_with(arg, "--fov=") && arg.size()>6) {
      fov = atoi(arg.substr(6).c_str());
      continue;
    }

    if (imgmode) {
      imgfn = arg;
    }  else {
      capture.open(arg);
      if (!capture.isOpened()) {
        cerr << "ERROR: Failed to open a video file [" << arg << "]" << endl;
        return 1;
      }
    }
    break;
  }

  if (imgmode && write_video) {
    cerr << "ERROR: --todisk and --imgmode cannot be used at the same time"
         << endl;
    return 1;
  }

  size_t nframe = 0; // total number of frame processed
  string window_name = "press q or esc to quit | s to save image";
  namedWindow(window_name); //resizable window;

  int fourcc = CV_FOURCC('m','p','4','v');
  VideoWriter video;
  if (write_video)
    video.open(outputfn, fourcc, framerate, Size(1280, 640));

  Size src_size(640-crop_in*2, 640-crop_in*2);
  Size dst_size(640, 640);
  Mat map_x = Mat::zeros(dst_size, CV_32F);
  Mat map_y = Mat::zeros(dst_size, CV_32F);

  cout << "Building map using method " << method << endl;
  switch (method) {
  case 1:
    buildMap(map_x, map_y, src_size, dst_size, fov, fov);
    break;
  case 2:
    fish2sphere(map_x, map_y, src_size, dst_size);
    break;
  default:
    cerr << "ERROR: unsupported method: " << method
         << endl;
    return 1;
  }
  cout << "Map done" << endl;

  for (;; nframe++) {
    Mat frame;
    bool frameok = true;
    if (imgmode)
      frame = imread(imgfn);
    else
      frameok = capture.read(frame);
    if (!frameok)
      return 0;

    Mat newframe = Mat::zeros(Size(1280, 640), CV_8UC3);

    Mat l_roi_in(frame, Rect(crop_in, crop_in, 640-crop_in*2, 640-crop_in*2));
    Mat l_out = Mat::zeros(Size(640, 640), CV_8UC3);

    transpose(l_roi_in, l_roi_in);
    flip(l_roi_in, l_roi_in, 1);

    remap(l_roi_in, l_out, map_x, map_y, CV_INTER_LINEAR);

    Mat l_out_crop(l_out, Rect(crop_out, crop_out, 640-crop_out*2, 640-crop_out*2));
    Mat l_out2;
    resize(l_out_crop, l_out2, Size(640,640));

    Mat l_roi_out(newframe, Rect(0, 0, 640, 640));
    l_out2.copyTo(l_roi_out);

    Mat r_roi_in(frame, Rect(640+crop_in, crop_in, 640-crop_in*2, 640-crop_in*2));
    Mat r_out = Mat::zeros(Size(640, 640), CV_8UC3);

    transpose(r_roi_in, r_roi_in);
    flip(r_roi_in, r_roi_in, 0);

    remap(r_roi_in, r_out, map_x, map_y, CV_INTER_LINEAR);

    Mat r_out_crop(r_out, Rect(crop_out, crop_out, 640-crop_out*2, 640-crop_out*2));
    Mat r_out2;
    resize(r_out_crop, r_out2, Size(640,640));

    Mat r_roi_out(newframe, Rect(640, 0, 640, 640));
    r_out2.copyTo(r_roi_out);

    if (write_video)
	video << newframe;
    else {

      imshow(window_name, newframe);
      //delay N millis, usually long enough to display and capture input
      char key = (char)waitKey(imgmode?0:50);
      switch (key) {
      case 'q':
      case 'Q':
      case 27: //escape key
        return 0;
      case 's':
      case 'S': {
	stringstream ss;
	ss << "unfish-output.jpg";
	cout << "Saved output image: " << ss.str() << endl;
	imwrite(ss.str(), newframe);
      }

      }
    }
  }

  return 0;
}

// ----------------------------------------------------------------------


// Copyright (C) 2015 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/extract_maximally_parallel_rigid_subgraph.h"

#include <Eigen/Core>
#include <Eigen/LU>
#include <ceres/rotation.h>

#include <algorithm>
#include <unordered_map>
#include <vector>

#include "theia/sfm/pose/util.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_graph/view_graph.h"
#include "theia/util/map_util.h"

namespace theia {
namespace {

void FormAngleMeasurementMatrix(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    const ViewGraph& view_graph,
    const std::unordered_map<ViewId, int>& view_ids_to_index,
    Eigen::MatrixXd* angle_measurements) {
  const auto& view_pairs = view_graph.GetAllEdges();
  angle_measurements->setZero();

  // Set up the matrix such that t_{i,j} x (c_j - c_i) = 0.
  int i = 0;
  for (const auto& view_pair : view_pairs) {
    // Get t_{i,j} and rotate it such that it is oriented in the global
    // reference frame.
    Eigen::Matrix3d world_to_view1_rotation;
    ceres::AngleAxisToRotationMatrix(
        FindOrDie(orientations, view_pair.first.first).data(),
        ceres::ColumnMajorAdapter3x3(world_to_view1_rotation.data()));
    const Eigen::Vector3d rotated_translation =
        world_to_view1_rotation.transpose() * view_pair.second.position_2;
    const Eigen::Matrix3d cross_product_mat =
        CrossProductMatrix(rotated_translation);

    // Find the column locations of the two views.
    const int view1_col =
        3 * FindOrDie(view_ids_to_index, view_pair.first.first);
    const int view2_col =
        3 * FindOrDie(view_ids_to_index, view_pair.first.second);

    angle_measurements->block<3, 3>(3 * i, view1_col) = -cross_product_mat;
    angle_measurements->block<3, 3>(3 * i, view2_col) = cross_product_mat;
    ++i;
  }
}

// Computes the cosine distance in each dimension x, y, and z and returns the
// maximum cosine distance.
//
// cos distance = 1.0 - a.dot(b) / (norm(a) * norm(b))
double ComputeCosineDistance(const Eigen::MatrixXd& mat1,
                             const Eigen::MatrixXd& mat2) {
  Eigen::Vector3d cos_distance;
  for (int i = 0; i < 3; i++) {
    cos_distance(i) = 1.0 - std::abs(mat1.row(i).dot(mat2.row(i)));
  }
  return cos_distance.maxCoeff();
}

// Find the maximal rigid component containing fixed_node. This is done by
// examining which nodes are parallel when removing node fixed_node from the
// null space. The nodes are only parallel if they are part of the maximal rigid
// component with fixed_node.
void FindMaximalParallelRigidComponent(const Eigen::MatrixXd& null_space,
                                       const int fixed_node,
                                       std::unordered_set<int>* largest_cc) {
  static const double kMaxCosDistance = 1e-5;
  static const double kMaxNorm = 1e-10;

  const int num_nodes = null_space.rows() / 3;

  largest_cc->insert(fixed_node);

  const Eigen::MatrixXd fixed_null_space_component =
      null_space.block(3 * fixed_node, 0, 3, null_space.cols());

  // Remove the fixed node from the rest of the null space.
  Eigen::MatrixXd modified_null_space =
      null_space - fixed_null_space_component.replicate(num_nodes, 1);

  // Normalize all rows to be unit-norm. If the rows have a very small norm then
  // they are parallel to the fixed_node. and should be set to zero.
  const Eigen::VectorXd norms = modified_null_space.rowwise().norm();

  modified_null_space.rowwise().normalize();

  // Find the pairs to match. Add all indices that are close to 0-vectors, as
  // they are clearly part of the rigid component.
  std::vector<int> indices_to_match;
  for (int i = 0; i < num_nodes; i++) {
    // Skip this index if it is fixed.
    if (i == fixed_node) {
      continue;
    }

    // Skip this index if it is nearly a 0-vector because this means it is
    // clearly part of the rigid component.
    if (norms(3 * i) < kMaxNorm && norms(3 * i + 1) < kMaxNorm &&
        norms(3 * i + 2) < kMaxNorm) {
      largest_cc->insert(i);
      continue;
    }

    indices_to_match.emplace_back(i);
  }

  // Each node has three dimensions (x, y, z). We only compare parallel-ness
  // between similar dimensions. If all x, y, z dimensions are parallel then
  // the two nodes will be parallel.
  for (int i = 0; i < indices_to_match.size(); i++) {
    // Test all other nodes (that have not been tested) to determine if they are
    // parallel to this node.
    const Eigen::MatrixXd& block1 = modified_null_space.block(
        3 * indices_to_match[i], 0, 3, null_space.cols());

    for (int j = i + 1; j < indices_to_match.size(); j++) {
      const Eigen::MatrixXd& block2 = modified_null_space.block(
          3 * indices_to_match[j], 0, 3, null_space.cols());

      const double cos_distance = ComputeCosineDistance(block1, block2);
      if (cos_distance < kMaxCosDistance) {
        largest_cc->insert(indices_to_match[i]);
        largest_cc->insert(indices_to_match[j]);
      }
    }
  }
}

}  // namespace

void ExtractMaximallyParallelRigidSubgraph(
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientations,
    ViewGraph* view_graph) {
  // Create a mapping of indexes to ViewIds for our linear system.
  std::unordered_map<ViewId, int> view_ids_to_index;
  view_ids_to_index.reserve(orientations.size());
  for (const auto& orientation : orientations) {
    if (!view_graph->HasView(orientation.first)) {
      continue;
    }
    const int current_index = view_ids_to_index.size();
    InsertIfNotPresent(&view_ids_to_index, orientation.first, current_index);
  }

  // Form the global angle measurements matrix from:
  //    t_{i,j} x (c_j - c_i) = 0.
  Eigen::MatrixXd angle_measurements(3 * view_graph->NumEdges(),
                                     3 * orientations.size());
  FormAngleMeasurementMatrix(
      orientations, *view_graph, view_ids_to_index, &angle_measurements);

  // Extract the null space of the angle measurements matrix.
  Eigen::FullPivLU<Eigen::MatrixXd> lu(angle_measurements.transpose() *
                                       angle_measurements);
  const Eigen::MatrixXd null_space = lu.kernel();

  // For each node in the graph (i.e. each camera), set the null space component
  // to be zero such that the camera position would be fixed at the origin. If
  // two nodes i and j are in the same rigid component, then their null spaces
  // will be parallel because the camera positions may only change by a
  // scale. We find all components that are parallel to find the rigid
  // components. The largest of such component is the maximally parallel rigid
  // component of the graph.
  std::unordered_set<int> maximal_rigid_component;
  for (int i = 0; i < orientations.size(); i++) {
    std::unordered_set<int> temp_cc;
    FindMaximalParallelRigidComponent(null_space, i, &temp_cc);
    if (temp_cc.size() > maximal_rigid_component.size()) {
      std::swap(temp_cc, maximal_rigid_component);
    }
  }

  // Only keep the nodes in the largest maximally parallel rigid component.
  for (const auto& orientation : orientations) {
    const int index = FindOrDie(view_ids_to_index, orientation.first);
    // If the view is not in the maximal rigid component then remove it from the
    // view graph.
    if (!ContainsKey(maximal_rigid_component, index) &&
        view_graph->HasView(orientation.first)) {
      CHECK(view_graph->RemoveView(orientation.first))
          << "Could not remove view id " << orientation.first
          << " from the view graph because it does not exist.";
    }
  }
}

}  // namespace theia


#include <iostream>
#include <algorithm>
#include <cmath>
#include <vector>
#include <map>
#include <set>
#include <tuple>
#include <stdbool.h>
#include <bitset>
#include <string>
#include <boost/multiprecision/cpp_int.hpp>

namespace mp = boost::multiprecision;
using namespace std;

// 領域数量を数える
vector<pair<int, bool>> count(const vector<bool>& src) {
    vector<pair<int, bool>> dst;

    bool label = false;
    int count = 0;

    bool reset = true;
    for(const auto& s : src) {
        if(reset) {
            label = s;
            count = 1;
            reset = false;
        } else {
            if(s != label) {
                // 途切れる
                dst.push_back(make_pair(count, label));
                label = s;
                count = 1;
                reset = false;
            } else {
                // 継続
                count++;
            }
        }
    }
    // 最後のラベル
    dst.push_back(make_pair(count, label));

    return move(dst);
}

// 一番大きくなるようにひっくり返せるように探せ
pair<int,int> search_greedy(const vector<pair<int, bool>>& src) {
    bool start = src[0].second; // falseだけ舐めたい
    int loop = src.size() / 2;

    int max_index = -1;
    int max_count = 0;
    for(int i = 0 ; i < loop; ++i) {
        int index = (i * 2) + (start ? 1 : 0);

        int count_before = (index > 0) ? src[index - 1].first : 0;
        int count_now = src[index].first; // ひっくり返されて加算
        int count_after = index < (src.size() - 1) ? src[index + 1].first : 0;
        int count = count_before + count_now + count_after; // ひっくり返されたときの特典

        if (max_count < count) {
            max_count = count;
            max_index = index;
        }
    }
    if (max_index > -1) {
        return make_pair(max_index, max_count); // ここが良さ麻生
    } else {
        return make_pair(-1, 0); // ない
    }
}
void flip(vector<pair<int, bool>>& src, int index) {
    if(index < 0 || index > src.size() - 1) {
        return;
    }
    int count = src[index].first;
    bool label = !src[index].second;
    // まわり
    if (index < src.size() - 1) {
        count += src[index+1].first;
        src.erase(src.begin() + index+1);
    }
    if (index > 0) {
        count += src[index-1].first;
        src.erase(src.begin() + index-1);
        // 手前にしないと参照が死ぬ
        index--;
    }
    // update
    src[index] = make_pair(count, label);
}

// 一番でかいのはどれ
int search_max(const vector<pair<int, bool>>& labels){
    int result = 0;
    for(const auto& l : labels) {
        if(!l.second) continue; // falseは除外

        if(result < l.first) {
            result = l.first;
        }
    }
    return result;
}
int flip_greedy(vector<pair<int, bool>>& labels, int k) {
    int count = 0;
    for(int i = 0 ; i < k ; ++i) {
        auto target = search_greedy(labels);
        if (target.first == -1) {
            if (i == 0) {
                // 一番大きいところを返す
                return search_max(labels);
            } else {
                return target.second; // もう動かすところがない
            }
        }
        count = target.second;
        flip(labels, target.first);
    }
    return count;
}

int main(void) {
    int n,k;
    string s;
    cin >> n >> k >> s;
    // k回の領域反転でsの1の数を増やす
    vector<bool> src;
    for(const auto c : s) {
        src.push_back(c == '1');
    }
    auto labels = count(src);
    auto result = flip_greedy(labels, k);
    cout << result << endl;
    return 0;
}


////////////////////////////////////////////////////////////////////////////////
// SymmetricMatrix.hh
////////////////////////////////////////////////////////////////////////////////
/*! @file
//  Classes to wrap a flattened symmetric rank 2 tensor (a 6-vector in 3D),
//  allowing it to be treated as a plain symmetric matrix.
//
//  Nearly all implementation is done in ConstSymmetricMatrixBase and
//  SymmetricMatrixBase which encapsulate the flattened data and provide most of
//  the operations needed. These classes work with both reference types
//  (aliases SymmetricMatrixRef and ConstSymmetricMatrixRef) and value types
//  (subclass SymmetricMatrix)
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  06/18/2014 17:59:39
////////////////////////////////////////////////////////////////////////////////
#ifndef SYMMETRICMATRIX_HH
#define SYMMETRICMATRIX_HH

#include "Flattening.hh"
#include <Eigen/Dense>
#include <Eigen/src/Core/Matrix.h>
#include <iostream>
#include <type_traits>
#include "utils.hh"

////////////////////////////////////////////////////////////////////////////////
// Forward declarations/aliases
////////////////////////////////////////////////////////////////////////////////
template<size_t t_N, typename Storage>
class SymmetricMatrix;

template<typename Real>
class DynamicSymmetricMatrix;

// The default storage-backed value type
template<typename _Real, size_t t_N>
using SymmetricMatrixValue =
        SymmetricMatrix<t_N, Eigen::Matrix<_Real, flatLen(t_N), 1>>;

// Compile-time mechanism for identifying symmetric matrix types.
class SymmetricMatrixType { };
template<class T>
struct is_symmetric_matrix : public std::is_base_of<SymmetricMatrixType, T> { };

template<typename _Real, size_t N> using SMEigenvaluesType  = Eigen::Matrix<_Real, N, 1>;
template<typename _Real, size_t N> using SMEigenvectorsType = Eigen::Matrix<_Real, N, N>;
template<typename _Real, size_t N> using SMEigenDecompositionType = std::pair<SMEigenvaluesType<_Real, N>, SMEigenvectorsType<_Real, N>>;

// All symmetric matrix classes currently inherit from ConstSymmetricMatrixBase,
// which in turn inherits from SymmetricMatrixType to support the
// identification mechanism above
template<typename _Real, size_t t_N,
         typename _Storage_t, typename _ConstStorageRef_t>
class ConstSymmetricMatrixBase : public SymmetricMatrixType {
public:
    static_assert(t_N > 0, "Dimension must be positive");
    ConstSymmetricMatrixBase(const ConstSymmetricMatrixBase &b) : m_data(b.m_data) { }
    ConstSymmetricMatrixBase(ConstSymmetricMatrixBase &&b) : m_data(std::move(b.m_data)) { }
    ConstSymmetricMatrixBase(const _Storage_t &data) : m_data(data) { }
    static constexpr size_t N = t_N;
    static constexpr size_t flatSize() { return (N * (N + 1)) / 2; }
    static size_t size() { return N; }

    _Real operator()(size_t i, size_t j) const {
        assert((i < N) && (j < N));
        return operator[](flattenIndices<N>(i, j));
    }

    using EigenvaluesType        = SMEigenvaluesType<_Real, N>;
    using EigenvectorsType       = SMEigenvectorsType<_Real, N>;
    using EigenDecompositionType = SMEigenDecompositionType<_Real, N>;

    Eigen::Matrix<_Real, N, N> toMatrix() const {
        Eigen::Matrix<_Real, N, N> mat;
        for (size_t j = 0; j < N; ++j)
            for (size_t i = 0; i < N; ++i)
                mat(i, j) = operator()(i, j);
        return mat;
    }

    // Note: this method provides the same conversion interface as Eigen::DenseBase::matrix();
    // this allows generic code to call `.matrix()` on SymmetricMatrix or Eigen types.
    Eigen::Matrix<_Real, N, N> matrix() const { return toMatrix(); }

    EigenvaluesType eigenvalues() const {
        Eigen::Matrix<_Real, N, N> mat;
        for (size_t j = 0; j < N; ++j)
            for (size_t i = 0; i <= j; ++i)
                mat(i, j) = operator()(i, j);
        return mat.template selfadjointView<Eigen::Upper>().eigenvalues();
    }

    // Get the eigenvalues (first) and eigenvectors (second) of this symmetric
    // matrix. Returns (diag(Lambda), Q) so that this matrix is
    // Q Lambda Q^T
    EigenDecompositionType eigenDecomposition() const {
        Eigen::Matrix<_Real, N, N> mat;
        for (size_t i = 0; i < N; ++i)
            for (size_t j = 0; j < N; ++j)
                mat(i, j) = operator()(i, j);
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<_Real, N, N>> solver;
        solver.compute(mat);
        return { solver.eigenvalues(), solver.eigenvectors() };
    }

    // Gets the symmetric matrix's eigenvectors, but scaled so their norms are
    // their eigenvalue.
    // Returns a matrix with each scaled eigenvector as a column. The
    // eigenvectors are sorted in decreasing eigenvalue **magnitude** order.
    EigenvectorsType eigenvalueScaledEigenvectors() const {
        auto decomposition = eigenDecomposition();

        std::vector<size_t> perm;
        std::vector<_Real> evMags(N);
        for (size_t i = 0; i < N; ++i) evMags[i] = std::abs(decomposition.first[i]);
        sortPermutation(evMags, perm, true);

        EigenvectorsType result;
        for (size_t i = 0; i < N; ++i)
            result.col(i) = decomposition.second.col(perm[i]) * evMags[perm[i]];

        return result;
    }

    _Real maxEigenvalue() const { return eigenvalues().maxCoeff(); }
    _Real minEigenvalue() const { return eigenvalues().minCoeff(); }
    _Real maxMagnitudeEigenvalue() const {
        auto eigs = eigenvalues();
        _Real maxEig = eigs.maxCoeff();
        _Real minEig = eigs.minCoeff();
        return (maxEig > std::abs(minEig)) ? maxEig : minEig;
    }

    _Real frobeniusNormSq() const { return this->doubleContract(*this); }

    template<typename _R2, typename _S2, typename _CSR2>
    _Real doubleContract(const ConstSymmetricMatrixBase<_R2, t_N, _S2, _CSR2> &b) const {
        // Diagonals
        _Real result(m_data[0] * b[0]);
        for (size_t i = 1; i < t_N; ++i)
            result += m_data[i] * b[i];
        // Off diagonals
        for (size_t i = t_N; i < flatSize(); ++i)
            result += 2 * m_data[i] * b[i];
        return result;
    }

    // Single contraction with vector (apply this matrix to vector on left)
    template<class Derived>
    Eigen::Matrix<typename Derived::Scalar, t_N, 1>
    contract(const Eigen::MatrixBase<Derived> &v) const {
        static_assert((Derived::RowsAtCompileTime == t_N) && (Derived::ColsAtCompileTime == 1),
                      "Invalid vector dimension for single contraction operation.");
        Eigen::Matrix<typename Derived::Scalar, t_N, 1> result;
        result.setZero();
        for (size_t i = 0; i < t_N; ++i) {
            for (size_t j = 0; j < t_N; ++j) {
                result[i] += (*this)(i, j) * v[j];
            }
        }
        return result;
    }

    // Applies an change of coordinates using the transformation rule:
    // s_ij' = s_pq R_ip R_jq
    // (When R is a rotation or reflection, this is the correct transformation
    // rule for cartesian tensors).
    // Storage-backed result
    SymmetricMatrixValue<_Real, t_N> transform(const Eigen::Matrix<_Real, t_N, t_N> &R) {
        SymmetricMatrixValue<_Real, t_N> result;
        for (size_t i = 0; i < t_N; ++i) {
            for (size_t j = i; j < t_N; ++j) {
                _Real comp = 0.0;
                for (size_t p = 0; p < t_N; ++p)
                    for (size_t q = 0; q < t_N; ++q)
                        comp += (*this)(p, q) * R(i, p) * R(j, q);
                result(i, j) = comp;
            }
        }
        return result;
    }

    // Flattened accessors
    _ConstStorageRef_t flattened() const { return m_data; }
    _Real operator[](size_t i) const { return m_data[i]; }

    // Allow us to masquarade as an Eigen vector too.
    size_t rows() const { return flatSize(); }

    friend std::ostream &operator<<(std::ostream &os, const ConstSymmetricMatrixBase &m) {
        for (size_t i = 0; i < t_N; ++i) {
            os << m(i, 0);
            for (size_t j = 1; j < t_N; ++j)
                os << " " << m(i, j);
            os << std::endl;
        }
        return os;
    }
protected:
    _Storage_t m_data;

    // SymmetricMatrix needs access to our m_data for efficient copy
    // construction.
    template<size_t t_N2, typename Storage>
    friend class SymmetricMatrix;
};

template<typename _Real, size_t t_N,
         typename _Storage_t, typename _ConstStorageRef_t, typename _StorageRef_t>
class SymmetricMatrixBase : public ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t> {
    typedef ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t> Base;
public:
    SymmetricMatrixBase(const SymmetricMatrixBase &b) : Base(b) { }
    SymmetricMatrixBase(SymmetricMatrixBase &&b) : Base(std::move(b)) { }
    SymmetricMatrixBase(const _Storage_t &data) : Base(data) { }

    using Base::operator(); // prevent hiding
    _Real &operator()(size_t i, size_t j) {
        assert((i < t_N) && (j < t_N));
        return operator[](flattenIndices<t_N>(i, j));
    }

    template<typename FType>
    void assign(const FType &f) {
        assert(f.rows() == Base::flatSize());
        for (size_t i = 0; i < Base::flatSize(); ++i)
            operator[](i) = f[i];
    }

    // Warning: these templates will be hidden by derived class' default operator=, preventing
    // mixed derived assignments unless subclass has "using Base::operator="
    // Note: `assign` can work both for flatened data types and
    // ConstSymmetricMatrixBase due to symmetric matrices' flattened access
    // operators.
    template<class SMType, typename std::enable_if<is_symmetric_matrix<SMType>::value, int>::type = 0>
    SymmetricMatrixBase &operator=(const SMType &sm) { assign(sm); return *this; }

    template<class FType, typename std::enable_if<FType::IsVectorAtCompileTime, int>::type = 0>
    SymmetricMatrixBase &operator=(const FType &f) { assign(f); return *this; }

    SymmetricMatrixBase &operator=(const SymmetricMatrixBase  &b) { m_data =           b.m_data;  return *this; }
    SymmetricMatrixBase &operator=(      SymmetricMatrixBase &&b) { m_data = std::move(b.m_data); return *this; }

    // Assignment from DynamicSymmetricMatrix is special.
    template<typename _R>
    SymmetricMatrixBase &operator=(const DynamicSymmetricMatrix<_R> &b) {
        if (b.size() != t_N) throw std::runtime_error("Dynamic-static size mismatch");
        for (size_t i = 0; i < t_N; ++i) {
            for (size_t j = 0; j <= i; ++j)
                (*this)(i, j) = b(i, j);
        }
        return *this;
    }

    // Assignment from a full matrix, validating symmetry
    template<class Derived, typename std::enable_if<(Derived::RowsAtCompileTime == t_N) &&
                                                    (Derived::ColsAtCompileTime == t_N), int>::type = 0>
    SymmetricMatrixBase &operator=(const Eigen::MatrixBase<Derived> &mat) {
        // Build symmetric matrix from upper triangle
        for (size_t i = 0; i < t_N; ++i)
            for (size_t j = i; j < t_N; ++j)
                (*this)(i, j) = mat(i, j);

        // Validate symmetry by checking lower triangle
        for (size_t i = 1; i < t_N; ++i) {
            for (size_t j = 0; j < i; ++j) {
                _Real diff = std::abs((*this)(i, j) - mat(i, j));
                if ((diff > 1e-10) && (diff > 1e-10 * std::abs(mat(i, j)))) // absolute and relative test
                    throw std::runtime_error("Attempted to construct SymmetricMatrix from asymmetric matrix");
            }
        }
        return *this;
    }

    SymmetricMatrixBase &operator*=(_Real s) {
        for (size_t i = 0; i < Base::flatSize(); ++i)
            operator[](i) *= s;
        return *this;
    }

    template<typename FType>
    SymmetricMatrixBase &operator+=(const FType &b) {
        assert(b.rows() == Base::flatSize());
        for (size_t i = 0; i < Base::flatSize(); ++i)
            operator[](i) += b[i];
        return *this;
    }

    // Addition with DynamicSymmetricMatrix is special.
    template<typename _R>
    SymmetricMatrixBase &operator+=(const DynamicSymmetricMatrix<_R> &b) {
        if (b.size() != t_N) throw std::runtime_error("Dynamic-static size mismatch");
        for (size_t i = 0; i < t_N; ++i) {
            for (size_t j = 0; j <= i; ++j)
                (*this)(i, j) += b(i, j);
        }
        return *this;
    }

    template<typename FType>
    SymmetricMatrixBase &operator-=(const FType &b) {
        assert(b.rows() == Base::flatSize());
        for (size_t i = 0; i < Base::flatSize(); ++i)
            operator[](i) -= b[i];
        return *this;
    }

    // Addition with DynamicSymmetricMatrix is special.
    template<typename _R>
    SymmetricMatrixBase &operator-=(const DynamicSymmetricMatrix<_R> &b) {
        if (b.size() != t_N) throw std::runtime_error("Dynamic-static size mismatch");
        for (size_t i = 0; i < t_N; ++i) {
            for (size_t j = 0; j <= i; ++j)
                (*this)(i, j) -= b(i, j);
        }
        return *this;
    }

    void clear() {
        for (size_t i = 0; i < Base::flatSize(); ++i)
            operator[](i) = 0.0;
    }

    // Flattened accessors
    // Bring in the base classes' definitions so they aren't hidden!!!
    using Base::operator[];
    using Base::flattened;
    _StorageRef_t flattened() { return m_data; }
    _Real &operator[](size_t i) { return m_data[i]; }

protected:
    using Base::m_data;
};

// SymmetricMatrixRef and ConstStorageRef are now just aliases to the bases...
template<size_t t_N, typename StorageRef, typename ConstStorageRef>
using SymmetricMatrixRef = SymmetricMatrixBase<typename StorageRef::Scalar, t_N,
                StorageRef, ConstStorageRef, StorageRef>;
template<size_t t_N, typename ConstStorageRef>
using ConstSymmetricMatrixRef = ConstSymmetricMatrixBase<typename
                ConstStorageRef::Scalar, t_N, ConstStorageRef, ConstStorageRef>;

// SymmetricMatrix needs to provide a few extra features that only make sense
// for storage-backed, non-reference-type matrices.
template<size_t t_N, typename Storage>
class SymmetricMatrix
    : public SymmetricMatrixBase<typename Storage::Scalar, t_N,
                                Storage, const Storage &, Storage &>
{
    typedef typename Storage::Scalar _Real;
    typedef SymmetricMatrixBase<_Real, t_N, Storage, const Storage &, Storage &> Base;
public:
    using Base::Base;
    SymmetricMatrix() : Base(Storage::Zero()) { }
    SymmetricMatrix(const SymmetricMatrix  &b) : Base(b) { }
    SymmetricMatrix(      SymmetricMatrix &&b) : Base(std::move(b)) { }
    SymmetricMatrix(size_t i) : Base(Storage::Zero()) {
        if (i >= Base::flatSize())
            throw std::runtime_error("Illegal basis element number.");
        this->operator[](i) = (i < t_N) ? 1.0 : 0.5;
    }

    struct skip_validation { };
    // Construction from general NxN matrix (doesn't validate symmetry)
    template<class Derived, typename std::enable_if<(Derived::RowsAtCompileTime == t_N) &&
                                                    (Derived::ColsAtCompileTime == t_N), int>::type = 0>
    SymmetricMatrix(const Eigen::MatrixBase<Derived> &mat, const skip_validation &): Base(Storage::Zero()) {
        // Build symmetric matrix from upper triangle
        for (size_t i = 0; i < t_N; ++i)
            for (size_t j = i; j < t_N; ++j)
                (*this)(i, j) = mat(i, j);
    }

    template<typename _ST2, typename _CSRT2>
    SymmetricMatrix(const ConstSymmetricMatrixBase<_Real, t_N, _ST2, _CSRT2> &b) : Base(b.m_data) { }

    // Construction from general NxN matrix (validates symmetry)
    template<class Derived, typename std::enable_if<(Derived::RowsAtCompileTime == t_N) &&
                                                    (Derived::ColsAtCompileTime == t_N), int>::type = 0>
    SymmetricMatrix(const Eigen::MatrixBase<Derived> &mat) : SymmetricMatrix(mat, skip_validation()) {
        // Validate symmetry by checking lower triangle
        for (size_t i = 1; i < t_N; ++i) {
            for (size_t j = 0; j < i; ++j) {
                _Real diff = std::abs((*this)(i, j) - mat(i, j));
                if ((diff > 1e-10) && (diff > 1e-10 * std::abs(mat(i, j)))) // absolute and relative test
                    throw std::runtime_error("Attempted to construct SymmetricMatrix from asymmetric matrix");
            }
        }
    }

    // Construct a unit canonical basis symmetric matrix:
    // e_ij = .5 * (e_i e_j^T + e_j e_i^T)
    static SymmetricMatrix CanonicalBasis(size_t i) {
        if (i >= Base::flatSize())
            throw std::runtime_error("Illegal basis element number.");
        SymmetricMatrix e_ij(Storage::Zero());
        e_ij[i] = (i < t_N) ? 1.0 : 0.5;
        return e_ij;
    }

    // Construct 0.5 * (a b^T + b a^T)
    template<class Vector>
    static SymmetricMatrix SymmetrizedOuterProduct(const Vector &a, const Vector &b) {
        static_assert(size_t(Vector::RowsAtCompileTime) == t_N,
                      "Axis vector dimensions must match matrix dimension.");
        SymmetricMatrix ab;
        for (size_t i = 0; i < t_N; ++i)
            for (size_t j = i; j < t_N; ++j)
                ab(i, j) = 0.5 * (a[i] * b[j] + b[i] * a[j]);
        return ab;
    }

    // Construct the symmetric matrix n n^T, which projects onto unit vector n.
    template<class Vector>
    static SymmetricMatrix ProjectionMatrix(const Vector &n) {
        static_assert(size_t(Vector::RowsAtCompileTime) == t_N,
                      "Axis vector dimension must match matrix dimension.");
        SymmetricMatrix nnt;
        for (size_t i = 0; i < t_N; ++i)
            for (size_t j = i; j < t_N; ++j)
                nnt(i, j) = n[i] * n[j];
        return nnt;
    }

    using Base::operator=;
    SymmetricMatrix &operator=(const SymmetricMatrix  &b) { Base::operator=(          b ); return *this; }
    SymmetricMatrix &operator=(      SymmetricMatrix &&b) { Base::operator=(std::move(b)); return *this; }

    SymmetricMatrix operator-() const { SymmetricMatrix result(*this); result *= -1.0; return result; }
};

////////////////////////////////////////////////////////////////////////////////
// Dynamic symmetric matrix--for dynamic cases which can be either 2x2 or 3x3
// Allocates storage for a 3x3, but dynamically chooses to use a subset.
////////////////////////////////////////////////////////////////////////////////
template<typename Real>
class DynamicSymmetricMatrix : public SymmetricMatrixValue<Real, 3> {
    using Base = SymmetricMatrixValue<Real, 3>;
public:
    // Base constructor initializes the matrix to zero.
    DynamicSymmetricMatrix(size_t n = 3) { resize(n); }
    DynamicSymmetricMatrix(const DynamicSymmetricMatrix  &b) = default;
    DynamicSymmetricMatrix(DynamicSymmetricMatrix &&b) : Base(std::move(b)), m_dynamicSize(b.m_dynamicSize) { }

    // Copy from generic symmetric matrix
    template<typename _Real, size_t _N, typename _ST, typename _CST>
    DynamicSymmetricMatrix(const ConstSymmetricMatrixBase<_Real, _N, _ST, _CST> &sm) {
        resize(_N);
        for (size_t i = 0; i < m_dynamicSize; ++i)
            for (size_t j = i; j < m_dynamicSize; ++j)
                (*this)(i, j) = sm(i, j);
    }

    Real operator()(size_t i, size_t j) const {
        if ((i >= m_dynamicSize) || (j >= m_dynamicSize)) throw std::runtime_error("DynamicSymmetricMatrix index out of bounds.");
        return (*this)[flattenIndices(m_dynamicSize, i, j)];
    }

    Real &operator()(size_t i, size_t j) {
        if ((i >= m_dynamicSize) || (j >= m_dynamicSize)) throw std::runtime_error("DynamicSymmetricMatrix index out of bounds.");
        return (*this)[flattenIndices(m_dynamicSize, i, j)];
    }

    // Note: matrix after resize is zero (doesn't simply
    // clip/pad--that would require a bit more code)
    void resize(size_t n) {
        Base::clear();
        reinterpret_resize(n);
    }

    // Set the dynamic size of the matrix without changing the underlying data.
    // This is useful for manually propogating dimension information to
    // results, e.g. for interpolation.
    void reinterpret_resize(size_t n) {
        if ((n < 2) || (n > 3)) throw std::runtime_error("Only 2x2 and 3x3 matrices supported.");
        m_dynamicSize = n;
    }

    using DVector = Eigen::Matrix<Real, Eigen::Dynamic, 1, 0, 3, 1>;
    DVector eigenvalues() const {
        if (m_dynamicSize == 3) {
            Eigen::Matrix<Real, 3, 3> mat;
            for (size_t j = 0; j < 3; ++j)
                for (size_t i = 0; i <= j; ++i)
                    mat(i, j) = operator()(i, j);
            return mat.template selfadjointView<Eigen::Upper>().eigenvalues();
        }
        else {
            Eigen::Matrix<Real, 2, 2> mat;
            for (size_t j = 0; j < 2; ++j)
                for (size_t i = 0; i <= j; ++i)
                    mat(i, j) = operator()(i, j);
            return mat.template selfadjointView<Eigen::Upper>().eigenvalues();
        }
    }

    // Base::frobeniusNormSq() gives the wrong result for 2x2 :(
    Real frobeniusNormSq() const {
        Real val = 0;
        for (size_t i = 0; i < size(); ++i) {
            for (size_t j = 0; j < size(); ++j) {
                Real e = (*this)(i, j);
                val += e * e;
            }
        }
        return val;
    }

    size_t     size() const { return m_dynamicSize; }
    size_t flatSize() const { return flatLen(m_dynamicSize); }
    ////////////////////////////////////////////////////////////////////////////
    // Assignment/compount assignment operator overloads.
    // These call the base operator after copying/checking dynamic size, but
    // need a static cast to avoid calling Base's operator with
    // DynamicSymmetricMatrix RHS (it would falsely throw a size mismatch).
    ////////////////////////////////////////////////////////////////////////////
    DynamicSymmetricMatrix &operator=(const DynamicSymmetricMatrix  &b) { m_dynamicSize = b.m_dynamicSize; Base::operator=(    static_cast<const Base &>(b) ); return *this; }
    DynamicSymmetricMatrix &operator=(      DynamicSymmetricMatrix &&b) { m_dynamicSize = b.m_dynamicSize; Base::operator=(std::move(static_cast<Base &>(b))); return *this; }
    // Base::operator*= scalar is safe, but operator[+/-]= must enforce matching
    // dynamic size
    DynamicSymmetricMatrix &operator+=(const DynamicSymmetricMatrix &b) {
        if (b.m_dynamicSize != m_dynamicSize) throw std::runtime_error("DynamicSymmetricMatrix size mismatch in operator+=");
        // Static cast to avoid calling Base's DynamicSymmetricMatrix RHS
        // version of operator= (it will complain about size mismatch).
        Base::operator+=(static_cast<const Base &>(b));
        return *this;
    }

    DynamicSymmetricMatrix &operator-=(const DynamicSymmetricMatrix &b) {
        if (b.m_dynamicSize != m_dynamicSize) throw std::runtime_error("DynamicSymmetricMatrix size mismatch in operator-=");
        // Static cast to avoid calling Base's DynamicSymmetricMatrix RHS
        // version of operator= (it will complain about size mismatch).
        Base::operator-=(static_cast<const Base &>(b));
        return *this;
    }

private:
    size_t m_dynamicSize;
};

////////////////////////////////////////////////////////////////////////////////
// Arithmetic operators--always have a storage-backed result.
////////////////////////////////////////////////////////////////////////////////
template<typename _Real, size_t t_N,
         typename _Storage_t, typename _ConstStorageRef_t>
SymmetricMatrixValue<_Real, t_N> operator*(_Real s, const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t> &mat)
{
    SymmetricMatrixValue<_Real, t_N> result(mat);
    result *= s;
    return result;
}

template<typename _Real, size_t t_N,
         typename _Storage_t, typename _ConstStorageRef_t>
SymmetricMatrixValue<_Real, t_N> operator*(const ConstSymmetricMatrixBase<_Real, t_N, _Storage_t, _ConstStorageRef_t> &mat, _Real s)
{
    SymmetricMatrixValue<_Real, t_N> result(mat);
    result *= s;
    return result;
}

template<typename _Real>
DynamicSymmetricMatrix<_Real> operator*(_Real s, const DynamicSymmetricMatrix<_Real> &mat)
{
    DynamicSymmetricMatrix<_Real> result(mat);
    result *= s;
    return result;
}

template<typename _Real>
DynamicSymmetricMatrix<_Real> operator*(const DynamicSymmetricMatrix<_Real> &mat, _Real s)
{
    DynamicSymmetricMatrix<_Real> result(mat);
    result *= s;
    return result;
}

////////////////////////////////////////////////////////////////////////////////
// Symmetric matrix view into a vector/vector slice.
////////////////////////////////////////////////////////////////////////////////
template<size_t t_N, class Derived>
struct SliceType {
    using type = decltype(std::declval<Eigen::MatrixBase<Derived>>().template segment<flatLen(t_N)>(0));
};

template<size_t t_N, class Derived>
struct ConstSliceType {
    using type = decltype(std::declval<const Eigen::MatrixBase<Derived>>().template segment<flatLen(t_N)>(0));
};

template<size_t t_N, class Derived>
using SMViewSliceType = SymmetricMatrixRef<t_N, typename SliceType<t_N, Derived>::type, typename ConstSliceType<t_N, Derived>::type>;

template<size_t t_N, class Derived>
using ConstSMViewSliceType = ConstSymmetricMatrixRef<t_N, typename ConstSliceType<t_N, Derived>::type>;

template<size_t t_N, class Derived>
SMViewSliceType<t_N, Derived>
symmetricMatrixViewSlice(Eigen::MatrixBase<Derived> &v, size_t offset) {
    return SMViewSliceType<t_N, Derived>(v.template segment<flatLen(t_N)>(offset));
}

template<size_t t_N, class Derived>
ConstSMViewSliceType<t_N, Derived>
symmetricMatrixViewSlice(const Eigen::MatrixBase<Derived> &v, size_t offset) {
    return ConstSMViewSliceType<t_N, Derived>(v.template segment<flatLen(t_N)>(offset));
}

#endif /* end of include guard: SYMMETRICMATRIX_HH */


#include "eigen.h"
#include <fstream>

#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Eigen>
#include <Eigen/Sparse>

Vector<float> as_vector (const VectorXf & x)
{
  return Vector<float>(x.size(), const_cast<float *>(x.data()));
}

Vector<float> as_vector (const MatrixXf & x)
{
  return Vector<float>(x.size(), const_cast<float *>(x.data()));
}

Vector<double> as_vector (const VectorXd & x)
{
  return Vector<double>(x.size(), const_cast<double *>(x.data()));
}

Vector<double> as_vector (const MatrixXd & x)
{
  return Vector<double>(x.size(), const_cast<double *>(x.data()));
}

void write_to_python (const VectorXf & x, ostream & os)
{
  const int entries_per_line = 8;

  os << "[";
  for (int i = 0; i < x.size(); ++i) {
    os << x[i] << ", ";
    if ((i + 1) % entries_per_line == 0) os << "\n  ";
  }
  os << "]";
}

void save_to_python (const MatrixXf & A, string filename)
{
  LOG("saving MatrixXf to " << filename);

  std::ofstream file(filename);

  file << "[";
  for (int i = 0; i < A.rows(); ++i) {
    file << "\n  [";
    for (int j = 0; j < A.cols(); ++j) {
      file << A(i,j) << ", ";
    }
    file << "],";
  }
  file << "\n]\n";
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/functional.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/pair.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_LAMBDA auto sorted = sort_by(less ^on^ first, list(
        pair(int_<3>, 'x'),
        pair(int_<1>, type<void>),
        pair(int_<2>, 9876)
    ));

    BOOST_HANA_CONSTEXPR_ASSERT(sorted == list(
        pair(int_<1>, type<void>),
        pair(int_<2>, 9876),
        pair(int_<3>, 'x')
    ));
    //! [main]
}


//=======================================================================
// Copyright (c) 2013 Piotr Wygocki
//               2014 Piotr Godlewski
//
// 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)
//=======================================================================
/**
 * @file steiner_network.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-06-24
 */
#ifndef PAAL_STEINER_NETWORK_HPP
#define PAAL_STEINER_NETWORK_HPP

#include "paal/iterative_rounding/ir_components.hpp"
#include "paal/iterative_rounding/iterative_rounding.hpp"
#include "paal/iterative_rounding/steiner_network/prune_restrictions_to_tree.hpp"
#include "paal/iterative_rounding/steiner_network/steiner_network_oracle.hpp"
#include "paal/lp/lp_row_generation.hpp"

#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/range/as_array.hpp>

namespace paal {
namespace ir {

namespace {
struct steiner_network_compare_traits {
    static const double EPSILON;
};

const double steiner_network_compare_traits::EPSILON = 1e-10;
}

/**
 * @class steiner_network
 * @brief The class for solving the Steiner Network problem using Iterative
* Rounding.
 *
 * @tparam Graph input graph
 * @tparam Restrictions connectivity restrictions for vertex pairs
 * @tparam CostMap map of edge costs
 * @tparam ResultNetworkOutputIterator
 */
template <typename Graph, typename Restrictions, typename CostMap,
          typename VertexIndex, typename ResultNetworkOutputIterator,
          typename Oracle = paal::lp::random_violated_separation_oracle>
class steiner_network {
    using edge = typename boost::graph_traits<Graph>::edge_descriptor;
    using vertex = typename boost::graph_traits<Graph>::vertex_descriptor;
    using vertex_idx = decltype(get(std::declval<VertexIndex>(),
                                   std::declval<VertexIndex>()));

    using edge_map = std::unordered_map<lp::col_id, edge>;
    using edge_list = std::vector<edge>;
    using compare = utils::compare<double>;
    using error_message = boost::optional<std::string>;

    steiner_network_violation_checker m_violation_checker;
    const Graph &m_g;
    const Restrictions &m_restrictions;
    CostMap m_cost_map;
    VertexIndex m_index;
    ResultNetworkOutputIterator m_result_network;
    RestrictionsVector m_restrictions_vec;
    Oracle m_oracle;

    edge_map m_edge_map;
    edge_list m_result_list;
    compare m_compare;

public:

    /**
     * Constructor.
     */
    steiner_network(const Graph & g, const Restrictions & restrictions,
                    CostMap cost_map, VertexIndex vertex_index,
                    ResultNetworkOutputIterator result_network,
                    Oracle oracle = Oracle{}) :
            m_g(g), m_restrictions(restrictions),
            m_cost_map(cost_map), m_index(vertex_index), m_result_network(result_network),
            m_restrictions_vec(prune_restrictions_to_tree(m_restrictions, num_vertices(m_g))),
            m_oracle(oracle), m_compare(steiner_network_compare_traits::EPSILON) {}

    /**
     * Checks if the connectivity restrictions can be fulfilled.
     */
    error_message check_input_validity() {
        steiner_network_violation_checker checker;
        if (!checker.check_if_solution_exists(*this)) {
            return error_message{ "A Steiner network satisfying the "
                                 "restrictions does not exist." };
        } else {
            return error_message{};
        }
    }

    /**
     * @brief
     *
     * @tparam LP
     * @param lp
     *
     * @return
     */
    template <typename LP>
    auto get_find_violation(LP & lp) {
        using candidate = steiner_network_violation_checker::Candidate;
        return m_oracle([&](){return m_violation_checker.get_violation_candidates(*this, lp);},
                        [&](candidate c){return m_violation_checker.check_violation(c, *this);},
                        [&](candidate c){return m_violation_checker.add_violated_constraint(c, *this, lp);});
    }

    /**
     * Returns the map from LP column IDs to edges.
     */
    const edge_map &get_edge_map() const { return m_edge_map; }

    /**
     * Returns the input graph.
     */
    const Graph &get_graph() const { return m_g; }

    /**
     * Returns the vertex index.
     */
    const VertexIndex &get_index() const { return m_index; }

    /**
     * Returns the bigger of the two restrictions for a given vertex pair.
     */
    // TODO when automatic return is used g++-4.8.2 crashes
    auto get_max_restriction(vertex_idx u, vertex_idx v) const
        -> decltype(std::declval<Restrictions>()(0, 0))
    {
        return std::max(m_restrictions(u, v), m_restrictions(v, u));
    }

    /**
     * Returns the restrictions vector.
     */
    const RestrictionsVector &get_restrictions_vec() const
    {
        return m_restrictions_vec;
    }

    /**
     * Returns the cost of an edge.
     */
    // TODO when automatic return is used g++-4.8.2 crashes
    auto get_cost(edge e) -> decltype(get(std::declval<CostMap>(), e))
    {
        return get(m_cost_map, e);
    }

    /**
     * Binds a graph edge to a LP column.
     */
    void bind_edge_to_col(edge e, lp::col_id col) {
        m_edge_map.insert(typename edge_map::value_type(col, e));
    }

    /**
     * Removes an LP column and the graph edge corresponding to it.
     */
    void remove_column(lp::col_id col_id) {
        auto ret = m_edge_map.erase(col_id);
        assert(ret == 1);
    }

    /**
     * Adds an edge corresponding to the given column to the result set.
     */
    void add_column_to_solution(lp::col_id col_id) {
        auto e = col_to_edge(col_id);
        *m_result_network = e;
        ++m_result_network;
        m_result_list.push_back(e);
    }

    /**
     * Returns the list of edges that are already added to the solution.
     */
    const edge_list &get_edges_in_solution() const { return m_result_list; }

    /**
     * Returns the double comparison object.
     */
    compare get_compare() const { return m_compare; }

  private:

    edge col_to_edge(lp::col_id col) {
        auto i = m_edge_map.find(col);
        assert(i != m_edge_map.end());
        return i->second;
    }

};

namespace detail {
/**
 * @brief Creates a steiner_network object. Non-named version.
 *
 * @tparam Graph
 * @tparam Restrictions
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam ResultNetworkOutputIterator
 * @param g
 * @param restrictions
 * @param cost_map
 * @param vertex_index
 * @param result_network
 * @param oracle
 *
 * @return steiner_network object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
    typename Graph, typename Restrictions, typename CostMap,
    typename VertexIndex, typename ResultNetworkOutputIterator>
steiner_network<Graph, Restrictions, CostMap, VertexIndex, ResultNetworkOutputIterator, Oracle>
make_steiner_network(const Graph & g, const Restrictions & restrictions,
                    CostMap cost_map, VertexIndex vertex_index,
                    ResultNetworkOutputIterator result_network,
                    Oracle oracle = Oracle{}) {
    return steiner_network<Graph, Restrictions, CostMap, VertexIndex,
                ResultNetworkOutputIterator, Oracle>(
                                    g, restrictions, cost_map, vertex_index, result_network, oracle);
}
} // detail

/**
 * Creates a steiner_network object. Named version.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam Restrictions
 * @tparam ResultNetworkOutputIterator
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param restrictions
 * @param params
 * @param result_network
 * @param oracle
 *
 * @return steiner_network object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
    typename Graph, typename Restrictions, typename ResultNetworkOutputIterator,
    typename P, typename T, typename R>
auto
make_steiner_network(const Graph & g, const Restrictions & restrictions,
                    const boost::bgl_named_params<P, T, R>& params,
                    ResultNetworkOutputIterator result_network,
                    Oracle oracle = Oracle()) ->
       steiner_network<Graph, Restrictions,
            decltype(choose_const_pmap(get_param(params, boost::edge_weight), g, boost::edge_weight)),
            decltype(choose_const_pmap(get_param(params, boost::vertex_index), g, boost::vertex_index)),
            ResultNetworkOutputIterator, Oracle> {
    return detail::make_steiner_network(g, restrictions,
                choose_const_pmap(get_param(params, boost::edge_weight), g, boost::edge_weight),
                choose_const_pmap(get_param(params, boost::vertex_index), g, boost::vertex_index),
                result_network, oracle);
}

/**
 * Creates a steiner_network object. All default parameters.
 * The returned object can be used to check input validity or to get a lower
* bound on the
 * optimal solution cost.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam Restrictions
 * @tparam ResultNetworkOutputIterator
 * @param g
 * @param restrictions
 * @param result_network
 * @param oracle
 *
 * @return steiner_network object
 */
template <typename Oracle = lp::random_violated_separation_oracle,
    typename Graph, typename Restrictions, typename ResultNetworkOutputIterator>
auto
make_steiner_network(const Graph & g, const Restrictions & restrictions,
                    ResultNetworkOutputIterator result_network, Oracle oracle = Oracle()) ->
        decltype(make_steiner_network(g, restrictions, boost::no_named_parameters(), result_network, oracle)) {
    return make_steiner_network(g, restrictions, boost::no_named_parameters(), result_network, oracle);
}

/**
 * Initialization of the IR Steiner Network algorithm.
 */
struct steiner_network_init {
    /**
     * Initializes the LP: variables for edges.
     */
    template <typename Problem, typename LP>
    void operator()(Problem &problem, LP &lp) {
        lp.set_lp_name("steiner network");
        lp.set_optimization_type(lp::MINIMIZE);
        add_variables(problem, lp);
    }

  private:
    // adding variables
    template <typename Problem, typename LP>
    void add_variables(Problem &problem, LP &lp) {
        for (auto e : boost::as_array(edges(problem.get_graph()))) {
            lp::col_id col = lp.add_column(problem.get_cost(e), 0, 1);
            problem.bind_edge_to_col(e, col);
        }
    }
};

/**
 * Round Condition of the IR Steiner Network algorithm.
 */
struct steiner_network_round_condition {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    steiner_network_round_condition(double epsilon =
                                        steiner_network_compare_traits::EPSILON)
        : m_round_half(epsilon), m_round_zero(epsilon) {}

    /**
     * Checks if a given column of the LP can be rounded to 0 or if it is
     * greater
     * then 1/2.
     * If the column is rounded to 0, the corresponding edge is removed from the
     * graph.
     * If the column is greater than 1/2, it is rounded to 1 and the
     * corresponding edge
     * is added to the solution.
     */
    template <typename Problem, typename LP>
    boost::optional<double> operator()(Problem & problem, const LP & lp, lp::col_id col_id) {
        auto ret = m_round_zero(problem, lp, col_id);
        if (ret) {
            //removing edge
            problem.remove_column(col_id);
            return ret;
        } else {
            ret = m_round_half(problem, lp, col_id);
            if (ret) {
                problem.add_column_to_solution(col_id);
                problem.remove_column(col_id);
            }
            return ret;
        }
    }

  private:
    round_condition_greater_than_half m_round_half;
    round_condition_equals<0> m_round_zero;
};

/**
 * Set Solution component of the IR Steiner Network algorithm.
 */
struct steiner_network_set_solution {
    /**
     * Constructor. Takes epsilon used in double comparison.
     */
    steiner_network_set_solution(double epsilon =
                                     steiner_network_compare_traits::EPSILON)
        : m_compare(epsilon) {}

    /**
     * Creates the result network form the LP (all edges corresponding to
     * columns with value 1).
     */
    template <typename Problem, typename GetSolution>
    void operator()(Problem &problem, const GetSolution &solution) {
        for (auto edge_and_col : problem.get_edge_map()) {
            if (m_compare.e(solution(edge_and_col.first), 1)) {
                problem.add_column_to_solution(edge_and_col.first);
            }
        }
    }

private:
    const utils::compare<double> m_compare;
};

template <typename Init = steiner_network_init,
          typename RoundCondition = steiner_network_round_condition,
          typename RelaxContition = utils::always_false,
          typename SetSolution = steiner_network_set_solution,
          typename SolveLPToExtremePoint = row_generation_solve_lp<>,
          typename ResolveLPToExtremePoint = row_generation_solve_lp<>>
using steiner_network_ir_components =
    IRcomponents<Init, RoundCondition, RelaxContition, SetSolution,
                 SolveLPToExtremePoint, ResolveLPToExtremePoint>;

namespace detail {
/**
 * @brief Solves the Steiner Network problem using Iterative Rounding. Non-named
* version.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam Restrictions
 * @tparam CostMap
 * @tparam VertexIndex
 * @tparam ResultNetworkOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param restrictions
 * @param cost
 * @param vertex_index
 * @param result
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle, typename Graph,
          typename Restrictions, typename CostMap, typename VertexIndex,
          typename ResultNetworkOutputIterator,
          typename IRcomponents = steiner_network_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult steiner_network_iterative_rounding(
        const Graph & g,
        const Restrictions & restrictions,
        CostMap cost,
        VertexIndex vertex_index,
        ResultNetworkOutputIterator result,
        IRcomponents components = IRcomponents(),
        Oracle oracle = Oracle(),
        Visitor visitor = Visitor()) {
    auto steiner = make_steiner_network(g, restrictions, cost, vertex_index, result, oracle);
    return solve_iterative_rounding(steiner, std::move(components), std::move(visitor));
}
} // detail

/**
 * @brief Solves the Steiner Network problem using Iterative Rounding. Named
* version.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam Restrictions
 * @tparam ResultNetworkOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @tparam P
 * @tparam T
 * @tparam R
 * @param g
 * @param restrictions
 * @param params
 * @param result
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle, typename Graph,
          typename Restrictions, typename ResultNetworkOutputIterator,
          typename IRcomponents = steiner_network_ir_components<>,
          typename Visitor = trivial_visitor, typename P, typename T,
          typename R>
IRResult steiner_network_iterative_rounding(
    const Graph &g, const Restrictions &restrictions,
    const boost::bgl_named_params<P, T, R> &params,
    ResultNetworkOutputIterator result,
    IRcomponents components = IRcomponents(), Oracle oracle = Oracle(),
    Visitor visitor = Visitor()) {
    return detail::steiner_network_iterative_rounding(
        g, restrictions,
        choose_const_pmap(get_param(params, boost::edge_weight), g,
                          boost::edge_weight),
        choose_const_pmap(get_param(params, boost::vertex_index), g,
                          boost::vertex_index),
        std::move(result), std::move(components), std::move(oracle),
        std::move(visitor));
}

/**
 * @brief Solves the Steiner Network problem using Iterative Rounding. All
* default parameters.
 *
 * @tparam Oracle
 * @tparam Graph
 * @tparam Restrictions
 * @tparam ResultNetworkOutputIterator
 * @tparam IRcomponents
 * @tparam Visitor
 * @param g
 * @param restrictions
 * @param result
 * @param components
 * @param oracle
 * @param visitor
 *
 * @return solution status
 */
template <typename Oracle = lp::random_violated_separation_oracle, typename Graph,
          typename Restrictions, typename ResultNetworkOutputIterator,
          typename IRcomponents = steiner_network_ir_components<>,
          typename Visitor = trivial_visitor>
IRResult steiner_network_iterative_rounding(const Graph &g,
                                            const Restrictions &restrictions,
                                            ResultNetworkOutputIterator result,
                                            IRcomponents components =
                                                IRcomponents(),
                                            Oracle oracle = Oracle(),
                                            Visitor visitor = Visitor()) {
    return steiner_network_iterative_rounding(
        g, restrictions, boost::no_named_parameters(), std::move(result),
        std::move(components), std::move(oracle), std::move(visitor));
}

} //! ir
} //! paal
#endif // PAAL_STEINER_NETWORK_HPP


#ifndef VERTEX_H
#define VERTEX_H

#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include <string>
#include <iostream>
#include <math.h>
#include <sstream>
#include <armadillo>

using namespace std;

class Vertex {
	public:
		float x;
		float y;
		float z;

        Vertex();
		Vertex(float _x, float _y, float _z);
		Vertex(Vertex *other);
		std::vector <float> getValues();
		std :: string toString();
        void print();
		arma::fcolvec getHomg();
		arma::frowvec get_value();
		void set_value(arma::frowvec trans);
		void set_value(float transx,float transy,float transz);

};

#endif


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    TestPRandomGraphSource.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/
#include "vtkAdjacentVertexIterator.h"
#include "vtkBitArray.h"
#include "vtkDataSetAttributes.h"
#include "vtkDistributedGraphHelper.h"
#include "vtkGraph.h"
#include "vtkIdTypeArray.h"
#include "vtkMath.h"
#include "vtkOutEdgeIterator.h"
#include "vtkPBGLBreadthFirstSearch.h"
#include "vtkPBGLConnectedComponents.h"
#include "vtkPBGLGraphAdapter.h"
#include "vtkPBGLMinimumSpanningTree.h"
#include "vtkPBGLVertexColoring.h"
#include "vtkPBGLRandomGraphSource.h"
#include "vtkSmartPointer.h"
#include "vtkStreamingDemandDrivenPipeline.h"
#include "vtkVertexListIterator.h"

#include <vtksys/stl/functional>
#include <vtksys/stl/string>

#include <boost/mpi/collectives.hpp>
#include <boost/mpi/communicator.hpp>
#include <boost/mpi/timer.hpp>
#include <boost/lexical_cast.hpp>

#define VTK_CREATE(type, name) \
  vtkSmartPointer<type> name = vtkSmartPointer<type>::New()

int main(int argc, char* argv[])
{
  boost::mpi::environment env(argc, argv);
  boost::mpi::communicator world;

  vtkIdType wantVertices = 100;
  vtkIdType wantEdges = 200;

  bool doPrint = false;
  bool onlyConnected = false;
  bool doVerify = true;
  bool doBFS = true;
  bool doColoring = true;
  bool doConnectedComponents = true;
  bool doMST = true;

  if (argc > 2)
    {
    wantVertices = boost::lexical_cast<vtkIdType>(argv[1]);
    wantEdges = boost::lexical_cast<vtkIdType>(argv[2]);
    }

  // The Dehne-Gotz minimum spanning tree algorithm actually allocates
  // O(|V|) memory, where |V| is the size of the full distributed
  // graph. For this reason, we won't (by default) run the minimum
  // spanning tree algorithm with > 1 million vertices.
  if (wantVertices > 1000000)
    {
    doMST = false;
    }

  for (int argIdx = 3; argIdx < argc; ++argIdx)
    {
      vtkstd::string arg = argv[argIdx];
      if (arg == "--print")
        {
        doPrint = true;
        }
      else if (arg == "--only-connected")
        {
        onlyConnected = true;
        }
      else if (arg == "--no-bfs")
        {
        doBFS = false;
        }
      else if (arg == "--no-coloring")
        {
        doColoring = false;
        }
      else if (arg == "--no-connected-components")
        {
        doConnectedComponents = false;
        }
      else if (arg == "--no-minumum-spanning-tree")
        {
        doMST = false;
        }
      else if (arg == "--minumum-spanning-tree")
        {
        doMST = true;
        }
    }

  vtkIdType totalNumberOfVertices;
  vtkIdType totalNumberOfEdges;
  vtkGraph* g;

  VTK_CREATE(vtkPBGLRandomGraphSource, source);

  int errors = 0;

  source->SetNumberOfVertices(wantVertices);
  source->SetNumberOfEdges(wantEdges);

  if (!onlyConnected)
    {
    if (world.rank() == 0)
      {
      cerr << "Testing simple random generator (" << wantVertices << ", "
           << wantEdges << ")..." << endl;
      }
    source->Update();
    g = source->GetOutput();

    totalNumberOfVertices
      = boost::mpi::all_reduce(world, g->GetNumberOfVertices(),
                               vtkstd::plus<vtkIdType>());
    if (totalNumberOfVertices != wantVertices)
      {
      cerr << "ERROR: Wrong number of vertices ("
           << totalNumberOfVertices << " != " << wantVertices << ")" << endl;
      errors++;
      }

    totalNumberOfEdges
      = boost::mpi::all_reduce(world, g->GetNumberOfEdges(),
                               vtkstd::plus<vtkIdType>());
    if (totalNumberOfEdges != wantEdges)
      {
      cerr << "ERROR: Wrong number of edges ("
           << totalNumberOfEdges << " != " << wantEdges << ")" << endl;
      errors++;
      }
    if (world.rank() == 0)
      {
      cerr << "...done." << endl;
      }
    }

  if (world.rank() == 0)
    {
    cerr << "Testing simple tree+random generator (" << wantVertices << ", "
         << wantEdges << ")..." << endl;
    }
  source->SetStartWithTree(true);
  source->Update();
  g = source->GetOutput();

  totalNumberOfVertices
    = boost::mpi::all_reduce(world, g->GetNumberOfVertices(),
                             vtkstd::plus<vtkIdType>());
  if (totalNumberOfVertices != wantVertices)
    {
    cerr << "ERROR: Wrong number of vertices ("
         << totalNumberOfVertices << " != " << wantVertices << ")" << endl;
    errors++;
    }

  totalNumberOfEdges
    = boost::mpi::all_reduce(world, g->GetNumberOfEdges(),
                             vtkstd::plus<vtkIdType>());
  if (totalNumberOfEdges != wantEdges + wantVertices - 1)
    {
    cerr << "ERROR: Wrong number of edges ("
         << totalNumberOfEdges << " != " << (wantEdges + wantVertices - 1)
         << ")" << endl;
    errors++;
    }

  if (world.rank() == 0)
    {
    cerr << "...done." << endl;
    }

  if (doPrint)
    {
    vtkSmartPointer<vtkVertexListIterator> vertices
      = vtkSmartPointer<vtkVertexListIterator>::New();
    g->GetVertices(vertices);
    while (vertices->HasNext())
      {
      vtkIdType u = vertices->Next();

      vtkSmartPointer<vtkOutEdgeIterator> outEdges
        = vtkSmartPointer<vtkOutEdgeIterator>::New();

      g->GetOutEdges(u, outEdges);
      while (outEdges->HasNext())
        {
        vtkOutEdgeType e = outEdges->Next();
        cerr << "  " << u << " -- " << e.Target << endl;
        }
      }
    }

  if (doBFS)
    {
    vtkSmartPointer<vtkPBGLBreadthFirstSearch> bfs
      = vtkSmartPointer<vtkPBGLBreadthFirstSearch>::New();
    bfs->SetInput(g);
    bfs->SetOriginVertex(g->GetDistributedGraphHelper()->MakeDistributedId(0, 0));

    // Run the breadth-first search
    if (world.rank() == 0)
      {
      cerr << "Breadth-first search...";
      }
    boost::mpi::timer timer;
    bfs->UpdateInformation();
    vtkStreamingDemandDrivenPipeline* exec =
      vtkStreamingDemandDrivenPipeline::SafeDownCast(bfs->GetExecutive());
    exec->SetUpdateNumberOfPieces(exec->GetOutputInformation(0), world.size());
    exec->SetUpdatePiece(exec->GetOutputInformation(0), world.rank());
    bfs->Update();

    // Verify the results of the breadth-first search
    if (world.rank() == 0)
      {
      cerr << " done in " << timer.elapsed() << " seconds" << endl;
      }
    }

  if (doColoring)
    {
    vtkSmartPointer<vtkPBGLVertexColoring> coloring
      = vtkSmartPointer<vtkPBGLVertexColoring>::New();
    coloring->SetInput(g);

    // Run the vertex-coloring
    if (world.rank() == 0)
      {
      cerr << "Vertex coloring...";
      }
    boost::mpi::timer timer;
    coloring->UpdateInformation();
    vtkStreamingDemandDrivenPipeline* exec =
      vtkStreamingDemandDrivenPipeline::SafeDownCast(coloring->GetExecutive());
    exec->SetUpdateNumberOfPieces(exec->GetOutputInformation(0), world.size());
    exec->SetUpdatePiece(exec->GetOutputInformation(0), world.rank());
    coloring->Update();

    if (world.rank() == 0)
      {
      cerr << " done in " << timer.elapsed() << " seconds" << endl;
      }

    if (doVerify)
      {
      vtkGraph* output = vtkGraph::SafeDownCast(coloring->GetOutput());
      if (world.rank() == 0)
        {
        cerr << " Verifying vertex coloring...";
        }

      // Turn the color array into a property map
      vtkIdTypeArray* colorArray
        = vtkIdTypeArray::SafeDownCast
            (output->GetVertexData()->GetAbstractArray("Color"));
      vtkDistributedVertexPropertyMapType<vtkIdTypeArray>::type colorMap
        = MakeDistributedVertexPropertyMap(output, colorArray);

      // Restart the timer
      timer.restart();

      vtkSmartPointer<vtkVertexListIterator> vertices
        = vtkSmartPointer<vtkVertexListIterator>::New();
      output->GetVertices(vertices);
      while (vertices->HasNext())
        {
        vtkIdType u = vertices->Next();
        vtkSmartPointer<vtkOutEdgeIterator> outEdges
          = vtkSmartPointer<vtkOutEdgeIterator>::New();

        output->GetOutEdges(u, outEdges);
        while (outEdges->HasNext())
          {
          vtkOutEdgeType e = outEdges->Next();
          if (get(colorMap, u) == get(colorMap, e.Target))
            {
            cerr << "ERROR: Found adjacent vertices " << u << " and "
                 << e.Target << " with the same color value ("
                 << get(colorMap, u) << ")" << endl;
            ++errors;
            }
          }
        }

      output->GetDistributedGraphHelper()->Synchronize();

      if (world.rank() == 0)
        {
        cerr << " done in " << timer.elapsed() << " seconds" << endl;
        }
      }
    }

  if (doConnectedComponents)
    {
    vtkSmartPointer<vtkPBGLConnectedComponents> cc
      = vtkSmartPointer<vtkPBGLConnectedComponents>::New();
    cc->SetInput(g);

    // Run the connected components algorithm
    if (world.rank() == 0)
      {
      cerr << "Connected components...";
      }
    boost::mpi::timer timer;
    cc->UpdateInformation();
    vtkStreamingDemandDrivenPipeline* exec =
      vtkStreamingDemandDrivenPipeline::SafeDownCast(cc->GetExecutive());
    exec->SetUpdateNumberOfPieces(exec->GetOutputInformation(0), world.size());
    exec->SetUpdatePiece(exec->GetOutputInformation(0), world.rank());
    cc->Update();

    if (world.rank() == 0)
      {
      cerr << " done in " << timer.elapsed() << " seconds" << endl;
      }

    if (doVerify)
      {
      vtkGraph* output = vtkGraph::SafeDownCast(cc->GetOutput());
      if (world.rank() == 0)
        {
        cerr << " Verifying connected components...";
        }

      // Turn the component array into a property map
      vtkIdTypeArray* componentArray
        = vtkIdTypeArray::SafeDownCast
            (output->GetVertexData()->GetAbstractArray("Component"));
      vtkDistributedVertexPropertyMapType<vtkIdTypeArray>::type componentMap
        = MakeDistributedVertexPropertyMap(output, componentArray);

      // Restart the timer
      timer.restart();

      vtkSmartPointer<vtkVertexListIterator> vertices
        = vtkSmartPointer<vtkVertexListIterator>::New();
      output->GetVertices(vertices);
      while (vertices->HasNext())
        {
        vtkIdType u = vertices->Next();
        vtkSmartPointer<vtkOutEdgeIterator> outEdges
          = vtkSmartPointer<vtkOutEdgeIterator>::New();

        output->GetOutEdges(u, outEdges);
        while (outEdges->HasNext())
          {
          vtkOutEdgeType e = outEdges->Next();
          if (get(componentMap, u) != get(componentMap, e.Target))
            {
            cerr << "ERROR: Found adjacent vertices " << u << " and "
                 << e.Target << " with different component values ("
                 << get(componentMap, u) << " and "
                 << get(componentMap, e.Target) << ")" << endl;
            ++errors;
            }
          }
        }

      output->GetDistributedGraphHelper()->Synchronize();

      if (world.rank() == 0)
        {
        cerr << " done in " << timer.elapsed() << " seconds" << endl;
        }
      }
    }

  if (doMST)
    {
    vtkSmartPointer<vtkPBGLMinimumSpanningTree> mst
      = vtkSmartPointer<vtkPBGLMinimumSpanningTree>::New();
    mst->SetInput(g);
    mst->SetEdgeWeightArrayName("Weight");

    // Create an edge-weight array with edge weights in [0, 1).
    vtkDoubleArray *edgeWeightArray = vtkDoubleArray::New();
    edgeWeightArray->SetName("Weight");
    g->GetEdgeData()->AddArray(edgeWeightArray);
    edgeWeightArray->SetNumberOfTuples(g->GetNumberOfEdges());
    vtkMath::RandomSeed(1177 + 17 * world.rank());
    for (vtkIdType i = 0; i < g->GetNumberOfEdges(); ++i)
      {
      edgeWeightArray->SetTuple1(i, vtkMath::Random());
      }

    // Run the shortest paths algorithm.
    if (world.rank() == 0)
      {
      cerr << "Minimum spanning tree...";
      }
    boost::mpi::timer timer;
    mst->UpdateInformation();
    vtkStreamingDemandDrivenPipeline* exec =
      vtkStreamingDemandDrivenPipeline::SafeDownCast(mst->GetExecutive());
    exec->SetUpdateNumberOfPieces(exec->GetOutputInformation(0), world.size());
    exec->SetUpdatePiece(exec->GetOutputInformation(0), world.rank());
    mst->Update();

    if (world.rank() == 0)
      {
      cerr << " done in " << timer.elapsed() << " seconds" << endl;
      }
    }

  return errors;
}


/**
 * @file numpdesetup_main.cc
 * @brief NPDE homework NumPDESetup code
 * @author Ralf Hiptmair
 * @date 17.02.2021
 * @copyright Developed at ETH Zurich
 */

#include <Eigen/Core>
#include <iostream>

#include "numpdesetup.h"

/* SAM_LISTING_BEGIN_2 */
int main(int /*argc*/, char** /*argv*/) {
  std::cout << "Dummy \"homework problem\" for NumPDE course" << std::endl;
  Eigen::VectorXd v = NumPDESetup::dummyFunction(42.0, 10);

  const static Eigen::IOFormat CSVFormat(Eigen::FullPrecision,
                                         Eigen::DontAlignCols, ", ", "\n");
  std::cout << v.transpose().format(CSVFormat) << std::endl;
  return 0;
}
/* SAM_LISTING_END_2 */


//////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::location_scale::include.hpp             //
//                                                                              //
//  (C) Copyright 2009 Erwann Rogard                                            //
//  Use, modification and distribution are 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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_LOCATION_SCALE_INCLUDE_HPP_ER_2009

#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/cdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/is_scalar_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/location_scale.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/mean.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/quantile.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/variance.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/location_scale/os.hpp>

#endif


//  Copyright (c) 2001-2010 Hartmut Kaiser
//
//  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)

// The purpose of this example is to demonstrate how custom, user defined types
// can be easily integrated with the lexer as token value types. Moreover, the
// custom token values are properly exposed to the parser as well, allowing to
// retrieve the custom values using the built in parser attribute propagation
// rules.

#include <boost/spirit/include/lex_lexertl.hpp>
#include <boost/spirit/include/qi.hpp>

namespace lex = boost::spirit::lex;
namespace qi = boost::spirit::qi;
namespace mpl = boost::mpl;

///////////////////////////////////////////////////////////////////////////////
// This is just a simple custom rational data structure holding two ints to be
// interpreted as a rational number
struct rational
{
    rational(int n = 0, int d = 0)
      : nominator_(n), denominator_(d)
    {}

    int nominator_;
    int denominator_;
};

///////////////////////////////////////////////////////////////////////////////
// A rational is represented as "{n,d}", where 'n' and 'd' are the nominator
// and denominator of the number. We use Spirit.Qi to do the low level parsing
// of the input sequence as matched by the lexer. Certainly, any other
// conversion could be used instead.
//
// The lexer uses the template assign_to_attribute_from_iterators<> to convert
// the matched input sequence (pair of iterators) to the token value type as
// specified while defining the lex::token_def<>.
//
// Our specialization of assign_to_attribute_from_iterators<> for the rational
// data type defined above has to be placed into the
// namespace boost::spirit::traits, otherwise it won't be found by the library.
namespace boost { namespace spirit { namespace traits
{
    template <typename Iterator>
    struct assign_to_attribute_from_iterators<rational, Iterator>
    {
        static void
        call(Iterator const& first, Iterator const& last, rational& attr)
        {
            int x, y;
            Iterator b = first;
            qi::parse(b, last,
                '{' >> qi::int_ >> ',' >> qi::int_ >> '}', x, y);
            attr = rational(x, y);
        }
    };
}}}

///////////////////////////////////////////////////////////////////////////////
// a lexer recognizing a single token type: rational
template <typename Lexer>
struct lex_rational : lex::lexer<Lexer>
{
    lex_rational()
    {
        this->self.add_pattern("INT", "[1-9][0-9]*");

        rt = "\\{{INT},{INT}\\}";
        this->self.add(rt);
    }
    lex::token_def<rational> rt;
};

int main()
{
    // the token type needs to know the iterator type of the underlying
    // input and the set of used token value types
    typedef lex::lexertl::token<std::string::iterator,
        mpl::vector<rational> > token_type;

    // use actor_lexer<> here if your token definitions have semantic
    // actions
    typedef lex::lexertl::lexer<token_type> lexer_type;

    // this is the iterator exposed by the lexer, we use this for parsing
    typedef lexer_type::iterator_type iterator_type;

    // create a lexer instance
    std::string input("{3,4}");
    std::string::iterator s = input.begin();

    lex_rational<lexer_type> lex;
    iterator_type b = lex.begin(s, input.end());

    // use the embedded token_def as a parser, it exposes its token value type
    // as its parser attribute type
    rational r;
    if (!qi::parse(b, lex.end(), lex.rt, r))
    {
        std::cerr << "Parsing failed!" << std::endl;
        return -1;
    }

    std::cout << "Parsing succeeded: {"
              << r.nominator_ << ", " << r.denominator_ << "}" << std::endl;
    return 0;
}