/* Authors: Lutong Wang and Bangqi Xu */
/*
 * Copyright (c) 2019, The Regents of the University of California
 * 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 University 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 REGENTS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "io/frShapeUtil.h"
#include <boost/icl/interval_map.hpp>

namespace fr {
  void frRect2Poly(frRect &rectIn, boostPolygon &polyOut) {
    frBox tmpBox;
    rectIn.getBBox(tmpBox);
    polyOut = boostPolygon();
    bg::append(polyOut, boostPoint(tmpBox.left(), tmpBox.bottom()));
    bg::append(polyOut, boostPoint(tmpBox.left(), tmpBox.top()));
    bg::append(polyOut, boostPoint(tmpBox.right(), tmpBox.top()));
    bg::append(polyOut, boostPoint(tmpBox.right(), tmpBox.bottom()));
    bg::append(polyOut, boostPoint(tmpBox.left(), tmpBox.bottom()));
  }

  void frPolygon2Poly(frPolygon &polygonIn, boostPolygon &polyOut) {
    polyOut = boostPolygon();
    auto polyPoints = polygonIn.getPoints();
    for (auto &point: polyPoints) {
      bg::append(polyOut, boostPoint((int)point.x(), (int)point.y()));
    }
  }

  void frBox2Poly(frBox &boxIn, boostPolygon &polyOut) {
    polyOut = boostPolygon();
    bg::append(polyOut, boostPoint(boxIn.left(), boxIn.bottom()));
    bg::append(polyOut, boostPoint(boxIn.left(), boxIn.top()));
    bg::append(polyOut, boostPoint(boxIn.right(), boxIn.top()));
    bg::append(polyOut, boostPoint(boxIn.right(), boxIn.bottom()));
    bg::append(polyOut, boostPoint(boxIn.left(), boxIn.bottom()));
  }

  void frBox2Rectangle(frBox &boxIn, Rectangle &rectOut) {
    rectOut = Rectangle(boxIn.left(), boxIn.bottom(), boxIn.right(), boxIn.top());
  }

  void frRect2Rectangle(frRect &rectIn, Rectangle &rectOut) {
    frBox rectInBox;
    rectIn.getBBox(rectInBox);
    rectOut = Rectangle(rectInBox.left(), rectInBox.bottom(), rectInBox.right(), rectInBox.top());
  }

  void frBlockage2Poly(frBlockage &blockageIn, boostPolygon &polyOut) {
    auto points = blockageIn.getPoints();
    polyOut = boostPolygon();
    for (auto point: points) {
      bg::append(polyOut, boostPoint(point.x(), point.y()));
    }
  }

  // Use before checking overlapping
  double frBox2BoxDist(frBox &box1, frBox &box2) {
    frCoord xDist = std::max(0, std::max(box1.left(), box2.left()) - std::min(box1.right(), box2.right()) );
    frCoord yDist = std::max(0, std::max(box1.bottom(), box2.bottom()) - std::min(box1.top(), box2.top()));
    return std::sqrt(xDist * xDist + yDist * yDist);
  }

  double frBox2BoxDist(box_t &box1, box_t &box2) {
    frCoord llx1, lly1, urx1, ury1, llx2, lly2, urx2, ury2;
    llx1 = bg::get<bg::min_corner, 0>(box1);
    lly1 = bg::get<bg::min_corner, 1>(box1);
    urx1 = bg::get<bg::max_corner, 0>(box1);
    ury1 = bg::get<bg::max_corner, 1>(box1);
    llx2 = bg::get<bg::min_corner, 0>(box2);
    lly2 = bg::get<bg::min_corner, 1>(box2);
    urx2 = bg::get<bg::max_corner, 0>(box2);
    ury2 = bg::get<bg::max_corner, 1>(box2);
    frCoord xDist = std::max(0, std::max(llx1, llx2) - std::min(urx1, urx2));
    frCoord yDist = std::max(0, std::max(lly1, lly2) - std::min(ury1, ury2));
    return std::sqrt(xDist * xDist + yDist * yDist);
  }

  bool isSamePoint(boostPoint &pt1, boostPoint &pt2) {
    auto x1 = bg::get<0>(pt1);
    auto y1 = bg::get<1>(pt1);
    auto x2 = bg::get<0>(pt2);
    auto y2 = bg::get<1>(pt2);
    return (x1 == x2 && y1 == y2);
  }

  frCoord getEdgeWidth(boostEdge &edge) {
    auto point1 = edge.first;
    auto point2 = edge.second;
    auto pt1X = bg::get<0>(point1);
    auto pt1Y = bg::get<1>(point1);
    auto pt2X = bg::get<0>(point2);
    auto pt2Y = bg::get<1>(point2);

    if (pt1X == pt2X) {
      return (std::abs(pt2Y - pt1Y));
    } else if (pt1Y == pt2Y) {
      return (std::abs(pt2X - pt1X));
    } else {
      std::cout << "Warning: edge is not orthogonal\n";
      return 0;
    }

  }

  void polyCovering(boostPolygon &polygon, frCollection<frRect> &rects) {
    // debug
    // std::cout << "  start polyCovering\n" << std::flush;
    frCollection<frCoord> x, y;
    frCollection<boostPoint> points;
    std::set<frCoord> xIntv, yIntv;
    std::vector<frCoord> xTick, yTick;
    std::set<std::pair<int,int> > HSliceStart, VSliceStart;
    std::vector<std::vector<bool> > isOccupied;
    std::set<std::pair<int, int> > maxHSlice, maxVSlice;
    std::set<std::tuple<int, int, int, int> > maxRectSet;
    std::pair<int, int> tmpPoint;

    // exterior ring
    auto ring = bg::exterior_ring(polygon);
    for (auto it = ring.begin(); it != ring.end(); ++it) {
      points.push_back(*it);
    }
    // interior rings
    BOOST_FOREACH(auto & ring, bg::interior_rings(polygon)) {
      for (auto it = ring.begin(); it != ring.end(); ++it) {
        points.push_back(*it);
      }
    }

    // push_back to x and y
    for (auto &point: points) {
      x.push_back(point.get<0>());
      y.push_back(point.get<1>());
    }
    xIntv = std::set<frCoord>(x.begin(), x.end());
    yIntv = std::set<frCoord>(y.begin(), y.end());

    std::copy(xIntv.begin(), xIntv.end(), std::back_inserter(xTick));
    std::copy(yIntv.begin(), yIntv.end(), std::back_inserter(yTick));

    // std::cout << "    polyCovering: num points = " << points.size() << std::endl;
    // std::cout << "    polyCovering: |xTick| = " << xTick.size() << ", |yTick| = " << yTick.size() << std::endl;

    isOccupied = std::vector<std::vector<bool> >(xTick.size() - 1, std::vector<bool>(yTick.size() - 1, false));

    // debug
    // std::cout << "    start building k-map\n" << std::flush;
    // build k-map
    for (int i = 0; i < (int)xTick.size() - 1; i++) {
      for (int j = 0; j < (int)yTick.size() - 1; j++) {
        frCoord tempX = (xTick.at(i) + xTick.at(i + 1)) / 2;
        frCoord tempY = (yTick.at(j) + yTick.at(j + 1)) / 2;
        boostPoint tempPt(tempX, tempY);
        if (bg::covered_by(tempPt, polygon)) {
          isOccupied[i][j] = true;
        }
      }
    }

    // printBoolMatrix(isOccupied);

    // debug
    // std::cout << "    start building slices\n" << std::flush;
    // build slices
    for (int i = 0; i < (int)isOccupied.size(); i++) {
      for (int j = 0; j < (int)isOccupied[i].size(); j++) {
        if (isOccupied[i][j] == false) {
          continue;
        } else {
          // horizontal
          // debug
          // std::cout << "      H slices\n" << std::flush;
          if (getMaxHSlice(i, j, isOccupied, maxHSlice, tmpPoint)) {
            // std::cout << "        getting max H rect\n" << std::flush;
            // std::cout << "          starting point = (" << tmpPoint.first << ", " << tmpPoint.second << ")\n";
            getMaxHRect(tmpPoint, isOccupied, maxHSlice, maxRectSet);
            // std::cout << "        finish getting max H rect\n" << std::flush;
          }
          // vertical
          // debug
          // std::cout << "      V slices\n" << std::flush;
          if (getMaxVSlice(i, j, isOccupied, maxVSlice, tmpPoint)) {
            // std::cout << "        getting max V rect\n" << std::flush;
            // std::cout << "          starting point = (" << tmpPoint.first << ", " << tmpPoint.second << ")\n";
            getMaxVRect(tmpPoint, isOccupied, maxVSlice, maxRectSet);
            // std::cout << "        finish getting max V rect\n" << std::flush;
          }
        }
      }
    }

    // debug
    // std::cout << "    start outputing result\n" << std::flush;
    // std::cout << "      num rects = " << maxRectSet.size() << std::endl;
    for (auto it = maxRectSet.begin(); it != maxRectSet.end(); ++it) {
      auto &tmpTuple = *it;
      // std::cout << "       (" << get<0>(tmpTuple) << ", " << get<1>(tmpTuple) << ") -- ("
      //           << get<2>(tmpTuple) << ", " << get<3>(tmpTuple) << ")" << std::endl;
      frBox tmpBox;
      frRect tmpRect;
      tmpBox.set(xTick[get<0>(tmpTuple)], yTick[get<1>(tmpTuple)], xTick[get<2>(tmpTuple) + 1], yTick[get<3>(tmpTuple) + 1]);
      tmpRect.setBBox(tmpBox);
      rects.push_back(tmpRect);
    }

  }

  // every rect in maxRectSet is maximum among all rects seen so far
  void maintainMaxRects(std::tuple<int, int, int, int> &newRect, std::set<std::tuple<int, int, int, int> > &maxRectSet) {
    // std::cout << "processNewRect of (" << std::get<0>(newRect) << ", " << std::get<1>(newRect) << ") -- ("
    //           << std::get<2>(newRect) << ", " << std::get<3>(newRect) << ")" << std::endl;

    boostBox tmpNewRect(boostPoint(std::get<0>(newRect), std::get<1>(newRect)), boostPoint(std::get<2>(newRect), std::get<3>(newRect)));

    if (maxRectSet.size() == 0) {
      // std::cout << "@@@debug@@@: insert (" << std::get<0>(newRect) << ", " << std::get<1>(newRect)
      //           << ") -- (" << std::get<2>(newRect) << "," << std::get<3>(newRect) << ")\n";

      maxRectSet.insert(newRect);
      return;
    }

    if (maxRectSet.find(newRect) != maxRectSet.end()) {
      return;
    }

    std::vector<std::tuple<int, int, int, int> > coveredRects;
    // isSkip = false;
    for (auto it = maxRectSet.begin(); it != maxRectSet.end(); it++) {
      boostBox tmpRect(boostPoint(std::get<0>(*it), std::get<1>(*it)), boostPoint(std::get<2>(*it), std::get<3>(*it)));
      // std::cout << "    checking against (" << std::get<0>(*it) << ", " << std::get<1>(*it) << ") -- ("
            // << std::get<2>(*it) << ", " << std::get<3>(*it) << ")" << std::endl;
      if (bg::covered_by(tmpNewRect, tmpRect)) {
        // std::cout << "@@@debug@@@: covered_by (" << std::get<0>(*it) << ", " << std::get<1>(*it)
        //           << ") -- (" << std::get<2>(*it) << "," << std::get<3>(*it) << ")\n";

        // newRect is strictly within some existing Rect
        return;
      } else if (bg::covered_by(tmpRect, tmpNewRect)) {
        coveredRects.push_back(*it);
        // maxRectSet.erase(it);
        // maxRectSet.insert(newRect);
        // break;
      }
    }

    if (!coveredRects.empty()) {
      for (int i = 0; i < (int)coveredRects.size(); i++) {
        auto it = maxRectSet.find(coveredRects[i]);
        // std::cout << "@@@debug@@@: erase (" << std::get<0>(coveredRects[i]) << ", " << std::get<1>(coveredRects[i])
        //         << ") -- (" << std::get<2>(coveredRects[i]) << "," << std::get<3>(coveredRects[i]) << ")\n";

        maxRectSet.erase(it);
      }

    }
    // std::cout << "@@@debug@@@: insert (" << std::get<0>(newRect) << ", " << std::get<1>(newRect)
    //             << ") -- (" << std::get<2>(newRect) << "," << std::get<3>(newRect) << ")\n";

    maxRectSet.insert(newRect);
    return;
  }

  bool processNewRect(std::tuple<int, int, int, int> &newRect, std::set<std::tuple<int, int, int, int> > &maxRectSet) {
    // std::cout << "processNewRect of (" << std::get<0>(newRect) << ", " << std::get<1>(newRect) << ") -- ("
    //           << std::get<2>(newRect) << ", " << std::get<3>(newRect) << ")" << std::endl;

    boostBox tmpNewRect(boostPoint(std::get<0>(newRect), std::get<1>(newRect)), boostPoint(std::get<2>(newRect), std::get<3>(newRect)));
    bool isChanged = true;
    if (maxRectSet.find(newRect) != maxRectSet.end()) {
      return false;
    }
    if (maxRectSet.size() == 0) {
      // std::cout << "@@@debug@@@: insert (" << std::get<0>(newRect) << ", " << std::get<1>(newRect)
      //           << ") -- (" << std::get<2>(newRect) << "," << std::get<3>(newRect) << ")\n";
      maxRectSet.insert(newRect);

      return true;
    }

    isChanged = false;
    std::vector<std::tuple<int, int, int, int> > coveredRects;
    // isSkip = false;
    for (auto it = maxRectSet.begin(); it != maxRectSet.end(); it++) {
      boostBox tmpRect(boostPoint(std::get<0>(*it), std::get<1>(*it)), boostPoint(std::get<2>(*it), std::get<3>(*it)));
      // std::cout << "    checking against (" << std::get<0>(*it) << ", " << std::get<1>(*it) << ") -- ("
            // << std::get<2>(*it) << ", " << std::get<3>(*it) << ")" << std::endl;
      if (bg::covered_by(tmpNewRect, tmpRect)) {
        // newRect is strictly within some existing Rect
        return false;
      } else if (bg::covered_by(tmpRect, tmpNewRect)) {
        coveredRects.push_back(*it);
        // maxRectSet.erase(it);
        // maxRectSet.insert(newRect);
        isChanged = true;
        break;
      }
    }
    if (isChanged == true) {
      for (int i = 0; i < (int)coveredRects.size(); i++) {
        auto it = maxRectSet.find(coveredRects[i]);
        maxRectSet.erase(it);
      }
      return true;
    }

    return false;
  }

  bool getMaxHSlice(int xIdx, int yIdx, std::vector<std::vector<bool> > &isOccupied, std::set<std::pair<int, int> > &sliceSet, std::pair<int, int> &retPt) {
    // std::cout << "getMaxHSlice" << std::endl;
    int xl, xh;
    xl = xh = xIdx;
    while (xl - 1 >= 0 && isOccupied[xl - 1][yIdx] == true) {
      xl--;
    }
    while (xh + 1 < (int)isOccupied.size() && isOccupied[xh + 1][yIdx] == true) {
      xh++;
    }
    std::pair<int, int> tmpPt = std::make_pair(xl, yIdx);
    retPt = tmpPt;
    if (sliceSet.find(tmpPt) != sliceSet.end()) {
      return false;
    } else {
      sliceSet.insert(tmpPt);
      return true;
    }
  }

  bool getMaxVSlice(int xIdx, int yIdx, std::vector<std::vector<bool> > &isOccupied, std::set<std::pair<int, int> > &sliceSet, std::pair<int, int> &retPt) {
    // std::cout << "getMaxVSlice" << std::endl;
    int yl, yh;
    yl = yh = yIdx;
    while (yl - 1 >= 0 && isOccupied[xIdx][yl - 1] == true) {
      yl--;
    }
    while (yh + 1 < (int)isOccupied[xIdx].size() && isOccupied[xIdx][yh + 1] == true) {
      yh++;
    }
    std::pair<int, int> tmpPt = std::make_pair(xIdx, yl);
    retPt = tmpPt;
    if (sliceSet.find(tmpPt) != sliceSet.end()) {
      return false;
    } else {
      sliceSet.insert(tmpPt);
      return true;
    }
  }

  void getMaxHRect(std::pair<int, int> startPt, std::vector<std::vector<bool> > &isOccupied, std::set<std::pair<int, int> > &sliceSet, std::set<std::tuple<int, int, int, int> > &maxRectSet) {
    // std::cout << "getMaxHRect start pnt = (" << startPt.first << ", " << startPt.second << ")" << std::endl;
    int xl, xh, yl, yh;
    xl = xh = startPt.first;
    yl = yh = startPt.second;
    while (xh + 1 < (int)isOccupied.size() && isOccupied[xh + 1][startPt.second] == true) {
      xh++;
    }

    // upward
    bool flag = true;
    while (flag) {
      // std::cout << "      upward\n" << std::flush;
      if (yh + 1 < (int)isOccupied[xl].size()) {
        for (int tempX = xl; tempX <= xh; tempX++) {
          if (isOccupied[tempX][yh + 1] == false) {
            flag = false;
            break;
          }
        }
        if (flag == true) {
          yh++;
        }
      } else {
        flag = false;
        break;
      }
    }

    // downward
    flag = true;
    while (flag) {
      // std::cout << "      downward\n" << std::flush;
      if (yl - 1 >= 0) {
        // yl--;
        for (int tempX = xl; tempX <= xh; tempX++) {
          if (isOccupied[tempX][yl - 1] == false) {
            flag = false;
            break;
          }
        }
        if (flag == true) {
          yl--;
        }
      } else {
        flag = false;
        break;
      }
    }
    // std::cout << "xl = " << xl << ", " << "xh = " << xh << std::endl;
    // std::cout << "yl = " << yl << ", yh = " << yh << std::endl;
    std::tuple<int, int, int, int> tmpTuple = std::make_tuple(xl, yl, xh, yh);
    // printRect(tmpTuple);
    maintainMaxRects(tmpTuple, maxRectSet);
    // return (maintainRects(tmpTuple, maxRectSet));

  }

  void getMaxVRect(std::pair<int, int> startPt, std::vector<std::vector<bool> > &isOccupied, std::set<std::pair<int, int> > &sliceSet, std::set<std::tuple<int, int, int, int> > &maxRectSet) {
    // std::cout << "getMaxVRect start pnt = (" << startPt.first << ", " << startPt.second << ")" << std::endl;
    int xl, xh, yl, yh;
    xl = xh = startPt.first;
    yl = yh = startPt.second;
    while (yh + 1 < (int)isOccupied[startPt.first].size() && isOccupied[startPt.first][yh + 1]) {
      yh++;
    }

    // upward
    bool flag = true;
    while (flag) {
      if (xh + 1 < (int)isOccupied.size()) {
        // xh++;
        for (int tempY = yl; tempY <= yh; tempY++) {
          if (isOccupied[xh + 1][tempY] == false) {
            flag = false;
            break;
          }
        }
        if (flag == true) {
          xh++;
        }
      } else {
        flag = false;
      }
    }
    // downward
    flag = true;
    while (flag) {
      if (xl - 1 >= 0) {
        // xl--;
        for (int tempY = yl; tempY <= yh; tempY++) {
          if (isOccupied[xl - 1][tempY] == false) {
            flag = false;
            break;
          }
        }
        if (flag == true) {
          xl--;
        }
      } else {
        flag = false;
      }
    }

    // std::cout << "xl = " << xl << ", " << "xh = " << xh << std::endl;
    // std::cout << "yl = " << yl << ", " << "yh = " << yh << std::endl;
    std::tuple<int, int, int, int> tmpTuple = std::make_tuple(xl, yl, xh, yh);
    maintainMaxRects(tmpTuple, maxRectSet);
    // return (processNewRect(tmpTuple, maxRectSet));

  }

  void getPolyWithHole(const std::vector<Point> &vertices, Polygon &outline, std::vector<Polygon> &holes) {
    typedef boost::icl::interval_map<int, int> IntvMap;
    std::vector<Polygon> polys;
    std::vector<std::pair<Point, Point> > edges;
    std::vector<bool> validEdges;
    std::map<int, IntvMap> horzIntv2Edge, vertIntv2Edge;
    for (int i = 0; i < (int)vertices.size(); ++i) {
      Point currPt = vertices[i];
      Point nextPt = vertices[(i + 1) % vertices.size()];
      edges.push_back(std::make_pair(currPt, nextPt));
      validEdges.push_back(true);

      int edgeIdx = -1;
      bool isHorizontal = true;
      auto itResHorz = horzIntv2Edge[nextPt.y()].equal_range(boost::icl::interval<int>::closed(nextPt.x(), nextPt.x()));
      int horzCnt = 0;
      for (auto it = itResHorz.first; it != itResHorz.second; ++it) {
        if (horzCnt > 0) {
          std::cout << "Error: more than one interval found\n";
          break;
        }
        if ((it->second - 1) > edgeIdx && validEdges[(it->second  - 1)] == true) {
          edgeIdx = it->second - 1;
          isHorizontal = true;
        }
        horzCnt++;
      }
      // vert
      auto itResVert = vertIntv2Edge[nextPt.x()].equal_range(boost::icl::interval<int>::closed(nextPt.y(), nextPt.y()));
      int vertCnt = 0;
      for (auto it = itResVert.first; it != itResVert.second; ++it) {
        if (vertCnt > 0) {
          std::cout << "Error: more than one interval found\n";
          break;
        }
        if ((it->second  - 1) > edgeIdx && validEdges[(it->second  - 1)] == true) {
          edgeIdx = it->second - 1;
          isHorizontal = false;
        }
        vertCnt++;
      }
      // get poly
      if (edgeIdx == -1) {
        ;
      } else if ((i - edgeIdx) > 2) {
        std::vector<Point> vertices;
        int edgeCnt = 0;
        for (int currEdgeIdx = edgeIdx; currEdgeIdx <= i; ++currEdgeIdx) {
          if (validEdges[currEdgeIdx] == false) {
            continue;
          }
          edgeCnt++;
          vertices.push_back(edges[currEdgeIdx].second);
          if (currEdgeIdx == edgeIdx) {
            if (nextPt == edges[currEdgeIdx].first) {
              // cout << "  invalidate " << currEdgeIdx << endl;
              validEdges[currEdgeIdx] = false;
            } else {
              Point tmpEndPoint = edges[currEdgeIdx].second;
              edges[currEdgeIdx].second = nextPt;
              if (isHorizontal) {
                horzIntv2Edge[nextPt.y()] -= std::make_pair(boost::icl::interval<int>::right_open(std::min(tmpEndPoint.x(), nextPt.x()), std::max(tmpEndPoint.x(), nextPt.x())), edgeIdx);
                // cout << "  erase y = " << nextPt.y() << ", x = [" << min(tmpEndPoint.x(), nextPt.x()) << ", " << max(tmpEndPoint.x(), nextPt.x()) << "] edgeIdx = " << edgeIdx << "\n";
              } else {
                vertIntv2Edge[nextPt.x()] -= std::make_pair(boost::icl::interval<int>::right_open(std::min(tmpEndPoint.y(), nextPt.y()), std::max(tmpEndPoint.y(), nextPt.y())), edgeIdx);
                // cout << "  erase x = " << nextPt.x() << ", x = [" << min(tmpEndPoint.y(), nextPt.y()) << ", " << max(tmpEndPoint.y(), nextPt.y()) << "] edgeIdx = " << edgeIdx << "\n";
              }
            }
          } else {
            // cout << "  invalidate " << currEdgeIdx << endl;
            validEdges[currEdgeIdx] = false;
          }
        }
        if (edgeCnt > 2) {
          // cout << "poly startIdx = " << edgeIdx << ", endIdx = " << i << endl;
          Polygon tmpPoly;
          Point pt1, pt2, pt3;
          pt1 = vertices.back();
          pt2 = vertices.front();
          pt3 = vertices[vertices.size() - 2];
          // cout << "xxx2\n";

          if ((pt1.x() == pt2.x() && pt2.x() == pt3.x()) ||
              (pt1.y() == pt2.y() && pt2.y() == pt3.y())) {
            vertices.pop_back();
          }
          set_points(tmpPoly, vertices.begin(), vertices.end());
          polys.push_back(tmpPoly);
        }
      } else {
        // cout << "erase startIdx = " << edgeIdx << ", endIdx = " << i << endl;
        for (int currEdgeIdx = edgeIdx; currEdgeIdx <= i; ++currEdgeIdx) {
          if (validEdges[currEdgeIdx] == false) {
            continue;
          }
          if (currEdgeIdx == edgeIdx) {
            if (nextPt == edges[currEdgeIdx].first) {
              validEdges[currEdgeIdx] = false;
            } else {
              edges[currEdgeIdx].second = nextPt;
            }
          } else {
            validEdges[currEdgeIdx] = false;
          }
        }
      }

      // insert to interval map
      if (edgeIdx == -1) {
        if (currPt.x() == nextPt.x()) {
          auto edgeIntv = boost::icl::interval<int>::closed(std::min(currPt.y(), nextPt.y()), std::max(currPt.y(), nextPt.y()));
          vertIntv2Edge[nextPt.x()] += std::make_pair(edgeIntv, i + 1);
          // cout << "add x = " << currPt.x() << ", y = [" << edgeIntv.lower() << ", " << edgeIntv.upper() << "]\n";
        }
        if (currPt.y() == nextPt.y()) {
          auto edgeIntv = boost::icl::interval<int>::closed(std::min(currPt.x(), nextPt.x()), std::max(currPt.x(), nextPt.x()));
          horzIntv2Edge[nextPt.y()] += std::make_pair(edgeIntv, i + 1);
          // cout << "add y = " << currPt.y() << ", x = [" << edgeIntv.lower() << ", " << edgeIntv.upper() << "]\n";
        }
      }

    }

    std::vector<Point> tmpPts;
    for (int currEdgeIdx = 0; currEdgeIdx < int(edges.size()); ++currEdgeIdx) {
      if (validEdges[currEdgeIdx] == false) {
        continue;
      }
      tmpPts.push_back(edges[currEdgeIdx].second);
      // std::cout << "pushing (" << tmpPts.back().x() << ", " << tmpPts.back().y() << "\n";
    }

    for (int i = 0; i < (int)polys.size() - 1; ++i) {
      holes.push_back(polys[i]);
    }
    if (polys.size() > 0) {
      outline = polys.back();
    }

  }

  bool isColinear(const Point &pt1, const Point &pt2, const Point &pt3) {
    return ((pt1.x() == pt2.x() && pt2.x() == pt3.x()) ||
            (pt1.y() == pt2.y() && pt2.y() == pt3.y()));
  }

  void getNonColinearVertex(const std::vector<Point> &in, std::vector<Point> &out) {
    int inSize = in.size();
    int cornerIdx = -1;
    int startIdx, endIdx;
    Point lastCorner;
    std::vector<int> cornerIdxs;
    if (inSize < 3) {
      out = in;
      return;
    }
    Point tmpPt1 = in[0];
    Point tmpPt2 = in[1];
    Point tmpPt3;
    for (int i = 2; i < inSize; ++i) {
      tmpPt3 = in[i];
      if (!isColinear(tmpPt1, tmpPt2, tmpPt3)) {
        cornerIdx = i - 1;
        break;
      }
    }
    if (cornerIdx == -1) {
      out.push_back(in.front());
      out.push_back(in.back());
      return;
    } else {
      cornerIdxs.push_back(cornerIdx);
      lastCorner = in[cornerIdx];
    }
    //
    for (int i = cornerIdx+1; i < cornerIdx + inSize; ++i) {
      startIdx = i % inSize;
      tmpPt1 = in[startIdx];
      // find next corner
      for (int j = i + 1; j <= cornerIdx + inSize; ++j) {
        endIdx = j % inSize;
        tmpPt2 = in[endIdx];
        if (!isColinear(lastCorner, tmpPt1, tmpPt2)) {
          int lastCornerIdx = (endIdx + inSize - 1) % inSize;
          cornerIdxs.push_back(lastCornerIdx);
          lastCorner = in[lastCornerIdx];
          i = j-1;
          break;
        }
      }
    }
    for (int i = 0; i < (int)cornerIdxs.size(); ++i) {
      out.push_back(in[cornerIdxs[i]]);
    }
    return;

  }

  void getPolyWithHole_new(const Polygon &polyIn, std::vector<Polygon> &polys) {
    using namespace boost::polygon::operators;
    using PolygonX = boost::polygon::polygon_90_with_holes_data<int>;
    using PolygonSetData = boost::polygon::polygon_90_set_data<int>;
    PolygonSetData psd;
    psd += polyIn;
    std::vector<PolygonX> psx;
    psd.get(psx);

    for (auto &poly: psx) {
      Polygon outlinePoly;
      std::vector<Point> outlinePolyVtx;
      for (auto it = poly.begin(); it != poly.end(); it++) {
        outlinePolyVtx.push_back(*it);
      }
      boost::polygon::set_points(outlinePoly, outlinePolyVtx.rbegin(), outlinePolyVtx.rend());
      for (auto it1 = poly.begin_holes(); it1 != poly.end_holes(); it1++) {
        std::vector<Point> vtx;
        Polygon holePoly;
        for (auto it2 = (*it1).begin(); it2 != (*it1).end(); it2++) {
          vtx.push_back(*it2);
        }
        boost::polygon::set_points(holePoly, vtx.rbegin(), vtx.rend());
        polys.push_back(holePoly);
      }
      polys.push_back(outlinePoly);
    }
  }

  void getPolyWithHole(const Polygon &polyIn, std::vector<Polygon> &polys) {
    typedef boost::icl::interval_map<int, int> IntvMap;
    std::vector<Point> origVertices, vertices;
    for (auto ptIt = begin_points(polyIn); ptIt != end_points(polyIn); ++ptIt) {
      origVertices.push_back(*ptIt);
    }

    // getNonColinearVertex(origVertices, vertices);
    vertices = origVertices;
    std::vector<std::pair<Point, Point> > edges;
    std::vector<bool> validEdges;
    std::map<int, IntvMap> horzIntv2Edge, vertIntv2Edge;
    for (int i = 0; i < (int)vertices.size(); ++i) {
      Point currPt = vertices[i];
      Point nextPt = vertices[(i + 1) % vertices.size()];
      edges.push_back(std::make_pair(currPt, nextPt));
      validEdges.push_back(true);

      int edgeIdx = -1;
      bool isHorizontal = true;
      auto itResHorz = horzIntv2Edge[nextPt.y()].equal_range(boost::icl::interval<int>::closed(nextPt.x(), nextPt.x()));
      int horzCnt = 0;
      for (auto it = itResHorz.first; it != itResHorz.second; ++it) {
        if (horzCnt > 0) {
          std::cout << "Error: more than one interval found\n";
          break;
        }
        if ((it->second - 1) > edgeIdx && validEdges[(it->second  - 1)] == true) {
          edgeIdx = it->second - 1;
          isHorizontal = true;
        }
        horzCnt++;
      }
      // vert
      auto itResVert = vertIntv2Edge[nextPt.x()].equal_range(boost::icl::interval<int>::closed(nextPt.y(), nextPt.y()));
      int vertCnt = 0;
      for (auto it = itResVert.first; it != itResVert.second; ++it) {
        if (vertCnt > 0) {
          std::cout << "Error: more than one interval found\n";
          break;
        }
        if ((it->second  - 1) > edgeIdx && validEdges[(it->second  - 1)] == true) {
          edgeIdx = it->second - 1;
          isHorizontal = false;
        }
        vertCnt++;
      }
      // get poly
      if (edgeIdx == -1) {
        ;
      } else if ((i - edgeIdx) > 2) {
        // std::cout << "push new poly\n";
        std::vector<Point> vertices;
        int edgeCnt = 0;
        for (int currEdgeIdx = edgeIdx; currEdgeIdx <= i; ++currEdgeIdx) {
          if (validEdges[currEdgeIdx] == false) {
            continue;
          }
          if (vertices.size() < 2) {
            edgeCnt++;
            vertices.push_back(edges[currEdgeIdx].second);
            // std::cout << "pushing (" << vertices.back().x() / 2000.0 << ", " << vertices.back().y() / 2000.0 << ")\n";
          } else if (!isColinear(vertices.back(), vertices[vertices.size() - 2], edges[currEdgeIdx].second)) {
            edgeCnt++;
            vertices.push_back(edges[currEdgeIdx].second);
            // std::cout << "pushing (" << vertices.back().x() / 2000.0 << ", " << vertices.back().y() / 2000.0 << ")\n";
          } else {
            // std::cout << "pop (" << vertices.back().x() / 2000.0 << ", " << vertices.back().y() / 2000.0 << ")\n";
            vertices.pop_back();
            vertices.push_back(edges[currEdgeIdx].second);
            // std::cout << "push (" << vertices.back().x() / 2000.0 << ", " << vertices.back().y() / 2000.0 << ")\n";
          }
          if (currEdgeIdx == edgeIdx) {
            if (nextPt == edges[currEdgeIdx].first) {
              // cout << "  invalidate " << currEdgeIdx << endl;
              validEdges[currEdgeIdx] = false;
            } else {
              Point tmpEndPoint = edges[currEdgeIdx].second;
              edges[currEdgeIdx].second = nextPt;
              if (isHorizontal) {
                horzIntv2Edge[nextPt.y()] -= std::make_pair(boost::icl::interval<int>::right_open(std::min(tmpEndPoint.x(), nextPt.x()), std::max(tmpEndPoint.x(), nextPt.x())), edgeIdx);
                // cout << "  erase y = " << nextPt.y() << ", x = [" << min(tmpEndPoint.x(), nextPt.x()) << ", " << max(tmpEndPoint.x(), nextPt.x()) << "] edgeIdx = " << edgeIdx << "\n";
              } else {
                vertIntv2Edge[nextPt.x()] -= std::make_pair(boost::icl::interval<int>::right_open(std::min(tmpEndPoint.y(), nextPt.y()), std::max(tmpEndPoint.y(), nextPt.y())), edgeIdx);
                // cout << "  erase x = " << nextPt.x() << ", x = [" << min(tmpEndPoint.y(), nextPt.y()) << ", " << max(tmpEndPoint.y(), nextPt.y()) << "] edgeIdx = " << edgeIdx << "\n";
              }
            }
          } else {
            // cout << "  invalidate " << currEdgeIdx << endl;
            validEdges[currEdgeIdx] = false;
          }
        }
        if (edgeCnt > 2) {
          // cout << "poly startIdx = " << edgeIdx << ", endIdx = " << i << endl;
          Polygon tmpPoly;
          Point pt1, pt2, pt3;
          pt1 = vertices.back();
          pt2 = vertices.front();
          pt3 = vertices[vertices.size() - 2];
          // cout << "xxx2\n";

          if ((pt1.x() == pt2.x() && pt2.x() == pt3.x()) ||
              (pt1.y() == pt2.y() && pt2.y() == pt3.y())) {
            vertices.pop_back();
          }
          set_points(tmpPoly, vertices.begin(), vertices.end());
          polys.push_back(tmpPoly);
        }
      } else {
        // cout << "erase startIdx = " << edgeIdx << ", endIdx = " << i << endl;
        for (int currEdgeIdx = edgeIdx; currEdgeIdx <= i; ++currEdgeIdx) {
          if (validEdges[currEdgeIdx] == false) {
            continue;
          }
          if (currEdgeIdx == edgeIdx) {
            if (nextPt == edges[currEdgeIdx].first) {
              validEdges[currEdgeIdx] = false;
            } else {
              edges[currEdgeIdx].second = nextPt;
            }
          } else {
            validEdges[currEdgeIdx] = false;
          }
        }
      }

      // insert to interval map
      if (edgeIdx == -1) {
        // vertical
        if (currPt.x() == nextPt.x()) {
          // fix bug for the case where the new interval intersects with old interval
          auto edgeIntv = boost::icl::interval<int>::closed(std::min(currPt.y(), nextPt.y()), std::max(currPt.y(), nextPt.y()));
          auto itResVert = vertIntv2Edge[nextPt.x()].equal_range(edgeIntv);
          std::vector<std::pair<int, int> > intvPairs;
          for (auto it = itResVert.first; it != itResVert.second; ++it) {
            auto &ovlpIntv = it->first;
            it->second = 0;
            intvPairs.push_back(std::make_pair(ovlpIntv.lower(), ovlpIntv.upper()));
          }
          for (auto &intvPair: intvPairs) {
            vertIntv2Edge[nextPt.x()] -= std::make_pair(boost::icl::interval<int>::right_open(intvPair.first, intvPair.second), -i - 1);
          }
          vertIntv2Edge[nextPt.x()] += std::make_pair(edgeIntv, i + 1);
          // cout << "add x = " << currPt.x() << ", y = [" << edgeIntv.lower() << ", " << edgeIntv.upper() << "]\n";
        }
        if (currPt.y() == nextPt.y()) {
          // horizontal
          auto edgeIntv = boost::icl::interval<int>::closed(std::min(currPt.x(), nextPt.x()), std::max(currPt.x(), nextPt.x()));
          auto itResHorz = horzIntv2Edge[nextPt.y()].equal_range(edgeIntv);
          std::vector<std::pair<int, int> > intvPairs;
          for (auto it = itResHorz.first; it != itResHorz.second; ++it) {
            auto &ovlpIntv = it->first;
            it->second = 0;
            intvPairs.push_back(std::make_pair(ovlpIntv.lower(), ovlpIntv.upper()));
          }
          for (auto &intvPair: intvPairs) {
            horzIntv2Edge[nextPt.y()] -= std::make_pair(boost::icl::interval<int>::right_open(intvPair.first, intvPair.second), -i - 1);
          }
          horzIntv2Edge[nextPt.y()] += std::make_pair(edgeIntv, i + 1);
          // cout << "add y = " << currPt.y() << ", x = [" << edgeIntv.lower() << ", " << edgeIntv.upper() << "]\n";
        }
      }

    }

    // std::vector<Point> tmpPts;
    // for (int currEdgeIdx = 0; currEdgeIdx < edges.size(); ++currEdgeIdx) {
    //   if (validEdges[currEdgeIdx] == false) {
    //     continue;
    //   }
    //   tmpPts.push_back(edges[currEdgeIdx].second);
    //   // std::cout << "pushing (" << tmpPts.back().x() << ", " << tmpPts.back().y() << "\n";
    // }

    // for (int i = 0; i < (int)polys.size() - 1; ++i) {
    //   holes.push_back(polys[i]);
    // }
    // if (polys.size() > 0) {
    //   outline = polys.back();
    // }

  }

}


//=======================================================================
// Copyright 2014-2015 David Simmons-Duffin.
// Distributed under the MIT License.
// (See accompanying file LICENSE or copy at
//  http://opensource.org/licenses/MIT)
//=======================================================================

#include <iostream>
#include <ostream>
#include <sstream>
#include "omp.h"
#include "boost/archive/text_oarchive.hpp"
#include "boost/archive/text_iarchive.hpp"
//Tweak to allow Ubuntu-14.04/gcc-4.8.4 and similar environments to compile
#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/filesystem.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS
#include "boost/filesystem/fstream.hpp"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "SDPSolver.h"
#include "serialize.h"
#include "Timers.h"

using boost::filesystem::path;
using boost::posix_time::time_duration;
using boost::posix_time::microseconds;
using std::cout;

ostream& operator<<(ostream& os, const SDPSolverParameters& p) {
  os << std::boolalpha;
  os << "maxIterations                = " << p.maxIterations                << endl;
  os << "maxRuntime                   = " << p.maxRuntime                   << endl;
  os << "checkpointInterval           = " << p.checkpointInterval           << endl;
  os << "noFinalCheckpoint            = " << p.noFinalCheckpoint            << endl;
  os << "findPrimalFeasible           = " << p.findPrimalFeasible           << endl;
  os << "findDualFeasible             = " << p.findDualFeasible             << endl;
  os << "detectPrimalFeasibleJump     = " << p.detectPrimalFeasibleJump     << endl;
  os << "detectDualFeasibleJump       = " << p.detectDualFeasibleJump       << endl;
  os << "precision(actual)            = " << p.precision << "(" << mpf_get_default_prec() << ")" << endl;
  os << "maxThreads(using)            = " << p.maxThreads << "(" << omp_get_max_threads() << ")" << endl;
  os << "dualityGapThreshold          = " << p.dualityGapThreshold          << endl;
  os << "primalErrorThreshold         = " << p.primalErrorThreshold         << endl;
  os << "dualErrorThreshold           = " << p.dualErrorThreshold           << endl;
  os << "initialMatrixScalePrimal     = " << p.initialMatrixScalePrimal     << endl;
  os << "initialMatrixScaleDual       = " << p.initialMatrixScaleDual       << endl;
  os << "feasibleCenteringParameter   = " << p.feasibleCenteringParameter   << endl;
  os << "infeasibleCenteringParameter = " << p.infeasibleCenteringParameter << endl;
  os << "stepLengthReduction          = " << p.stepLengthReduction          << endl;
  os << "choleskyStabilizeThreshold   = " << p.choleskyStabilizeThreshold   << endl;
  os << "maxComplementarity           = " << p.maxComplementarity           << endl;
  return os;
}

ostream &operator<<(ostream& os, const SDPSolverTerminateReason& r) {
  switch (r) {
  case PrimalDualOptimal:
    os << "found primal-dual optimal solution";
    break;
  case PrimalFeasible:
    os << "found primal feasible solution";
    break;
  case DualFeasible:
    os << "found dual feasible solution";
    break;
  case PrimalFeasibleJumpDetected:
    os << "primal feasible jump detected";
    break;
  case DualFeasibleJumpDetected:
    os << "dual feasible jump detected";
    break;
  case MaxIterationsExceeded:
    os << "maxIterations exceeded";
    break;
  case MaxRuntimeExceeded:
    os << "maxRuntime exceeded";
    break;
  case MaxComplementarityExceeded:
    os << "maxComplementarity exceeded";
    break;
  }
  return os;
}

void SDPSolver::printHeader() {
  cout << "\n     time      mu        P-obj       D-obj      gap         P-err       D-err      P-step   D-step   beta  dim/stabilized\n";
  cout << "-------------------------------------------------------------------------------------------------------------------------\n";
}

void SDPSolver::printIteration(int iteration,
                               Real mu,
                               Real primalStepLength,
                               Real dualStepLength,
                               Real betaCorrector) {
  time_duration td(microseconds(timers["Solver runtime"].elapsed().wall)/1000);
  std::stringstream ss;
  ss << td;
  gmp_fprintf(stdout,
              "%3d  %s  %-8.1Fe %-+11.2Fe %-+11.2Fe %-9.2Fe  %-+10.2Fe  %-+10.2Fe  %-8.3Fg %-8.3Fg %-4.2Fg  %d/%d",
              iteration,
              ss.str().substr(0, 8).c_str(),
              mu.get_mpf_t(),
              primalObjective.get_mpf_t(),
              dualObjective.get_mpf_t(),
              dualityGap.get_mpf_t(),
              primalError.get_mpf_t(),
              dualError.get_mpf_t(),
              primalStepLength.get_mpf_t(),
              dualStepLength.get_mpf_t(),
              betaCorrector.get_mpf_t(),
              static_cast<int>(sdp.dualObjective.size()),
              Q.rows);
  cout << endl;
}

void backupCheckpointFile(path const& checkpointFile) {
  path backupFile(checkpointFile);
  backupFile.replace_extension(".ck.bk");
  cout << "Backing up checkpoint to: " << backupFile << endl;
  copy_file(checkpointFile, backupFile, boost::filesystem::copy_option::overwrite_if_exists);
}

void SDPSolver::saveCheckpoint(const path &checkpointFile) {
  if (exists(checkpointFile))
    backupCheckpointFile(checkpointFile);
  boost::filesystem::ofstream ofs(checkpointFile);
  boost::archive::text_oarchive ar(ofs);
  cout << "Saving checkpoint to    : " << checkpointFile << endl;
  boost::serialization::serializeSDPSolverState(ar, x, X, y, Y);
  timers["Last checkpoint"].start();
}

void SDPSolver::loadCheckpoint(const path &checkpointFile) {
  boost::filesystem::ifstream ifs(checkpointFile);
  boost::archive::text_iarchive ar(ifs);
  cout << "Loading checkpoint from : " << checkpointFile << endl;
  boost::serialization::serializeSDPSolverState(ar, x, X, y, Y);
}

void SDPSolver::saveSolution(const SDPSolverTerminateReason terminateReason, const path &outFile) {
  boost::filesystem::ofstream ofs(outFile);
  float runtime = static_cast<float>(timers["Solver runtime"].elapsed().wall)/1000000000;
  cout << "Saving solution to      : " << outFile << endl;
  ofs.precision(static_cast<int>(primalObjective.get_prec() * 0.31 + 5));
  ofs << "terminateReason = \"" << terminateReason << "\";\n";
  ofs << "primalObjective = " << primalObjective   << ";\n";
  ofs << "dualObjective   = " << dualObjective     << ";\n";
  ofs << "dualityGap      = " << dualityGap        << ";\n";
  ofs << "primalError     = " << primalError       << ";\n";
  ofs << "dualError       = " << dualError         << ";\n";
  ofs << "runtime         = " << runtime           << ";\n";
  ofs << "y = " << y << ";\n";
  // ofs << "Y = " << Y << ";\n";
  ofs << "x = " << x << ";\n";
  // ofs << "X = " << X << ";\n";
}


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

#include <data_tools/gsf_data.h>

#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/date_time.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS
#include <gsf.h>
#include <data_tools/lat_long_utm.h>

using namespace std;

namespace gsf_data {

using namespace std_data;
using namespace csv_data;

void match_sound_speeds(gsf_mbes_ping::PingsT& pings, gsf_sound_speed::SpeedsT& speeds)
{

    std::stable_sort(pings.begin(), pings.end(), [](const gsf_mbes_ping& ping1, const gsf_mbes_ping& ping2) {
        return ping1.time_stamp_ < ping2.time_stamp_;
    });
    std::stable_sort(speeds.begin(), speeds.end(), [](const gsf_sound_speed& speed1, const gsf_sound_speed& speed2) {
        return speed1.time_stamp_ < speed2.time_stamp_;
    });

    auto pos = speeds.begin();
    for (gsf_mbes_ping& ping : pings) {
        pos = std::find_if(pos, speeds.end(), [&](const gsf_sound_speed& speed) {
            return speed.time_stamp_ > ping.time_stamp_;
        });
        double ss;
        if (pos == speeds.end()) {
            ss = speeds.back().below_speed;
        }
        else {
            ss = pos->below_speed;
        }
        for (int i = 0; i < ping.travel_times.size(); ++i) {
            ping.distances.push_back(.5*ss*ping.travel_times[i]);
            //ping.distances.push_back(ss*ping.travel_times[i]);
        }
    }

}

mbes_ping::PingsT convert_matched_entries(gsf_mbes_ping::PingsT& pings, gsf_nav_entry::EntriesT& entries)
{
    mbes_ping::PingsT new_pings;

    std::stable_sort(entries.begin(), entries.end(), [](const gsf_nav_entry& entry1, const gsf_nav_entry& entry2) {
        return entry1.time_stamp_ < entry2.time_stamp_;
    });

    auto pos = entries.begin();
    for (gsf_mbes_ping& ping : pings) {
        pos = std::find_if(pos, entries.end(), [&](const gsf_nav_entry& entry) {
            return entry.time_stamp_ > ping.time_stamp_;
        });

        mbes_ping new_ping;
        new_ping.time_stamp_ = ping.time_stamp_;
        new_ping.time_string_ = ping.time_string_;
        new_ping.first_in_file_ = ping.first_in_file_;
        if (pos == entries.end()) {
            new_ping.pos_ = entries.back().pos_;
            new_ping.heading_ = entries.back().yaw_;
            new_ping.pitch_ = entries.back().pitch_;
            new_ping.roll_ = entries.back().roll_;
        }
        else {
            if (pos == entries.begin()) {
                //if (ping.long_ == 0) {
                new_ping.pos_ = pos->pos_;
                /*}
                else {
                    new_ping.pos_ = Eigen::Vector3d(ping.lat_, ping.long_, -ping.depth_);
                }*/
                if (ping.heading_ == 0) {
                    new_ping.heading_ = pos->yaw_;
                    new_ping.pitch_ = pos->pitch_;
                    new_ping.roll_ = pos->roll_;
                }
                else {
                    new_ping.heading_ = ping.heading_;
                    new_ping.pitch_ = ping.pitch_;
                    new_ping.roll_ = ping.roll_;
                }
            }
            else {
                gsf_nav_entry& previous = *(pos - 1);
                double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_);
                //if (ping.long_ == 0) {
                new_ping.pos_ = previous.pos_ + ratio*(pos->pos_ - previous.pos_);
                /*}
                else {
                    new_ping.pos_ = Eigen::Vector3d(ping.lat_, ping.long_, -ping.depth_);
                }*/
                if (ping.heading_ == 0) {
                    new_ping.heading_ = previous.yaw_ + ratio*(pos->yaw_ - previous.yaw_);
                    new_ping.pitch_ = previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_);
                    new_ping.roll_ = previous.roll_ + ratio*(pos->roll_ - previous.roll_);
                }
                else {
                    new_ping.heading_ = ping.heading_;
                    new_ping.pitch_ = ping.pitch_;
                    new_ping.roll_ = ping.roll_;
                    cout << "heading diff: " << previous.yaw_ + ratio*(pos->yaw_ - previous.yaw_) - new_ping.heading_ << endl;
                    cout << "pitch diff: " << previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_) - new_ping.pitch_ << endl;
                    cout << "roll diff: " << previous.roll_ + ratio*(pos->roll_ - previous.roll_) - new_ping.roll_ << endl;
                }
                //cout << "Ping timestamp: " << ping.time_string_ << ", pos time stamp: " << pos->time_string_ << endl;
            }
        }

        //new_ping.heading_ = 0.5*M_PI-new_ping.heading_;
        for (int i = 0; i < ping.distances.size(); ++i) {
            double d = 1.*ping.distances[i];
            if (d < 0.1) { // || ping.amplitudes[i] < 10) {
                continue;
            }
            double th = M_PI/180.*ping.beam_angles[i];
            Eigen::Vector3d p = new_ping.pos_;
            Eigen::Vector3d sensor_p(0.62, 0., 0.);
            Eigen::Vector3d beam(0., -d*sin(th), -d*cos(th)); // 0.55 comes from the log
            //std::swap(new_ping.roll_, new_ping.pitch_);
            //Eigen::Vector3d beam = ping.beams[i];

            //Eigen::Matrix3d beam_Rx = Eigen::AngleAxisd(-0.12, Eigen::Vector3d::UnitX()).matrix();
            Eigen::Matrix3d beam_Rx = Eigen::AngleAxisd(-0.06, Eigen::Vector3d::UnitX()).matrix();
            Eigen::Matrix3d beam_Ry = Eigen::AngleAxisd(-0.15, Eigen::Vector3d::UnitY()).matrix();
            Eigen::Matrix3d beam_Rz = Eigen::Matrix3d::Identity();

            Eigen::Matrix3d Rx = Eigen::AngleAxisd(0*new_ping.roll_, Eigen::Vector3d::UnitX()).matrix();
            Eigen::Matrix3d Ry = Eigen::AngleAxisd(0*new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix();
            Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix();
            Eigen::Matrix3d R = Rz*Ry*Rx;

            new_ping.beams.push_back(p + R*(sensor_p+beam_Rz*beam_Ry*beam_Rx*beam));
            //new_ping.beams.push_back(p + R*(sensor_p+beam));
        }

        new_pings.push_back(new_ping);
    }

    return new_pings;
}

mbes_ping::PingsT convert_pings(gsf_mbes_ping::PingsT& pings)
{
    mbes_ping::PingsT new_pings;

    for (gsf_mbes_ping& ping : pings) {

        mbes_ping new_ping;
        new_ping.time_stamp_ = ping.time_stamp_;
        new_ping.time_string_ = ping.time_string_;
        new_ping.first_in_file_ = ping.first_in_file_;
        new_ping.heading_ = ping.heading_;
        new_ping.pitch_ = ping.pitch_;
        new_ping.roll_ = ping.roll_;
        double easting, northing;
        string utm_zone;
        tie(northing, easting, utm_zone) = lat_long_utm::lat_long_to_UTM(ping.lat_, ping.long_);
        new_ping.pos_ = Eigen::Vector3d(easting, northing, -ping.depth_);

        //ping.pos_ = new_ping.pos_;
        int i = 0;
        for (const Eigen::Vector3d& beam : ping.beams) {
            if (beam(2) > -5. || beam(2) < -25.) {
                ++i;
                continue;
            }
            Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix();
            /*Eigen::Matrix3d Ry = Eigen::AngleAxisd(new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix();
            Eigen::Matrix3d Rx = Eigen::AngleAxisd(new_ping.roll_, Eigen::Vector3d::UnitX()).matrix();
            Eigen::Matrix3d R = Rx*Ry*Rz;*/

            // it seems it has already been compensated for pitch, roll
            new_ping.beams.push_back(new_ping.pos_ + Rz*beam);
            new_ping.back_scatter.push_back(ping.amplitudes[i]);
            ++i;
        }

        new_pings.push_back(new_ping);
    }

    return new_pings;
}

mbes_ping::PingsT convert_matched_entries(gsf_mbes_ping::PingsT& pings, csv_nav_entry::EntriesT& entries)
{
    mbes_ping::PingsT new_pings;

    std::stable_sort(entries.begin(), entries.end(), [](const csv_nav_entry& entry1, const csv_nav_entry& entry2) {
        return entry1.time_stamp_ < entry2.time_stamp_;
    });

    auto pos = entries.begin();
    for (gsf_mbes_ping& ping : pings) {
        pos = std::find_if(pos, entries.end(), [&](const csv_nav_entry& entry) {
            return entry.time_stamp_ > ping.time_stamp_;
        });

        mbes_ping new_ping;
        new_ping.time_stamp_ = ping.time_stamp_;
        new_ping.time_string_ = ping.time_string_;
        new_ping.first_in_file_ = ping.first_in_file_;
        //cout << "Ping has time: " << ping.time_string_ << ", time stamp: " << ping.time_stamp_ << endl;
        if (pos == entries.end()) {
            //cout << "Found only last entry with time: " << entries.back().time_string_ << ", time stamp: " << entries.back().time_stamp_ << endl;
            new_ping.pos_ = entries.back().pos_;
            new_ping.heading_ = entries.back().heading_;
            new_ping.pitch_ = entries.back().pitch_;
            new_ping.roll_ = entries.back().roll_;
        }
        else {
            if (pos == entries.begin()) {
                //cout << "Found only first entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl;
                new_ping.pos_ = pos->pos_;
                if (ping.heading_ == 0) {
                    new_ping.heading_ = pos->heading_;
                    new_ping.pitch_ = pos->pitch_;
                    new_ping.roll_ = pos->roll_;
                }
                else {
                    new_ping.heading_ = ping.heading_;
                    new_ping.pitch_ = ping.pitch_;
                    new_ping.roll_ = ping.roll_;
                }
            }
            else {
                //cout << "Found entry with time: " << pos->time_string_ << ", time stamp: " << pos->time_stamp_ << endl;
                csv_nav_entry& previous = *(pos - 1);
                double ratio = double(ping.time_stamp_ - previous.time_stamp_)/double(pos->time_stamp_ - previous.time_stamp_);
                new_ping.pos_ = previous.pos_ + ratio*(pos->pos_ - previous.pos_);
                if (ping.heading_ == 0) {
                    new_ping.heading_ = previous.heading_ + ratio*(pos->heading_ - previous.heading_);
                    new_ping.pitch_ = previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_);
                    new_ping.roll_ = previous.roll_ + ratio*(pos->roll_ - previous.roll_);
                }
                else {
                    new_ping.heading_ = ping.heading_;
                    new_ping.pitch_ = ping.pitch_;
                    new_ping.roll_ = ping.roll_;
                    //cout << "heading diff: " << previous.yaw_ + ratio*(pos->yaw_ - previous.yaw_) - new_ping.heading_ << endl;
                    //cout << "pitch diff: " << previous.pitch_ + ratio*(pos->pitch_ - previous.pitch_) - new_ping.pitch_ << endl;
                    //cout << "roll diff: " << previous.roll_ + ratio*(pos->roll_ - previous.roll_) - new_ping.roll_ << endl;
                }
            }
        }

        for (const Eigen::Vector3d& beam : ping.beams) {
            //new_ping.beams.push_back(new_ping.pos_ + beam);

            Eigen::Matrix3d Rx = Eigen::AngleAxisd(new_ping.roll_, Eigen::Vector3d::UnitX()).matrix();
            Eigen::Matrix3d Ry = Eigen::AngleAxisd(new_ping.pitch_, Eigen::Vector3d::UnitY()).matrix();
            Eigen::Matrix3d Rz = Eigen::AngleAxisd(new_ping.heading_, Eigen::Vector3d::UnitZ()).matrix();
            Eigen::Matrix3d R = Rz*Ry*Rx;

            new_ping.beams.push_back(new_ping.pos_ + R*beam);
        }

        new_pings.push_back(new_ping);
    }

    return new_pings;
}

} // namespace gsf_data

namespace std_data {

using namespace gsf_data;

/*
% VEHICLE_POSE_FILE VERSION 3
%
% File:          dr_pose_est.data
% Date Created:  Thu Jun  9 22:06:01 2011
% Created from:  bpslam
%
% Timing Statistics:
%     Program took 0.533 minutes to process mission.
%     Total Mission Time: 157.063 minutes.
%     Start Nav Time: 1224108800.202
%     Stop Time:  1224118223.970
%     Start Map Time: 1224109095.920
%     Loop closure detection took 0.058 minutes to process.
%     Particle Weighting took -0.000 minutes to process.
%     GP Raytracing took 0.000 minutes to process.
%     GP Learning took 0.000 minutes to process.
% SLAM Statistics:
%     Number of particles:    320
%     Average number of particles:    320.000
%     Number of resampling events passed:    0
%     Number of resampling events prevented: 0
%     Number of resampling events other: 0
%     Number of multibeam poses available: 51416
%     Number of multibeam poses stored: 11849
%     Number of multibeam poses sampled: 2954
%     Percentage of multibeam stored: 23.045%
%
%     Percentage of multibeam sampled: 5.745%
%
%
% Pose Statistics:
% 	Number of Poses Written: 11849
%
%
% Each line of this file describes the pose of the vehicle relative to the local
% navigation frame. The vehicle poses may have been estimated since they are the
% locations at which stereo images or multibeam sonar data were acquired.
%
% If a pose was estimated because it was the location images were acquired,
% additional information for that pose can be found in the file
% stereo_pose_est.data. The pose identifier can be used to locate matching
% poses.
%
% The X and Y coordinates are produced using a local transverse Mercator
% projection using the WGS84 ellipsoid and a central meridian at the origin
% latitude. You will probably want to use the provided latitude and longitude to
% produce coordinates in what map projection you require.
%
% The first two lines of the data contain the latitude and longitude of the
% origin.
%
% Each line contains the following items describing the pose of the vehicle:
%
% 1) Pose identifier                   - integer value
% 2) Timestamp                         - in seconds
% 3) Latitude                          - in degrees
% 4) Longitude                         - in degrees
% 5) X position (Northing)             - in meters, relative to local nav frame
% 6) Y position (Easting)              - in meters, relative to local nav frame
% 7) Z position (Depth)                - in meters, relative to local nav frame
% 8) X-axis Euler angle (Roll)         - in radians, relative to local nav frame
% 9) Y-axis Euler angle (Pitch)        - in radians, relative to local nav frame
% 10) Z-axis Euler angle (Yaw/Heading) - in radians, relative to local nav frame
% 11) Altitude                         - in meters. (0 when unknown)
*/
template <>
gsf_nav_entry::EntriesT parse_file(const boost::filesystem::path& file)
{
    gsf_nav_entry::EntriesT entries;

    gsf_nav_entry entry;
    double x, y, z;
    double time_seconds;
    const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");

    string line;
    std::ifstream infile(file.string());
    while (std::getline(infile, line))  // this does the checking!
    {
        if (line.empty() || line[0] == '%' || line[0] == '\n') {
            continue;
        }
        istringstream iss(line);

        double roll, pitch, yaw;
		iss >> entry.id_ >> time_seconds >> entry.lat_ >> entry.long_ >> x >> y >> z >> roll >> pitch >> yaw >> entry.altitude;
        entry.pos_ = Eigen::Vector3d(y, x, -z);
        entry.yaw_ = 0.5*M_PI-yaw-2.*M_PI;
        entry.pitch_ = roll;
        entry.roll_ = pitch;

        entry.time_stamp_ = (long long)(1000. * time_seconds); // double seconds to milliseconds

        boost::posix_time::ptime t = epoch + boost::posix_time::milliseconds(entry.time_stamp_);

        stringstream time_ss;
        time_ss << t;
        entry.time_string_ = time_ss.str();

		entries.push_back(entry);
    }

	return entries;
}

// reads multibeam swaths from a .gsf file
template <>
gsf_mbes_ping::PingsT parse_file(const boost::filesystem::path& file)
{
    gsf_mbes_ping::PingsT pings;
    if (boost::filesystem::extension(file) != ".gsf") {
        return pings;
    }

    if (!boost::filesystem::exists(file)) {
        cout << "File " << file << " does not exist, exiting..." << endl;
        exit(0);
    }
    int handle;
    //gsfOpen(file.string().c_str(), GSF_READONLY, &handle);
    if (gsfOpen(file.string().c_str(), GSF_READONLY, &handle) != 0 || handle < 0)
    {
        cout << "File " << file << " could not be opened!" << endl;
        return pings;
        //exit(0);
    }
    //cout << "Result: " << result << ", handle: " << handle << endl;

    gsfDataID data_id;
    gsfRecords records;
    const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");

    while (gsfRead(handle, GSF_NEXT_RECORD, &data_id, &records, nullptr, 0) != -1) {
        if (data_id.recordID == GSF_RECORD_SWATH_BATHYMETRY_PING) {
            gsf_mbes_ping ping;
            for (int i = 0; i < records.mb_ping.number_beams; ++i) {
                ping.travel_times.push_back(records.mb_ping.travel_time[i]);
                ping.beam_angles.push_back(records.mb_ping.beam_angle[i]);
                ping.amplitudes.push_back(records.mb_ping.mr_amplitude[i]);
            }
            if (records.mb_ping.depth != nullptr) {
                double x, y, z;
                for (int i = 0; i < records.mb_ping.number_beams; ++i) {
                    x = records.mb_ping.along_track[i];
                    y = -records.mb_ping.across_track[i];
                    z = -records.mb_ping.depth[i];
                    ping.beams.push_back(Eigen::Vector3d(x, y, z));
                }
            }
            if (records.mb_ping.heading != 0) {
                ping.heading_ = M_PI/180.*records.mb_ping.heading;
                ping.heading_ = 0.5*M_PI-ping.heading_; // TODO: need to keep this for old data
                ping.roll_ = M_PI/180.*records.mb_ping.roll;
                ping.pitch_ = M_PI/180.*records.mb_ping.pitch;
            }
            else {
                ping.heading_ = ping.roll_ = ping.pitch_ = 0.;
            }
            if (records.mb_ping.latitude != 0) {
                ping.lat_ = records.mb_ping.latitude;
                ping.long_ = records.mb_ping.longitude;
                ping.depth_ = records.mb_ping.depth_corrector;
            }
            else {
                ping.lat_ = ping.long_ = ping.depth_ = 0.;
            }

            long long sec = records.mb_ping.ping_time.tv_sec;
            long long nsec = records.mb_ping.ping_time.tv_nsec;
            ping.time_stamp_ = 1000*sec + nsec/1000000;
            boost::posix_time::ptime t = epoch + boost::posix_time::milliseconds(ping.time_stamp_);

            stringstream time_ss;
            time_ss << t;
            ping.time_string_ = time_ss.str();

            ping.first_in_file_ = false;
            pings.push_back(ping);
        }
    }

    gsfClose(handle);

    if (!pings.empty()) {
        pings[0].first_in_file_ = true;
    }

    return pings;
}

/*
% SOUND_SPEED_FILE VERSION 1
%
% Produced by mk_sound_speed
%
%
% Each line of this file describes the sound speed measured
% at the time indicated.
%
% On each line of the file are 4 items:
%
% 1) Record identifier                  - integer value
% 2) Timestamp                        - in seconds
% 3) Sound speed at vehicle           - in meters/second
% 4) Mean sound speed beneath vehicle - in meters/second (best guess)
*/
template <>
gsf_sound_speed::SpeedsT parse_file(const boost::filesystem::path& file)
{
    gsf_sound_speed::SpeedsT speeds;

    gsf_sound_speed speed;
    double time_seconds;
    int id_;
    const boost::posix_time::ptime epoch = boost::posix_time::time_from_string("1970-01-01 00:00:00.000");

    string line;
    std::ifstream infile(file.string());
    while (std::getline(infile, line))  // this does the checking!
    {
        if (line.empty() || line[0] == '%' || line[0] == '\n') {
            continue;
        }
        istringstream iss(line);

		iss >> id_ >> time_seconds >> speed.near_speed >> speed.below_speed;

        speed.time_stamp_ = (long long)(1000. * time_seconds); // double seconds to milliseconds

        boost::posix_time::ptime t = epoch + boost::posix_time::milliseconds(speed.time_stamp_);

        stringstream time_ss;
        time_ss << t;
        speed.time_string_ = time_ss.str();

		speeds.push_back(speed);
    }

	return speeds;
}

} // namespace std_data


/**
 *
 * Copyright (c) 2010 Matthias Walter (xammy@xammy.homelinux.net)
 *
 * Authors: Matthias Walter
 *
 * 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)
 *
 */

#ifndef BOOST_GRAPH_BIPARTITE_HPP
#define BOOST_GRAPH_BIPARTITE_HPP

#include <utility>
#include <vector>
#include <exception>
#include <boost/graph/properties.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/bind.hpp>

namespace boost {

  namespace detail {

    /**
     * The bipartite_visitor_error is thrown if an edge cannot be colored.
     * The witnesses are the edges incident vertices.
     */

    template <typename Vertex>
    struct bipartite_visitor_error: std::exception
    {
      std::pair <Vertex, Vertex> witnesses;

      bipartite_visitor_error (Vertex a, Vertex b) :
        witnesses (a, b)
      {

      }

      const char* what () const throw ()
      {
        return "Graph is not bipartite.";
      }
    };

    /**
     * Functor which colors edges to be non-monochromatic.
     */

    template <typename PartitionMap>
    struct bipartition_colorize
    {
      typedef on_tree_edge event_filter;

      bipartition_colorize (PartitionMap partition_map) :
        partition_map_ (partition_map)
      {

      }

      template <typename Edge, typename Graph>
      void operator() (Edge e, const Graph& g)
      {
        typedef typename graph_traits <Graph>::vertex_descriptor vertex_descriptor_t;
        typedef color_traits <typename property_traits <PartitionMap>::value_type> color_traits;

        vertex_descriptor_t source_vertex = source (e, g);
        vertex_descriptor_t target_vertex = target (e, g);
        if (get (partition_map_, source_vertex) == color_traits::white ())
          put (partition_map_, target_vertex, color_traits::black ());
        else
          put (partition_map_, target_vertex, color_traits::white ());
      }

    private:
      PartitionMap partition_map_;
    };

    /**
     * Creates a bipartition_colorize functor which colors edges
     * to be non-monochromatic.
     *
     * @param partition_map Color map for the bipartition
     * @return The functor.
     */

    template <typename PartitionMap>
    inline bipartition_colorize <PartitionMap> colorize_bipartition (PartitionMap partition_map)
    {
      return bipartition_colorize <PartitionMap> (partition_map);
    }

    /**
     * Functor which tests an edge to be monochromatic.
     */

    template <typename PartitionMap>
    struct bipartition_check
    {
      typedef on_back_edge event_filter;

      bipartition_check (PartitionMap partition_map) :
        partition_map_ (partition_map)
      {

      }

      template <typename Edge, typename Graph>
      void operator() (Edge e, const Graph& g)
      {
        typedef typename graph_traits <Graph>::vertex_descriptor vertex_descriptor_t;

        vertex_descriptor_t source_vertex = source (e, g);
        vertex_descriptor_t target_vertex = target (e, g);
        if (get (partition_map_, source_vertex) == get (partition_map_, target_vertex))
          throw bipartite_visitor_error <vertex_descriptor_t> (source_vertex, target_vertex);
      }

    private:
      PartitionMap partition_map_;
    };

    /**
     * Creates a bipartition_check functor which raises an error if a
     * monochromatic edge is found.
     *
     * @param partition_map The map for a bipartition.
     * @return The functor.
     */

    template <typename PartitionMap>
    inline bipartition_check <PartitionMap> check_bipartition (PartitionMap partition_map)
    {
      return bipartition_check <PartitionMap> (partition_map);
    }

    /**
     * Find the beginning of a common suffix of two sequences
     *
     * @param sequence1 Pair of bidirectional iterators defining the first sequence.
     * @param sequence2 Pair of bidirectional iterators defining the second sequence.
     * @return Pair of iterators pointing to the beginning of the common suffix.
     */

    template <typename BiDirectionalIterator1, typename BiDirectionalIterator2>
    inline std::pair <BiDirectionalIterator1, BiDirectionalIterator2> reverse_mismatch (std::pair <
        BiDirectionalIterator1, BiDirectionalIterator1> sequence1, std::pair <BiDirectionalIterator2,
        BiDirectionalIterator2> sequence2)
    {
      if (sequence1.first == sequence1.second || sequence2.first == sequence2.second)
        return std::make_pair (sequence1.first, sequence2.first);

      BiDirectionalIterator1 iter1 = sequence1.second;
      BiDirectionalIterator2 iter2 = sequence2.second;

      while (true)
      {
        --iter1;
        --iter2;
        if (*iter1 != *iter2)
        {
          ++iter1;
          ++iter2;
          break;
        }
        if (iter1 == sequence1.first)
          break;
        if (iter2 == sequence2.first)
          break;
      }

      return std::make_pair (iter1, iter2);
    }

  }

  /**
   * Checks a given graph for bipartiteness and fills the given color map with
   * white and black according to the bipartition. If the graph is not
   * bipartite, the contents of the color map are undefined. Runs in linear
   * time in the size of the graph, if access to the property maps is in
   * constant time.
   *
   * @param graph The given graph.
   * @param index_map An index map associating vertices with an index.
   * @param partition_map A color map to fill with the bipartition.
   * @return true if and only if the given graph is bipartite.
   */

  template <typename Graph, typename IndexMap, typename PartitionMap>
  bool is_bipartite (const Graph& graph, const IndexMap index_map, PartitionMap partition_map)
  {
    /// General types and variables
    typedef typename property_traits <PartitionMap>::value_type partition_color_t;
    typedef typename graph_traits <Graph>::vertex_descriptor vertex_descriptor_t;

    /// Declare dfs visitor
    //    detail::empty_recorder recorder;
    //    typedef detail::bipartite_visitor <PartitionMap, detail::empty_recorder> dfs_visitor_t;
    //    dfs_visitor_t dfs_visitor (partition_map, recorder);

    /// Call dfs
    try
    {
      depth_first_search (graph, vertex_index_map (index_map).visitor (make_dfs_visitor (std::make_pair (
          detail::colorize_bipartition (partition_map), std::make_pair (detail::check_bipartition (partition_map),
              put_property (partition_map, color_traits <partition_color_t>::white (), on_start_vertex ()))))));
    }
    catch (detail::bipartite_visitor_error <vertex_descriptor_t> error)
    {
      return false;
    }

    return true;
  }

  /**
   * Checks a given graph for bipartiteness.
   *
   * @param graph The given graph.
   * @param index_map An index map associating vertices with an index.
   * @return true if and only if the given graph is bipartite.
   */

  template <typename Graph, typename IndexMap>
  bool is_bipartite (const Graph& graph, const IndexMap index_map)
  {
    typedef one_bit_color_map <IndexMap> partition_map_t;
    partition_map_t partition_map (num_vertices (graph), index_map);

    return is_bipartite (graph, index_map, partition_map);
  }

  /**
   * Checks a given graph for bipartiteness. The graph must
   * have an internal vertex_index property. Runs in linear time in the
   * size of the graph, if access to the property maps is in constant time.
   *
   * @param graph The given graph.
   * @return true if and only if the given graph is bipartite.
   */

  template <typename Graph>
  bool is_bipartite (const Graph& graph)
  {
    return is_bipartite (graph, get (vertex_index, graph));
  }

  /**
   * Checks a given graph for bipartiteness and fills a given color map with
   * white and black according to the bipartition. If the graph is not
   * bipartite, a sequence of vertices, producing an odd-cycle, is written to
   * the output iterator. The final iterator value is returned. Runs in linear
   * time in the size of the graph, if access to the property maps is in
   * constant time.
   *
   * @param graph The given graph.
   * @param index_map An index map associating vertices with an index.
   * @param partition_map A color map to fill with the bipartition.
   * @param result An iterator to write the odd-cycle vertices to.
   * @return The final iterator value after writing.
   */

  template <typename Graph, typename IndexMap, typename PartitionMap, typename OutputIterator>
  OutputIterator find_odd_cycle (const Graph& graph, const IndexMap index_map, PartitionMap partition_map,
      OutputIterator result)
  {
    /// General types and variables
    typedef typename property_traits <PartitionMap>::value_type partition_color_t;
    typedef typename graph_traits <Graph>::vertex_descriptor vertex_descriptor_t;
    typedef typename graph_traits <Graph>::vertex_iterator vertex_iterator_t;
    vertex_iterator_t vertex_iter, vertex_end;

    /// Declare predecessor map
    typedef std::vector <vertex_descriptor_t> predecessors_t;
    typedef iterator_property_map <typename predecessors_t::iterator, IndexMap, vertex_descriptor_t,
        vertex_descriptor_t&> predecessor_map_t;

    predecessors_t predecessors (num_vertices (graph), graph_traits <Graph>::null_vertex ());
    predecessor_map_t predecessor_map (predecessors.begin (), index_map);

    /// Initialize predecessor map
    for (boost::tie (vertex_iter, vertex_end) = vertices (graph); vertex_iter != vertex_end; ++vertex_iter)
    {
      put (predecessor_map, *vertex_iter, *vertex_iter);
    }

    /// Call dfs
    try
    {
      depth_first_search (graph, vertex_index_map (index_map).visitor (make_dfs_visitor (std::make_pair (
          detail::colorize_bipartition (partition_map), std::make_pair (detail::check_bipartition (partition_map),
              std::make_pair (put_property (partition_map, color_traits <partition_color_t>::white (),
                  on_start_vertex ()), record_predecessors (predecessor_map, on_tree_edge ())))))));
    }
    catch (detail::bipartite_visitor_error <vertex_descriptor_t> error)
    {
      typedef std::vector <vertex_descriptor_t> path_t;

      path_t path1, path2;
      vertex_descriptor_t next, current;

      /// First path
      next = error.witnesses.first;
      do
      {
        current = next;
        path1.push_back (current);
        next = predecessor_map[current];
      }
      while (current != next);

      /// Second path
      next = error.witnesses.second;
      do
      {
        current = next;
        path2.push_back (current);
        next = predecessor_map[current];
      }
      while (current != next);

      /// Find beginning of common suffix
      std::pair <typename path_t::iterator, typename path_t::iterator> mismatch = detail::reverse_mismatch (
          std::make_pair (path1.begin (), path1.end ()), std::make_pair (path2.begin (), path2.end ()));

      /// Copy the odd-length cycle
      result = std::copy (path1.begin (), mismatch.first + 1, result);
      return std::reverse_copy (path2.begin (), mismatch.second, result);
    }

    return result;
  }

  /**
   * Checks a given graph for bipartiteness. If the graph is not bipartite, a
   * sequence of vertices, producing an odd-cycle, is written to the output
   * iterator. The final iterator value is returned. Runs in linear time in the
   * size of the graph, if access to the property maps is in constant time.
   *
   * @param graph The given graph.
   * @param index_map An index map associating vertices with an index.
   * @param result An iterator to write the odd-cycle vertices to.
   * @return The final iterator value after writing.
   */

  template <typename Graph, typename IndexMap, typename OutputIterator>
  OutputIterator find_odd_cycle (const Graph& graph, const IndexMap index_map, OutputIterator result)
  {
    typedef one_bit_color_map <IndexMap> partition_map_t;
    partition_map_t partition_map (num_vertices (graph), index_map);

    return find_odd_cycle (graph, index_map, partition_map, result);
  }

  /**
   * Checks a given graph for bipartiteness. If the graph is not bipartite, a
   * sequence of vertices, producing an odd-cycle, is written to the output
   * iterator. The final iterator value is returned. The graph must have an
   * internal vertex_index property. Runs in linear time in the size of the
   * graph, if access to the property maps is in constant time.
   *
   * @param graph The given graph.
   * @param result An iterator to write the odd-cycle vertices to.
   * @return The final iterator value after writing.
   */

  template <typename Graph, typename OutputIterator>
  OutputIterator find_odd_cycle (const Graph& graph, OutputIterator result)
  {
    return find_odd_cycle (graph, get (vertex_index, graph), result);
  }
}

#endif /// BOOST_GRAPH_BIPARTITE_HPP