#include "spdlog/spdlog.h"
#include <Eigen/Dense>
#include <cmath>
#include <iomanip>
#include <nanogui/nanogui.h>

#include "lodepng.h"
#include "renderer.hpp"

using namespace Eigen;
using namespace nanogui;
using namespace SnowSimulator;

Renderer::Renderer(Screen &screen, Grid &grid, MaterialPoints &materialPoints)
    : m_screen(screen), m_grid(grid), m_materialPoints(materialPoints) {
  logger = spdlog::get("snowsim");
  m_snowShader.initFromFiles("snow_shader", "../shaders/snow.vert",
                             "../shaders/snow.frag");

  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glEnable(GL_BLEND);

  glEnable(GL_PROGRAM_POINT_SIZE);
  glEnable(GL_DEPTH_TEST);

  // Vector3d avg_pm_position(0, 0, 0);

  Vector3d gridDimensions = grid.m_dim.cast<double>();

  Vector3d target(gridDimensions * (0.5 * grid.m_spacing));
  Vector3d c_dir(0., 0., 0.);

  m_canonicalViewDistance = (gridDimensions * grid.m_spacing).norm() / 2 * 1.5;
  m_scrollRate = m_canonicalViewDistance / 10;

  m_viewDistance = m_canonicalViewDistance * 2;
  m_minViewDistance = m_canonicalViewDistance / 10.0;
  m_maxViewDistance = m_canonicalViewDistance * 20.0;

  // canonicalCamera is a copy used for view resets

  m_camera.place(target, acos(c_dir.y()), atan2(c_dir.x(), c_dir.z()),
                 m_viewDistance, m_minViewDistance, m_maxViewDistance);
  m_canonicalCamera.place(target, acos(c_dir.y()), atan2(c_dir.x(), c_dir.z()),
                          m_viewDistance, m_minViewDistance, m_maxViewDistance);

  m_screenWidth = m_defaultWindowSize(0);
  m_screenHeight = m_defaultWindowSize(1);

  double hFov = 50;
  double vFov = 35;
  double nearClip = 0.01;
  double farClip = 10000;

  m_camera.configure(nearClip, farClip, hFov, vFov, m_screenWidth,
                     m_screenHeight);
  m_canonicalCamera.configure(nearClip, farClip, hFov, vFov, m_screenWidth,
                              m_screenHeight);
}

void Renderer::render() {
  m_snowShader.bind();

  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);

  // m_camera.rotate_by(0, 1.0e-3);

  Matrix4d view = getViewMatrix();
  Matrix4d projection = getProjectionMatrix();

  Matrix4f modelViewProjection = (projection * view).cast<float>();

  m_snowShader.setUniform("modelViewProjection", modelViewProjection);

  int numNodes = m_grid.dim().prod();
  auto nodes = m_grid.nodes();

  float s = m_grid.spacing();

  MatrixXf positions(3, numNodes * 36);
  RowVectorXf masses(numNodes * 36);

  for (int i = 0; i < nodes.size(); i++) {
    auto node = nodes[i];
    Vector3f position = node->position();

    double mass = node->mass();
    if (mass > 1) {
      mass = 1;
    } else if (mass < 0) {
      mass = 0;
    }

    int idx = i * 36;

    for (int j = 0; j < 36; j++) {
      masses(idx + j) = mass;
    }

    float x = position.x(), y = position.y(), z = position.z();

    positions.col(idx) << x, y, z;
    positions.col(idx + 1) << x, y + s, z;
    positions.col(idx + 2) << x + s, y, z;

    positions.col(idx + 3) << x + s, y, z;
    positions.col(idx + 4) << x, y + s, z;
    positions.col(idx + 5) << x + s, y + s, z;

    positions.col(idx + 6) << x, y, z + s;
    positions.col(idx + 7) << x, y + s, z + s;
    positions.col(idx + 8) << x + s, y, z + s;

    positions.col(idx + 9) << x + s, y, z + s;
    positions.col(idx + 10) << x, y + s, z + s;
    positions.col(idx + 11) << x + s, y + s, z + s;

    positions.col(idx + 12) << x, y, z;
    positions.col(idx + 13) << x, y, z + s;
    positions.col(idx + 14) << x, y + s, z;

    positions.col(idx + 15) << x, y + s, z;
    positions.col(idx + 16) << x, y, z + s;
    positions.col(idx + 17) << x, y + s, z + s;

    positions.col(idx + 18) << x + s, y, z;
    positions.col(idx + 19) << x, y, z + s;
    positions.col(idx + 20) << x + s, y + s, z;

    positions.col(idx + 21) << x + s, y + s, z;
    positions.col(idx + 22) << x, y, z + s;
    positions.col(idx + 23) << x, y + s, z + s;

    positions.col(idx + 24) << x, y, z;
    positions.col(idx + 25) << x, y, z + s;
    positions.col(idx + 26) << x + s, y, z;

    positions.col(idx + 27) << x + s, y, z;
    positions.col(idx + 28) << x, y, z + s;
    positions.col(idx + 29) << x + s, y, z + s;

    positions.col(idx + 30) << x, y + s, z;
    positions.col(idx + 31) << x, y + s, z + s;
    positions.col(idx + 32) << x + s, y + s, z;

    positions.col(idx + 33) << x + s, y + s, z;
    positions.col(idx + 34) << x, y + s, z + s;
    positions.col(idx + 35) << x + s, y + s, z + s;
  }

  m_snowShader.setUniform("in_color", Color(1.0f, 1.0f, 1.0f, 1.0f));

  m_snowShader.uploadAttrib("in_position", positions);
  m_snowShader.uploadAttrib("in_mass", masses);

  // GLint massIdx = m_snowShader.attrib("in_mass");
  // glVertexBindingDivisor(massIdx, 36);

  m_snowShader.drawArray(GL_TRIANGLES, 0, numNodes * 36);

  // for (auto &node : m_grid.nodes()) {
  //   // float density = node->density();
  //   double mass = std::clamp(node->mass(), 0, 1);
  //   positions()
  //
  //       idx++
  // }

  // int numParticles = m_materialPoints.particles().size();
  // MatrixXf particlePositions(3, numParticles);
  // RowVectorXf masses(numParticles);
  //
  // for (int i = 0; i < numParticles; i++) {
  //   particlePositions.col(i) = m_materialPoints.particles()[i]->m_position;
  //   masses(i) = m_materialPoints.particles()[i]->mass();
  // }

  // MatrixXf positions(3, 1000);
  // int idx = 0;
  //
  // for (int i = 0; i < 10; i++) {
  //   for (int j = 0; j < 10; j++) {
  //     for (int k = 0; k < 10; k++) {
  //       positions.col(idx++) << i, j, k;
  //     }
  //   }
  // }

  // m_snowShader.uploadAttrib("in_position", particlePositions);
  // m_snowShader.uploadAttrib("in_mass", masses);
  // m_snowShader.drawArray(GL_POINTS, 0, numParticles);

  // Draw the grid bounding box

  // Vector3f bboxMin = m_grid.m_origin;
  // Vector3f bboxMax = bboxMin + m_grid.m_dim.cast<float>() * m_grid.m_spacing;
  //
  // MatrixXf bboxVertices(3, 24);
  //
  // bboxVertices.col(0) = bboxMin;
  // bboxVertices.col(1) << bboxMax.x(), bboxMin.y(), bboxMin.z();
  //
  // bboxVertices.col(2) = bboxMin;
  // bboxVertices.col(3) << bboxMin.x(), bboxMax.y(), bboxMin.z();
  //
  // bboxVertices.col(4) = bboxMin;
  // bboxVertices.col(5) << bboxMin.x(), bboxMin.y(), bboxMax.z();
  //
  // bboxVertices.col(6) << bboxMax.x(), bboxMin.y(), bboxMax.z();
  // bboxVertices.col(7) << bboxMax.x(), bboxMin.y(), bboxMin.z();
  //
  // bboxVertices.col(8) << bboxMax.x(), bboxMin.y(), bboxMax.z();
  // bboxVertices.col(9) << bboxMax.x(), bboxMax.y(), bboxMax.z();
  //
  // bboxVertices.col(10) << bboxMax.x(), bboxMin.y(), bboxMax.z();
  // bboxVertices.col(11) << bboxMin.x(), bboxMin.y(), bboxMax.z();
  //
  // bboxVertices.col(12) << bboxMin.x(), bboxMax.y(), bboxMax.z();
  // bboxVertices.col(13) << bboxMin.x(), bboxMax.y(), bboxMin.z();
  //
  // bboxVertices.col(14) << bboxMin.x(), bboxMax.y(), bboxMax.z();
  // bboxVertices.col(15) = bboxMax;
  //
  // bboxVertices.col(16) << bboxMin.x(), bboxMax.y(), bboxMax.z();
  // bboxVertices.col(17) << bboxMin.x(), bboxMin.y(), bboxMax.z();
  //
  // bboxVertices.col(18) << bboxMax.x(), bboxMax.y(), bboxMin.z();
  // bboxVertices.col(19) << bboxMin.x(), bboxMax.y(), bboxMin.z();
  //
  // bboxVertices.col(20) << bboxMax.x(), bboxMax.y(), bboxMin.z();
  // bboxVertices.col(21) = bboxMax;
  //
  // bboxVertices.col(22) << bboxMax.x(), bboxMax.y(), bboxMin.z();
  // bboxVertices.col(23) << bboxMax.x(), bboxMin.y(), bboxMin.z();
  //
  // m_snowShader.uploadAttrib("in_position", bboxVertices);
  // m_snowShader.drawArray(GL_LINES, 0, 24);
}

// ============================================================================
// Event Handling
// ============================================================================

bool Renderer::keyCallbackEvent(int key, int scancode, int action, int mods) {
  m_ctrlDown = (bool)(mods & GLFW_MOD_CONTROL);

  if (action == GLFW_PRESS) {
    switch (key) {
    case GLFW_KEY_ESCAPE:
      // is_alive = false;
      break;
    case 'r':
    case 'R':
      // cloth->reset();
      break;
    case ' ':
      resetCamera();
      break;
    case 'p':
    case 'P':
      m_isPaused = !m_isPaused;
      break;
    }
  }

  return true;
}

bool Renderer::cursorPosCallbackEvent(double x, double y) {
  if (m_leftDown && !m_middleDown && !m_rightDown) {
    if (m_ctrlDown) {
      mouseRightDragged(x, y);
    } else {
      mouseLeftDragged(x, y);
    }
  } else if (!m_leftDown && !m_middleDown && m_rightDown) {
    mouseRightDragged(x, y);
  } else if (!m_leftDown && !m_middleDown && !m_rightDown) {
    mouseMoved(x, y);
  }

  m_mouseX = x;
  m_mouseY = y;

  return true;
}

bool Renderer::mouseButtonCallbackEvent(int button, int action, int modifiers) {
  switch (action) {
  case GLFW_PRESS:
    switch (button) {
    case GLFW_MOUSE_BUTTON_LEFT:
      m_leftDown = true;
      break;
    case GLFW_MOUSE_BUTTON_MIDDLE:
      m_middleDown = true;
      break;
    case GLFW_MOUSE_BUTTON_RIGHT:
      m_rightDown = true;
      break;
    }
    return true;

  case GLFW_RELEASE:
    switch (button) {
    case GLFW_MOUSE_BUTTON_LEFT:
      m_leftDown = false;
      break;
    case GLFW_MOUSE_BUTTON_MIDDLE:
      m_middleDown = false;
      break;
    case GLFW_MOUSE_BUTTON_RIGHT:
      m_rightDown = false;
      break;
    }
    return true;
  }

  return false;
}

void Renderer::mouseMoved(double x, double y) { y = m_screenHeight - y; }

void Renderer::mouseLeftDragged(double x, double y) {
  float dx = x - m_mouseX;
  float dy = y - m_mouseY;

  m_camera.rotate_by(-dy * (PI / m_screenHeight), -dx * (PI / m_screenWidth));
}

void Renderer::mouseRightDragged(double x, double y) {
  m_camera.move_by(m_mouseX - x, y - m_mouseY, m_canonicalViewDistance);
}

bool Renderer::scrollCallbackEvent(double x, double y) {
  m_camera.move_forward(y * m_scrollRate);
  return true;
}

bool Renderer::resizeCallbackEvent(int width, int height) {
  m_screenWidth = width;
  m_screenHeight = height;

  m_camera.set_screen_size(m_screenWidth, m_screenHeight);
  return true;
}

void Renderer::resetCamera() { m_camera.copy_placement(m_canonicalCamera); }

Matrix4d Renderer::getProjectionMatrix() {
  Matrix4d perspective;
  perspective.setZero();

  double near = m_camera.near_clip();
  double far = m_camera.far_clip();

  double theta = m_camera.v_fov() * M_PI / 360;
  double range = far - near;
  double invtan = 1. / tanf(theta);

  perspective(0, 0) = invtan / m_camera.aspect_ratio();
  perspective(1, 1) = invtan;
  perspective(2, 2) = -(near + far) / range;
  perspective(3, 2) = -1;
  perspective(2, 3) = -2 * near * far / range;
  perspective(3, 3) = 0;

  return perspective;
}

Matrix4d Renderer::getViewMatrix() {
  Matrix4d lookAt;
  Matrix3d R;

  lookAt.setZero();

  Vector3d c_pos = m_camera.position();
  Vector3d c_udir = m_camera.up_dir();
  Vector3d c_target = m_camera.view_point();

  Vector3d eye(c_pos.x(), c_pos.y(), c_pos.z());
  Vector3d up(c_udir.x(), c_udir.y(), c_udir.z());
  Vector3d target(c_target.x(), c_target.y(), c_target.z());

  R.col(2) = (eye - target).normalized();
  R.col(0) = up.cross(R.col(2)).normalized();
  R.col(1) = R.col(2).cross(R.col(0));

  lookAt.topLeftCorner<3, 3>() = R.transpose();
  lookAt.topRightCorner<3, 1>() = -R.transpose() * eye;
  lookAt(3, 3) = 1.0;

  return lookAt;
}

void Renderer::writeScreenshot(int stepCount) {
  std::vector<unsigned char> windowPixels(4 * m_screenWidth * m_screenHeight);
  glReadPixels(0, 0, m_screenWidth, m_screenHeight, GL_RGBA, GL_UNSIGNED_BYTE,
               &windowPixels[0]);

  std::vector<unsigned char> flippedPixels(4 * m_screenWidth * m_screenHeight);
  for (int row = 0; row < m_screenHeight; ++row)
    memcpy(&flippedPixels[row * m_screenWidth * 4],
           &windowPixels[(m_screenHeight - row - 1) * m_screenWidth * 4],
           4 * m_screenWidth);

  time_t t = time(nullptr);
  tm *lt = localtime(&t);
  std::stringstream ss;

  ss << "../renders/render_" << std::setfill('0') << std::setw(6) << stepCount
     << ".png";

  std::string file = ss.str();
  if (lodepng::encode(file, flippedPixels, m_screenWidth, m_screenHeight)) {
    logger->error("Failed to write screenshot to disk!");
  }
}


// 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)

// Author: Steffen Urban (steffen.urban@googlemail.com), March 2021

#include "theia/sfm/global_pose_estimation/LiGT_position_estimator.h"

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <algorithm>
#include <ceres/rotation.h>
#include <glog/logging.h>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include "spectra/include/SymEigsShiftSolver.h"

#include "theia/math/graph/triplet_extractor.h"
#include "theia/math/matrix/spectra_linear_operator.h"
#include "theia/sfm/find_common_tracks_in_views.h"
#include "theia/sfm/global_pose_estimation/compute_triplet_baseline_ratios.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/types.h"
#include "theia/sfm/view_triplet.h"
#include "theia/util/map_util.h"
#include "theia/util/threadpool.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace {

Eigen::Matrix3d GetSkew(const Eigen::Vector3d& f) {
  Eigen::Matrix3d skew_mat;
  skew_mat << 0.0, -f(2), f(1), f(2), 0.0, -f(0), -f(1), f(0), 0.0;
  return skew_mat;
}

Eigen::Matrix3d GetRij(const Eigen::Matrix3d& i, const Eigen::Matrix3d& j) {
  return j * i.transpose();
}

double GetThetaSq(const Eigen::Vector3d& feat_i,
                  const Eigen::Vector3d& feat_j,
                  const Eigen::Matrix3d& Rij) {
  return (GetSkew(feat_j) * Rij * feat_i).squaredNorm();
}

Eigen::Vector3d Get_aij(const Eigen::Matrix3d& Rij,
                        const Eigen::Vector3d Xi,
                        const Eigen::Vector3d Xj) {
  return (GetSkew(Rij * Xi) * Xj).transpose() * GetSkew(Xj);
}

// Adds the constraint from the triplet to the symmetric matrix. Our standard
// constraint matrix A is a 3M x 3N matrix with M triplet constraints and N
// cameras. We seek to construct A^t * A directly. For each triplet constraint
// in our matrix A (i.e. a 3-row block), we can compute the corresponding
// entries in A^t * A with the following summation:
//
//   A^t * A += Row(i)^t * Row(i)
//
// for each triplet constraint i.
void AddTripletConstraintToSymmetricMatrix(
    const std::vector<Matrix3d>& constraints,
    const std::vector<int>& view_indices,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
  // Construct Row(i)^t * Row(i). If we denote the row as a block matrix:
  //
  //   Row(i) = [A | B | C]
  //
  // then we have:
  //
  //   Row(i)^t * Row(i) = [A | B | C]^t * [A | B | C]
  //                     = [ A^t * A  |  A^t * B  |  A^t * C]
  //                       [ B^t * A  |  B^t * B  |  B^t * C]
  //                       [ C^t * A  |  C^t * B  |  C^t * C]
  //
  // Since A^t * A is symmetric, we only store the upper triangular portion.
  for (int i = 0; i < 3; i++) {
    for (int j = 0; j < 3; j++) {
      // Skip any block entries that correspond to the lower triangular portion
      // of the matrix.
      if (view_indices[i] > view_indices[j]) {
        continue;
      }

      // Compute the A^t * B, etc. matrix.
      const Eigen::Matrix3d symmetric_constraint =
          constraints[i].transpose() * constraints[j];

      // Add to the 3x3 block corresponding to (i, j)
      for (int r = 0; r < 3; r++) {
        for (int c = 0; c < 3; c++) {
          const std::pair<int, int> row_col(view_indices[i] + r,
                                            view_indices[j] + c);
          (*sparse_matrix_entries)[row_col] += symmetric_constraint(r, c);
        }
      }
    }
  }
}

inline Matrix3d AngleAxisToRotationMatrix(const Vector3d angle_axis) {
  const double angle = angle_axis.norm();
  const Eigen::AngleAxisd rotation_aa(angle, angle_axis / angle);
  return rotation_aa.toRotationMatrix();
}

// Returns true if the vector R1 * (c2 - c1) is in the same direction as t_12.
bool VectorsAreSameDirection(const Vector3d& position1,
                             const Vector3d& position2,
                             const Vector3d& rotation1,
                             const Vector3d& relative_position12) {
  const Vector3d global_relative_position =
      (position2 - position1).normalized();
  Vector3d rotated_relative_position;
  ceres::AngleAxisRotatePoint(rotation1.data(),
                              global_relative_position.data(),
                              rotated_relative_position.data());
  return rotated_relative_position.dot(relative_position12) > 0;
}

// Returns the features as a unit-norm pixel ray after camera intrinsics
// (i.e. focal length an principal point) have been removed.
Feature GetNormalizedFeature(const View& view, const TrackId track_id) {
  Feature feature = *view.GetFeature(track_id);
  const Camera& camera = view.Camera();
  Eigen::Vector3d ray = camera.PixelToNormalizedCoordinates(feature.point_);
  Feature normalized_Feature(ray.hnormalized());
  // todo normalized covariance?
  return normalized_Feature;
}

std::pair<ViewId, ViewId> GetBestBaseViews(
    const Reconstruction& reconstruction,
    const TrackId& track_id) {

    const Track* track = reconstruction.Track(track_id);
    std::vector<ViewId> view_ids(track->ViewIds().begin(), track->ViewIds().end());
    double theta_max = 0.0;
    std::pair<ViewId, ViewId> base_views;
    for (size_t i = 0; i < view_ids.size(); ++i) {
      for (size_t j = i+1; j < view_ids.size(); ++j) {
        ViewId id1 = view_ids[i];
        ViewId id2 = view_ids[j];
        const View* view1 = reconstruction.View(id1);
        const View* view2 = reconstruction.View(id2);
        const Vector3d feature1 =
            GetNormalizedFeature(*view1, track_id).point_.homogeneous();
        const Vector3d feature2 =
            GetNormalizedFeature(*view2, track_id).point_.homogeneous();

        const Eigen::Matrix3d R1 = view1->Camera().GetOrientationAsRotationMatrix();
        const Eigen::Matrix3d R2 = view2->Camera().GetOrientationAsRotationMatrix();
        const Matrix3d R12 = GetRij(R1, R2);
        const double theta = GetThetaSq(feature1, feature2, R12);
        if (theta > theta_max) {
            base_views = std::make_pair(id1, id2);
            theta_max = theta;
        }
      }
    }
    return base_views;
}

}  // namespace

LiGTPositionEstimator::LiGTPositionEstimator(
    const Options& options, const Reconstruction& reconstruction)
    : options_(options), reconstruction_(reconstruction) {
  CHECK_GT(options.num_threads, 0);
}

bool LiGTPositionEstimator::EstimatePositions(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Vector3d>& orientations,
    std::unordered_map<ViewId, Vector3d>* positions) {
  CHECK_NOTNULL(positions)->clear();

  num_triplets_for_view_.clear();
  linear_system_index_.clear();
  BCDs_.clear();
  triplets_for_tracks_.clear();

  view_pairs_ = &view_pairs;
  orientations_ = &orientations;

  VLOG(2) << "Extracting triplets from tracks and calculating BCDs for tracks.";
  FindTripletsForTracks();

//  VLOG(2) << "Calculating BCD for tracks.";
//  for (const auto& t : triplets_for_tracks_) {
//    auto t_id = t.first;
//    auto view_ids = t.second;
//    for (const auto& vids : view_ids) {
//      const auto view1 = reconstruction_.View(std::get<0>(vids));
//      const auto view2 = reconstruction_.View(std::get<1>(vids));
//      const auto view3 = reconstruction_.View(std::get<2>(vids));
//      std::tuple<Matrix3d, Matrix3d, Matrix3d> BCD;
//      CalculateBCDForTrack(view1, view2, view3, t_id, BCD);

////      Eigen::Vector3d shouldnull = std::get<1>(BCD)*view1->Camera().GetPosition() +
////              std::get<0>(BCD)*view2->Camera().GetPosition() +
////              std::get<2>(BCD)*view3->Camera().GetPosition();
////      std::cout<<"shouldnull: "<<shouldnull<<"\n";

//      BCDs_[t_id].push_back(BCD);
//    }
//  }

  VLOG(2) << "Building the constraint matrix...";
  // Create the linear system based on triplet constraints.
  Eigen::SparseMatrix<double> constraint_matrix;
  CreateLinearSystem(&constraint_matrix);

  // Solve for positions by examining the smallest eigenvalues. Since we have
  // set one position constant at the origin, we only need to solve for the
  // eigenvector corresponding to the smallest eigenvalue. This can be done
  // efficiently with inverse power iterations.
  VLOG(2) << "Solving for positions from the sparse eigenvalue problem...";
  SparseSymShiftSolveLLT op(constraint_matrix);
  Spectra::SymEigsShiftSolver<double, Spectra::LARGEST_MAGN,
                              SparseSymShiftSolveLLT>
  eigs(&op, 1, 6, 0.0);
  eigs.init();
  eigs.compute();

  // Compute with power iterations.
  const Eigen::VectorXd solution = eigs.eigenvectors().col(0);
  // Add the solutions to the output. Set the position with an index of -1 to
  // be at the origin.
  for (const auto &view_index : linear_system_index_) {
    if (view_index.second < 0) {
      (*positions)[view_index.first].setZero();
    } else {
      (*positions)[view_index.first] =
          solution.segment<3>(view_index.second * 3);
    }
  }

  return true;
}

void LiGTPositionEstimator::CalculateBCDForTrack(
    const theia::View* view1,
    const theia::View* view2,
    const theia::View* view3,
    const TrackId& track_id,
    std::tuple<Eigen::Matrix3d, Eigen::Matrix3d, Eigen::Matrix3d>& BCD) {
  const Vector3d feature1 =
      GetNormalizedFeature(*view1, track_id).point_.homogeneous();
  const Vector3d feature2 =
      GetNormalizedFeature(*view2, track_id).point_.homogeneous();
  const Vector3d feature3 =
      GetNormalizedFeature(*view3, track_id).point_.homogeneous();

  const Eigen::Matrix3d R1 = view1->Camera().GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d R2 = view2->Camera().GetOrientationAsRotationMatrix();
  const Eigen::Matrix3d R3 = view3->Camera().GetOrientationAsRotationMatrix();

  const Matrix3d R31 = GetRij(R3, R1);
  const Matrix3d R32 = GetRij(R3, R2);

  const Vector3d a32 = Get_aij(R32, feature3, feature2);

  const Matrix3d skew_feat1 = GetSkew(feature1);
  // equation 18
  std::get<0>(BCD) = skew_feat1 * R31 * feature3 * a32.transpose() * R2;

  const double theta = GetThetaSq(feature3, feature2, R32);
  std::get<1>(BCD) = theta * skew_feat1 * R1;

  std::get<2>(BCD) = -(std::get<0>(BCD) + std::get<1>(BCD));
}

void LiGTPositionEstimator::FindTripletsForTracks() {
  auto track_ids = reconstruction_.TrackIds();
  uint32_t total_nr_triplets = 0;
  for (size_t t = 0; t < track_ids.size(); ++t) {
    auto t_id = track_ids[t];
    std::cout<<"searching triplet for "<<t_id<<"\n";

    auto view_ids_for_track = reconstruction_.Track(t_id)->ViewIds();
    std::cout<<"view_ids_for_track size: "<<view_ids_for_track.size()<<"\n";
    if (view_ids_for_track.size() < 3) {
      continue;
    }
    // implements equation 29 from paper. Get base views for point
    std::pair<ViewId, ViewId> base_views = GetBestBaseViews(reconstruction_, t_id);
    // now iterate all other observations beside the base views
    for (size_t v = 0; v < view_ids_for_track.size(); ++v) {
      ViewId cur_id = *std::next(view_ids_for_track.begin(), v);
      // check if the current id is one of the base views
      if (cur_id == base_views.first || cur_id == base_views.second) {
          continue;
      }
      std::cout<<"Track: "<<t_id<<" triplet: (base l, central, base r) ("<<base_views.first<<", "<<cur_id<<", "<<base_views.second<<")\n";

      ViewIdTriplet triplet = std::make_tuple(base_views.first, cur_id, base_views.second);
      AddTripletConstraint(triplet);

      const auto view1 = reconstruction_.View(base_views.first);
      const auto view2 = reconstruction_.View(cur_id);
      const auto view3 = reconstruction_.View(base_views.second);
      std::tuple<Matrix3d, Matrix3d, Matrix3d> BCD;
      CalculateBCDForTrack(view1, view2, view3, t_id, BCD);

//      Eigen::Vector3d shouldbezero = std::get<1>(BCD)*view1->Camera().GetPosition() +
//              std::get<0>(BCD)*view2->Camera().GetPosition() +
//              std::get<2>(BCD)*view3->Camera().GetPosition();
//      std::cout<<"shouldbezero: "<<shouldbezero<<"\n";

      triplets_for_tracks_[t_id].push_back(triplet);
      BCDs_[t_id].push_back(BCD);
      total_nr_triplets++;
    }
  }

  std::cout<<"Total number of triplets: "<<total_nr_triplets<<" for "<< track_ids.size()<<" tracks and "<<reconstruction_.ViewIds().size()<<" views.\n";
}

// An alternative interface is to instead add triplets one by one to linear
// estimator. This allows for adding redundant observations of triplets, which
// may be useful if there are multiple estimates of the data.
void LiGTPositionEstimator::AddTripletConstraint(
    const ViewIdTriplet& view_triplet) {
  num_triplets_for_view_[std::get<0>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<1>(view_triplet)] += 1;
  num_triplets_for_view_[std::get<2>(view_triplet)] += 1;

  // Determine the order of the views in the linear system. We subtract 1 from
  // the linear system index so that the first position added to the system
  // will be set constant (index of -1 is intentionally not evaluated later).
  InsertIfNotPresent(&linear_system_index_,
                     std::get<0>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<1>(view_triplet),
                     linear_system_index_.size() - 1);
  InsertIfNotPresent(&linear_system_index_,
                     std::get<2>(view_triplet),
                     linear_system_index_.size() - 1);
}

// Sets up the linear system with the constraints that each triplet adds.
void LiGTPositionEstimator::CreateLinearSystem(
    Eigen::SparseMatrix<double>* constraint_matrix) {
  const int num_views = num_triplets_for_view_.size();

    std::unordered_map<std::pair<int, int>, double> sparse_matrix_entries;
    sparse_matrix_entries.reserve(27 * num_triplets_for_view_.size());
    for (const auto& triplet_vector : triplets_for_tracks_) {
        const TrackId t_id = triplet_vector.first;
        const std::vector<ViewIdTriplet> triplet_v = triplet_vector.second;
        for (size_t i = 0; i < triplet_v.size(); ++i) {
            const ViewId &view_id1 = std::get<0>(triplet_v[i]);
            const ViewId &view_id2 = std::get<1>(triplet_v[i]);
            const ViewId &view_id3 = std::get<2>(triplet_v[i]);
            AddTripletConstraintToSparseMatrix(view_id1, view_id2, view_id3,
                                               BCDs_[t_id][i],
                                               &sparse_matrix_entries);

        }
    }

    // Set the sparse matrix from the container of the accumulated entries.
    std::vector<Eigen::Triplet<double>> triplet_list;
    triplet_list.reserve(sparse_matrix_entries.size());
    for (const auto &sparse_matrix_entry : sparse_matrix_entries) {
      // Skip this entry if the indices are invalid. This only occurs when we
      // encounter a constraint with the constant camera (which has a view index
      // of -1).
      if (sparse_matrix_entry.first.first < 0 ||
          sparse_matrix_entry.first.second < 0) {
        continue;
      }
      triplet_list.emplace_back(sparse_matrix_entry.first.first,
                                sparse_matrix_entry.first.second,
                                sparse_matrix_entry.second);
    }

    // We construct the constraint matrix A^t * A directly, which is an
    // N - 1 x N - 1 matrix where N is the number of cameras (and 3 entries per
    // camera, corresponding to the camera position entries).

    constraint_matrix->resize((num_views - 1) * 3, (num_views - 1) * 3);
    constraint_matrix->setFromTriplets(triplet_list.begin(),
    triplet_list.end());
}

void LiGTPositionEstimator::ComputeRotatedRelativeTranslationRotations(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    Eigen::Matrix3d* r012,
    Eigen::Matrix3d* r201,
    Eigen::Matrix3d* r120) {
  // Relative camera positions.
  const Eigen::Vector3d& orientation0_aa =
      FindOrDieNoPrint(*orientations_, view_id0);
  const Eigen::Vector3d& orientation1_aa =
      FindOrDieNoPrint(*orientations_, view_id1);
  const Matrix3d orientation0 = AngleAxisToRotationMatrix(orientation0_aa);
  const Matrix3d orientation1 = AngleAxisToRotationMatrix(orientation1_aa);
  const Vector3d t01 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id1)).position_2;
  const Vector3d t02 =
      -orientation0.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id0, view_id2)).position_2;
  const Vector3d t12 =
      -orientation1.transpose() *
      FindOrDieNoPrint(*view_pairs_, ViewIdPair(view_id1, view_id2)).position_2;

  // Rotations between the translation vectors.
  *r012 = Eigen::Quaterniond::FromTwoVectors(t12, -t01).toRotationMatrix();
  *r201 = Eigen::Quaterniond::FromTwoVectors(t01, t02).toRotationMatrix();
  *r120 = Eigen::Quaterniond::FromTwoVectors(-t02, -t12).toRotationMatrix();
}

// Adds a triplet constraint to the linear system. The weight of the constraint
// (w), the global orientations, baseline (ratios), and view triplet information
// are needed to form the constraint.
void LiGTPositionEstimator::AddTripletConstraintToSparseMatrix(
    const ViewId view_id0,
    const ViewId view_id1,
    const ViewId view_id2,
    const std::tuple<Matrix3d, Matrix3d, Matrix3d>& BCD,
    std::unordered_map<std::pair<int, int>, double>* sparse_matrix_entries) {
//  // Weight each term by the inverse of the # of triplet that the nodes
//  // participate in.
//  const double w =
//      1.0 / std::sqrt(std::min({num_triplets_for_view_[view_id0],
//                                num_triplets_for_view_[view_id1],
//                                num_triplets_for_view_[view_id2]}));

  // Get the index of each camera in the sparse matrix.
  const std::vector<int> view_indices = {
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id0)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id1)),
      static_cast<int>(3 * FindOrDie(linear_system_index_, view_id2))};

  // important to use index 1 0 2 here.
  // 0 ist the central camera and equation 17 is using it that way
  std::vector<Matrix3d> constraints = {std::get<1>(BCD),std::get<0>(BCD),std::get<2>(BCD)};
  AddTripletConstraintToSymmetricMatrix(
      constraints, view_indices, sparse_matrix_entries);
}

void LiGTPositionEstimator::FlipSignOfPositionsIfNecessary(
    std::unordered_map<ViewId, Vector3d>* positions) {
  // If this value is below zero, then we should flip the sign.
  int correct_sign_votes = 0;
  for (const auto& view_pair : *view_pairs_) {
    // Only count the votes for edges where both positions were successfully
    // estimated.
    const Vector3d* position1 = FindOrNull(*positions, view_pair.first.first);
    const Vector3d* position2 = FindOrNull(*positions, view_pair.first.second);
    if (position1 == nullptr || position2 == nullptr) {
      continue;
    }

    // Check the relative translation of views 1 and 2 in the triplet.
    if (VectorsAreSameDirection(
            *position1,
            *position2,
            FindOrDieNoPrint(*orientations_, view_pair.first.first),
            view_pair.second.position_2)) {
      correct_sign_votes += 1;
    } else {
      correct_sign_votes -= 1;
    }
  }

  // If the sign of the votes is below zero, we must flip the sign of all
  // position estimates.
  if (correct_sign_votes < 0) {
    const int num_correct_votes =
        (view_pairs_->size() + correct_sign_votes) / 2;
    VLOG(2) << "Sign of the positions was incorrect: " << num_correct_votes
            << " of " << view_pairs_->size()
            << " relative translations had the correct sign. "
               "Flipping the sign of the camera positions.";
    for (auto& position : *positions) {
      position.second *= -1.0;
    }
  }
}

std::unordered_map<ViewId, Eigen::Vector3d>
LiGTPositionEstimator::EstimatePositionsWrapper(
    const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
    const std::unordered_map<ViewId, Eigen::Vector3d>& orientation) {
  std::unordered_map<ViewId, Eigen::Vector3d> positions;
  EstimatePositions(view_pairs, orientation, &positions);
  return positions;
}

}  // namespace theia


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 */

#define BOOST_TEST_MAIN

#include <limits>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_physical_constants )

//! Test if the physical constants have the correct relations (ratios/offsets).
BOOST_AUTO_TEST_CASE( testRelationsBetweenPhysicalConstant )
{
    using namespace physical_constants;

    // Test for the number of seconds in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR, JULIAN_DAY * JULIAN_YEAR_IN_DAYS,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the number of seconds in a year.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_YEAR, JULIAN_DAY * SIDEREAL_YEAR_IN_DAYS,
                                std::numeric_limits< double >::epsilon( ) );

    // Test pre-computed powers of speed of light.
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -2.0 ), INVERSE_SQUARE_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -3.0 ), INVERSE_CUBIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -4.0 ), INVERSE_QUARTIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( std::pow( SPEED_OF_LIGHT, -5.0 ), INVERSE_QUINTIC_SPEED_OF_LIGHT,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test if physical constants have the expected value.
BOOST_AUTO_TEST_CASE( testOtherConstants )
{
    using namespace physical_constants;

    // Test for gravitational constant.
    BOOST_CHECK_CLOSE_FRACTION( GRAVITATIONAL_CONSTANT, 6.67259e-11,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for speed of light.
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT, 299792458.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT_LONG, 299792458.0L,
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for astronomical unit.
    BOOST_CHECK_CLOSE_FRACTION( ASTRONOMICAL_UNIT, 1.49597870691e11,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for molar gas constant.
    BOOST_CHECK_CLOSE_FRACTION( MOLAR_GAS_CONSTANT, 8.3144598,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Avogadro's number.
    BOOST_CHECK_CLOSE_FRACTION( AVOGADRO_CONSTANT, 6.022140857e23,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Planck constant.
    BOOST_CHECK_CLOSE_FRACTION( PLANCK_CONSTANT, 6.62606957E-34,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Boltzmann constant.
    BOOST_CHECK_CLOSE_FRACTION( BOLTZMANN_CONSTANT, 1.3806488E-23,
                                std::numeric_limits< double >::epsilon( ) );

    // Test permittivity/permeability fo vacuum
    BOOST_CHECK_CLOSE_FRACTION( VACUUM_PERMEABILITY, 4.0 * mathematical_constants::PI * 1.0E-7,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( VACUUM_PERMITTIVITY, 1.0 /
                                ( 4.0 * mathematical_constants::PI * 1.0E-7 * std::pow( SPEED_OF_LIGHT, 2.0 ) ),
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Stefan-Boltzmann constant relation (derived from Planck and Boltzmann constants).
    BOOST_CHECK_CLOSE_FRACTION( STEFAN_BOLTZMANN_CONSTANT, 2.0 * physical_constants::compile_time_pow(
                                    mathematical_constants::PI, 5 ) *
                                physical_constants::compile_time_pow( BOLTZMANN_CONSTANT, 4 ) /
                                ( 15.0 * SPEED_OF_LIGHT * SPEED_OF_LIGHT *
                                  PLANCK_CONSTANT * PLANCK_CONSTANT * PLANCK_CONSTANT ),
                                std::numeric_limits< double >::epsilon( ) );

    // Test for Stefan-boltzmann constant value (NIST, 2013)
    BOOST_CHECK_CLOSE_FRACTION( STEFAN_BOLTZMANN_CONSTANT, 5.670373E-8, 1.0E-7 );

    // Test time scale rate difference factors
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM, 6.969290134E-10, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM, 1.550519768E-8, std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM_LONG, 6.969290134E-10L, std::numeric_limits< long double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM_LONG, 1.550519768E-8L, std::numeric_limits< long  double >::epsilon( ) );

}

//! Check if the time constants have the expected values.
BOOST_AUTO_TEST_CASE( testTimeConstants )
{
    using namespace physical_constants;

    // Test for the number of Julian days in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS, 365.25,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS_LONG, 365.25L,
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for the number of sidereal days in a year.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_YEAR_IN_DAYS, 365.25636,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the Julian day length.
    BOOST_CHECK_CLOSE_FRACTION( SIDEREAL_DAY, 86164.09054,
                                std::numeric_limits< double >::epsilon( ) );

    // Test for the sidereal day length.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY, 86400.0,
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY_LONG, 86400.0L,
                                std::numeric_limits< long double >::epsilon( ) );
}

//! Test templated get physical constant functions
BOOST_AUTO_TEST_CASE( testTemplatedConstantFunctions )
{
    using namespace physical_constants;

    // Test for the number of Julian days in a year.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS, getJulianYearInDays< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_YEAR_IN_DAYS_LONG, getJulianYearInDays< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for the sidereal day length.
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY, getJulianDay< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( JULIAN_DAY_LONG, getJulianDay< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test for speed of light.
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT, getSpeedOfLight< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( SPEED_OF_LIGHT_LONG, getSpeedOfLight< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );

    // Test time scale rate difference factors
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM, getLgTimeRateTerm< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM, getLbTimeRateTerm< double >( ),
                                std::numeric_limits< double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LG_TIME_RATE_TERM_LONG, getLgTimeRateTerm< long double >( ),
                                std::numeric_limits< long double >::epsilon( ) );
    BOOST_CHECK_CLOSE_FRACTION( LB_TIME_RATE_TERM_LONG, getLbTimeRateTerm< long double >( ),
                                std::numeric_limits< long  double >::epsilon( ) );

}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


#include <algorithm>
#include <cstdlib>
#include <memory>
#include <tuple>
#include <vector>
#include <utility>

#include <boost/filesystem.hpp>

#include "blas.h"

namespace {

class Index {
 public:
  Index(int order, int trans, int ld) : order_(order), trans_(trans), ld_(ld) { }
  int operator()(int i, int j) {
    if (trans_ == Blas::Trans) { int tmp = i; i = j; j = tmp; }  // transpose
    return (order_ == Blas::RowMajor) ? (i * ld_ + j) : (i + ld_ * j);
  }

 private:
  int order_;
  int trans_;
  int ld_;
};

float builtin_sdot(int size, const float *x, int xstride, const float *y, int ystride) {
  float result = 0.0f;
  for (int i = 0; i < size; ++i) result += (x[i*xstride] * y[i*ystride]);
  return result;
}

void builtin_saxpy(const int size, const float alpha,
                   const float *x, const int xstride,
                   float *y, const int ystride)
{
  for (int i = 0; i < size; ++i) {
    y[i * ystride] += alpha * x[i * xstride];
  }
}

// Convert sparse matrix from CSC to CSR format
// CSC and CSR format described here: http://docs.nvidia.com/cuda/cusparse/#compressed-sparse-row-format-csr
// Here is short summary for CSR format (Compressed Sparse Row Format):
// - float* _val is an array of length NNZ (represents non-zero values of the matrix),
// - int*   _ptr is an array of length m+1 (represents indices of the first non-zero element in row)
// - int*   _ind is an array of length NNZ (represents column indices of non-zero values of the matrix)
// All indices are 0-based.
void builtin_scsr2csc(int m, int n, int nnz,
                      const float *csr_val, const int* csr_row_ptr, const int *csr_col_ind,
                      float *csc_val, int* csc_row_ind, int* csc_col_ptr)
{
  if (nnz <= 0) return;
  std::vector<std::tuple<int, int, float>> coo_row_ind(nnz);
  for (int i = 0; i < m; ++i) {
    for (int j = csr_row_ptr[i]; j < csr_row_ptr[i + 1]; j++) {
      std::get<0>(coo_row_ind[j]) = csr_col_ind[j];
      std::get<1>(coo_row_ind[j]) = i;
      std::get<2>(coo_row_ind[j]) = csr_val[j];
    }
  }

  std::stable_sort(coo_row_ind.begin(), coo_row_ind.end());

  for (int i = 0; i < nnz; ++i) {
    csc_row_ind[i] = std::get<1>(coo_row_ind[i]);
    csc_val[i] = std::get<2>(coo_row_ind[i]);
  }

  csc_col_ptr[n] = nnz;
  for (int j = 0, i = 0; j < n; ++j) {
    csc_col_ptr[j] = i;
    while ((i < nnz) && (std::get<0>(coo_row_ind[i]) == j))
      i++;
  }
}

void builtin_sgemm(int order, const int transa, const int transb,
                   const int m, const int n, const int k,
                   const float alpha,
                   const float * a, const int lda,
                   const float * b, const int ldb,
                   const float beta,
                   float * c, const int ldc)
{
  Index ia(order, transa, lda);
  Index ib(order, transb, ldb);
  Index ic(order, Blas::NoTrans, ldc);

  bool rowa_contiguous = (order == Blas::ColMajor) ? (transa == Blas::Trans) : (transa != Blas::Trans);
  bool colb_contiguous = (order == Blas::ColMajor) ? (transb != Blas::Trans) : (transb == Blas::Trans);

  // Remember that if any stride is non-contiguous then computation will be ~10 times slower.
  // In such case consider to store transposed version of the matrix.
  int astride = rowa_contiguous ? 1 : lda;
  int bstride = colb_contiguous ? 1 : ldb;

  for (int i = 0; i < m; ++i) {
    for (int j = 0; j < n; ++j) {
      const float* aa = a + ia(i, 0);
      const float* bb = b + ib(0, j);
      float& cc = c[ic(i, j)];
      float result = builtin_sdot(k, aa, astride, bb, bstride);
      cc = alpha * result + cc * beta;
    }
  }
}

class BuiltinBlas : public Blas {
 public:
  BuiltinBlas() {
    sgemm = builtin_sgemm;
    sdot = builtin_sdot;
    saxpy = builtin_saxpy;
    scsr2csc = builtin_scsr2csc;
  }

  virtual bool is_loaded() { return true; }
};

}  // namespace

Blas* Blas::builtin() {
  static BuiltinBlas impl;
  return &impl;
}


#include <iostream>
#include <Eigen/Core>
#include <sys/time.h>

#include "celerite.h"

using namespace Eigen;

double get_timestamp () {
  struct timeval now;
  gettimeofday (&now, NULL);
  return double(now.tv_usec) * 1.0e-6 + double(now.tv_sec);
}

template <typename matrix, typename vector>
typename matrix::Scalar compute_likelihood (
  const vector& a, const matrix& U, const matrix& V, const matrix& P, const vector& y
) {
  typedef typename matrix::Scalar T;
  const int J = matrix::ColsAtCompileTime;
  int N = a.rows();

  vector d, z;
  matrix W;
  Matrix<T, J, J> S(J, J), bS(J, J);
  Matrix<T, J, 1> F(J), G(J), bF(J), bG(J);

  S.setZero();
  d = a;
  W = V;
  int flag = celerite::factor(U, P, d, W, S);
  T ll = log(d.array()).sum();

  z = y;
  celerite::solve(U, P, d, W, z, F, G);
  ll += y.transpose() * z;

  return ll;
}

template <typename matrix, typename vector>
typename matrix::Scalar compute_grad_likelihood (
  const vector& a, const matrix& U, const matrix& V, const matrix& P, const vector& y,
  vector& ba, matrix& bU, matrix& bV, matrix& bP, vector& by
) {
  typedef typename matrix::Scalar T;
  const int J = matrix::ColsAtCompileTime;
  int N = a.rows();

  vector d, z, bd(N), bz(N);
  matrix W, bW(N, J);
  Matrix<T, J, J> S(J, J), bS(J, J);
  Matrix<T, J, 1> F(J), G(J), bF(J), bG(J);

  S.setZero();
  d = a;
  W = V;
  int flag = celerite::factor(U, P, d, W, S);
  if (flag) std::cerr << flag << std::endl;
  T ll = log(d.array()).sum();

  z = y;
  celerite::solve(U, P, d, W, z, F, G);
  ll += y.transpose() * z;

  // Seed gradients.
  bz = y;
  by = z;
  bd.array() = 1.0 / d.array();

  bF.setZero();
  bG.setZero();

  bU.setZero();
  bP.setZero();
  bW.setZero();

  celerite::solve_grad(U, P, d, W, z, F, G, bz, bF, bG, bU, bP, bd, bW, by);

  bS.setZero();
  ba = bd;
  bV = bW;
  celerite::factor_grad(U, P, d, W, S, bS, bU, bP, ba, bV);

  return ll;
}

#define NUMERICAL_GRAD(ARG, BARG)                         \
    ARG += eps;                                           \
    plus = compute_likelihood(a, U, V, P, y);             \
    ARG -= 2*eps;                                         \
    minus = compute_likelihood(a, U, V, P, y);            \
    ARG += eps;                                           \
    error = std::abs(BARG - 0.5 * (plus - minus) / eps);  \
    if (error > max_error) {                              \
      max_error = error;                                  \
      max_error_name = #ARG;                              \
    }

template <typename T, int J>
void run_test (int N) {
  const auto Options = J == 1 ? ColMajor : RowMajor;
  typedef Matrix<T, Dynamic, J, Options> matrix;
  typedef Matrix<T, Dynamic, 1> vector;

  // Random matrices
  srand(1234);
  vector a(N), y = vector::Random(N);
  matrix U = matrix::Random(N, J),
         V = matrix::Random(N, J),
         P = matrix::Random(N-1, J);
  a.setConstant(10*J);

  // Gradients
  vector ba(N), by(N);
  matrix bU(N, J),
         bV(N, J),
         bP(N-1, J);

  compute_grad_likelihood(a, U, V, P, y, ba, bU, bV, bP, by);

  T eps = T(1e-8);
  T plus, minus, error, max_error = T(0.0);
  auto max_error_name = "nothing";
  for (int n = 0; n < N; ++n) {
    NUMERICAL_GRAD(a(n), ba(n));
    NUMERICAL_GRAD(y(n), by(n));
    for (int j = 0; j < J; ++j) {
      NUMERICAL_GRAD(U(n, j), bU(n, j));
      NUMERICAL_GRAD(V(n, j), bV(n, j));
      if (n < N-1) {
        NUMERICAL_GRAD(P(n, j), bP(n, j));
      }
    }
  }

  std::cout << max_error << std::endl;
  std::cout << max_error_name << std::endl;
}

int main ()
{
  run_test<double, 5>(10);

  return 0;
}


#include <blitz/timer.h>
#include <iostream>
#include <fstream>

BZ_USING_NAMESPACE(blitz)
BZ_USING_NAMESPACE(std)

extern "C" {
    void echo_f77tuned(int& N, int& niters, float& check, int& blockSize);
}

int main()
{
    int N = 1024;
    int niters = 48;
    float check;
    double Mflops = niters * 9;
    Timer timer;

    ofstream ofs("echotune.log");

    cout << "This program decides on the best block size for a typical 2D "
         << endl << "stencil operation.  Pick the block size which has the "
         << endl << "maximum Mflops/s." << endl << endl;

    cout << "Block size\tMflops/s" << endl;

    int blockSize;

    for (blockSize=1; blockSize < 32; ++blockSize)
    {
        timer.start();
        echo_f77tuned(N, niters, check, blockSize);
        timer.stop();
        cout << blockSize << "\t" << (Mflops/timer.elapsedSeconds()) << endl;
        ofs << blockSize << "\t" << (Mflops/timer.elapsedSeconds()) << endl;
    }
    for (; blockSize < 1024; blockSize += 32)
    {
        timer.start();
        echo_f77tuned(N, niters, check, blockSize);
        timer.stop();
        cout << blockSize << "\t" << (Mflops/timer.elapsedSeconds()) << endl;
        ofs << blockSize << "\t" << (Mflops/timer.elapsedSeconds()) << endl;
    }

    return 0;
}


/*
@copyright Louis Dionne 2015
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/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/negate.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::negate(hana::int_c<3>) == hana::int_c<-3>);
    static_assert(hana::negate(2) == -2, "");
}


#include "mview.h"
#include "FreeImage.h"
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <sstream>

static CameraParameter ReadCameraParameterSet(std::string);
static std::pair<GrayImage, RgbImage> ReadImageFromFile(std::string);
static float ColourToGray(Eigen::Vector3f rgb);

std::string dataset_file() {
	return "data/rathaus/parameter.txt";
}

auto read_dataset(std::istream& inputfile) -> std::vector<CameraParameter> {
    std::vector<CameraParameter> cameraParameters;
    std::string line;

    while(std::getline(inputfile,line)) {
		cameraParameters.push_back(ReadCameraParameterSet(line));
	}

	return cameraParameters;
}

auto read_image(CameraParameter cameraParameter) -> Image
{
  Image image;
  image.intrinsics = cameraParameter.intrinsics;
  image.extrinsics = cameraParameter.extrinsics;
  std::tie(image.gray_pixels, image.rgb_pixels) = ReadImageFromFile(cameraParameter.filename);

  return image;
}

std::pair<GrayImage, RgbImage> ReadImageFromFile(std::string filename)
{
  std::cout << "Reading " << filename << std::endl;
  std::string linebuffer;
  int w = 3072, h = 2048;

  std::fstream file(filename, std::ios_base::in | std::ios_base::binary);
  file >> linebuffer >> w >> h >> linebuffer;

  std::vector<unsigned char> raw_bytes(w*h*3);
  auto read = file.rdbuf()->sgetn((char*)raw_bytes.data(), raw_bytes.size());
  assert(read == raw_bytes.size());

  RgbImage rgb_target { h, w };
  for(int i = 0; i < w*h; i++)
	for(int s = 0; s < 3; s++)
      rgb_target(i/w, i%w)(s) = static_cast<float>(raw_bytes[3*i+s])/255.f;

  GrayImage gray_target = rgb_target.unaryExpr([](auto color) { return ColourToGray(color); });

  return {gray_target, rgb_target};
}

static CameraParameter ReadCameraParameterSet(std::string line) {
	CameraParameter cameraParameter;
	std::istringstream iss(line);
	iss >> cameraParameter.filename;
	float ignore;

	for(int i = 0; i < 3; i++)
		for(int j = 0; j < 3; j++)
			iss >> cameraParameter.intrinsics(i, j);

	for(int i = 0; i < 3; i++)
		iss >> ignore;

	Eigen::Matrix3f rotation (3, 3);
	Eigen::Vector3f translation (3, 1);
	for(int i = 0; i < 3; i++)
		for(int j = 0; j < 3; j++)
			iss >> rotation(i, j);
	for(int i = 0; i < 3; i++)
		iss >> translation(i, 0);

	cameraParameter.extrinsics.block<3,3>(0, 0) = rotation.transpose();
	cameraParameter.extrinsics.block<3,1>(0, 3) = -rotation.transpose()*translation;
	cameraParameter.extrinsics(3, 3) = 1.0;
	return cameraParameter;
}

static float ColourToGray(Eigen::Vector3f rgb) {
  static constexpr float GAMMA = 2.2;

  const Eigen::Vector3f gamma_correct = Eigen::Array3f(rgb).pow(GAMMA);
  return gamma_correct.dot(Eigen::Vector3f { .2126, .7152, .0722 });
}


/*
 *  utils.hpp
 *
 *	Author(s): Tamas D. Nagy
 *	Created on: 2016-11-08
 *
 *  Useful functions to provide compatibility
 *  of basic datatypes, mainly in matematical
 *  calculations.
 *
 */

#ifndef DVRK_UTILS_HPP_
#define DVRK_UTILS_HPP_

#include <iostream>
#include <sstream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <limits>

#include <std_msgs/Float32.h>
#include <irob_msgs/FloatArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Transform.h>
#include <sensor_msgs/JointState.h>
#include <irob_msgs/ToolPose.h>
#include <irob_msgs/ToolPoseStamped.h>
#include <irob_msgs/Environment.h>

namespace saf {

typedef enum InterpolationMethod
{LINEAR, BEZIER} InterpolationMethod;

inline double degToRad(double deg) {
  return (deg / 180.0) * M_PI;
}

inline double radToDeg(double rad) {
  return (rad * 180.0) / M_PI;
}

template<typename T>
std::ostream& operator<<(std::ostream& out, const std::vector<T>& v) {
  out << "[";
  size_t last = v.size() - 1;
  for(size_t i = 0; i < v.size(); ++i) {
    out << v[i];
    if (i != last)
      out << ", ";
  }
  out << "]";
  return out;
}

/*std::ostream& operator<<(std::ostream& out, const Eigen::Affine3d& T) {
  out << "Translation:\t" << T.translation() << "\tRotation:\t" << T.rotation();
  return out;
}*/

// Interpolation

template<typename T>
inline T interpolate(double a, T const& x1, T const& x2) {
  return ((1.0-a) * x1) + ((a) * x2);
}

template <>
inline Eigen::Quaterniond interpolate(double a,
                                             const Eigen::Quaterniond& x1,
                                             const Eigen::Quaterniond& x2) {
  return x1.slerp(a, x2);
}

// Distance

template<typename T>
inline double distanceEuler(T const& x1, T const& x2) {
  return std::abs(x2 - x1);
}

template <>
inline double distanceEuler(const Eigen::Vector3d& x1,
                            const Eigen::Vector3d& x2) {
  return std::abs((x2-x1).norm());
}

// Conversion from ROS msg
template<typename MsgT, typename DataT>
inline DataT unwrapMsg(const MsgT& msg);

template <>
inline irob_msgs::Environment unwrapMsg(const irob_msgs::Environment& msg){
  return msg;
}

template <>
inline double unwrapMsg(const std_msgs::Float32& msg){
  return msg.data;
}

template <>
inline Eigen::Vector3d unwrapMsg(const geometry_msgs::Vector3& msg){
  Eigen::Vector3d ret(msg.x, msg.y, msg.z);
  return ret;
}

template <>
inline Eigen::Vector3d unwrapMsg(const geometry_msgs::Point& msg){
  Eigen::Vector3d ret(msg.x, msg.y, msg.z);
  return ret;
}

template <>
inline Eigen::Quaterniond unwrapMsg(const geometry_msgs::Quaternion& msg){
  Eigen::Quaterniond ret(msg.w, msg.x, msg.y, msg.z);
  return ret;
}

template <>
inline std::vector<double> unwrapMsg(const irob_msgs::FloatArray& msg){
  return msg.data;
}

template <>
inline Eigen::Affine3d unwrapMsg(const geometry_msgs::Transform& msg){
  Eigen::Quaterniond q(msg.rotation.w, msg.rotation.x,
                            msg.rotation.y,msg.rotation.z);
  Eigen::Translation3d t(msg.translation.x, msg.translation.y, msg.translation.z);
  Eigen::Affine3d ret(t * q);
  return ret;
}

// Conversion to ROS msg
template<typename MsgT, typename DataT>
inline MsgT wrapToMsg(const DataT& data);

template <>
inline std_msgs::Float32 wrapToMsg(const double& data){
  std_msgs::Float32 msg;
  msg.data = data;
  return msg;
}

template <>
inline sensor_msgs::JointState wrapToMsg(const double& data){
  sensor_msgs::JointState msg;
  msg.name.push_back("jaw");
  msg.position.push_back(data);
  return msg;
}

template <>
inline geometry_msgs::Vector3 wrapToMsg(const Eigen::Vector3d& data){
  geometry_msgs::Vector3 msg;
  msg.x = data.x();
  msg.y = data.y();
  msg.z = data.z();
  return msg;
}

template <>
inline geometry_msgs::Point wrapToMsg(const Eigen::Vector3d& data){
  geometry_msgs::Point msg;
  msg.x = data.x();
  msg.y = data.y();
  msg.z = data.z();
  return msg;
}

template <>
inline geometry_msgs::Quaternion wrapToMsg(
    const Eigen::Quaterniond& data){
  geometry_msgs::Quaternion msg;
  msg.w = data.w();
  msg.x = data.x();
  msg.y = data.y();
  msg.z = data.z();
  return msg;
}

template <>
inline geometry_msgs::Transform wrapToMsg(
    const Eigen::Affine3d& data){
  geometry_msgs::Transform msg;

  Eigen::Vector3d translation(data.translation());
  Eigen::Quaterniond rotation(data.rotation());
  msg.translation.x = translation.x();
  msg.translation.y = translation.y();
  msg.translation.z = translation.z();

  msg.rotation.x = rotation.x();
  msg.rotation.y = rotation.y();
  msg.rotation.z = rotation.z();
  msg.rotation.w = rotation.w();
  return msg;
}

// NaN
template<typename DataT>
inline DataT makeNaN();

template <>
inline double makeNaN(){
  return std::numeric_limits<double>::quiet_NaN();
}

template <>
inline Eigen::Vector3d makeNaN(){
  Eigen::Vector3d ret(std::numeric_limits<double>::quiet_NaN(),
                      std::numeric_limits<double>::quiet_NaN(),
                      std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline Eigen::Quaterniond makeNaN(){
  Eigen::Quaterniond ret(std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN(),
                                std::numeric_limits<double>::quiet_NaN());
  return ret;
}

template <>
inline Eigen::Affine3d makeNaN(){
  Eigen::Affine3d ret(Eigen::Translation3d(makeNaN<Eigen::Vector3d>()));
  return ret;
}

template <>
inline std_msgs::Float32 makeNaN(){
  std_msgs::Float32 msg;
  msg.data = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline irob_msgs::FloatArray makeNaN(){
  irob_msgs::FloatArray msg;
  msg.data.push_back(std::numeric_limits<double>::quiet_NaN());
  return msg;
}

template <>
inline geometry_msgs::Pose makeNaN(){
  geometry_msgs::Pose nanp;
  nanp.position.x = std::numeric_limits<double>::quiet_NaN();
  nanp.position.y = std::numeric_limits<double>::quiet_NaN();
  nanp.position.z = std::numeric_limits<double>::quiet_NaN();
  nanp.orientation.x = std::numeric_limits<double>::quiet_NaN();
  nanp.orientation.y = std::numeric_limits<double>::quiet_NaN();
  nanp.orientation.z = std::numeric_limits<double>::quiet_NaN();
  nanp.orientation.w = std::numeric_limits<double>::quiet_NaN();
  return nanp;
}

template <>
inline irob_msgs::ToolPose makeNaN(){
  irob_msgs::ToolPose nanp;
  nanp.transform.translation.x = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.translation.y = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.translation.z = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.rotation.x = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.rotation.y = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.rotation.z = std::numeric_limits<double>::quiet_NaN();
  nanp.transform.rotation.w = std::numeric_limits<double>::quiet_NaN();
  nanp.jaw = std::numeric_limits<double>::quiet_NaN();
  return nanp;
}

template <>
inline irob_msgs::Environment makeNaN(){
  irob_msgs::Environment nanp;
  nanp.valid = irob_msgs::Environment::INVALID;
  return nanp;
}

template <>
inline geometry_msgs::Point makeNaN(){
  geometry_msgs::Point msg;
  msg.x = std::numeric_limits<double>::quiet_NaN();
  msg.y = std::numeric_limits<double>::quiet_NaN();
  msg.z = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline geometry_msgs::Transform makeNaN(){
  geometry_msgs::Transform msg;
  msg.translation.x = std::numeric_limits<double>::quiet_NaN();
  msg.translation.y = std::numeric_limits<double>::quiet_NaN();
  msg.translation.z = std::numeric_limits<double>::quiet_NaN();

  msg.rotation.x = std::numeric_limits<double>::quiet_NaN();
  msg.rotation.y = std::numeric_limits<double>::quiet_NaN();
  msg.rotation.z = std::numeric_limits<double>::quiet_NaN();
  msg.rotation.w = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

template <>
inline geometry_msgs::Quaternion makeNaN(){
  geometry_msgs::Quaternion msg;
  msg.w = std::numeric_limits<double>::quiet_NaN();
  msg.x = std::numeric_limits<double>::quiet_NaN();
  msg.y = std::numeric_limits<double>::quiet_NaN();
  msg.z = std::numeric_limits<double>::quiet_NaN();
  return msg;
}

// isnan
template<typename DataT>
inline bool isnan(const DataT& d);

template <>
inline bool isnan(const double& d)
{
  return std::isnan(d);
}

template <>
inline bool isnan(const Eigen::Vector3d& d)
{
  return (std::isnan(d.x())
          || std::isnan(d.y())
          || std::isnan(d.z()));
}

template <>
inline bool isnan(const Eigen::Quaterniond& d)
{
  return (std::isnan(d.x())
          || std::isnan(d.y())
          || std::isnan(d.z())
          || std::isnan(d.w()));
}

template <>
inline bool isnan(const Eigen::Affine3d& d)
{
  return (d.translation().hasNaN() || d.rotation().hasNaN());
}

template <>
inline bool isnan(const std_msgs::Float32& d)
{

  return (std::isnan(d.data));
}

template <>
inline bool isnan(const geometry_msgs::Pose& d)
{
  return (std::isnan(d.position.x)
          || std::isnan(d.position.y)
          || std::isnan(d.position.z)
          || std::isnan(d.orientation.x)
          || std::isnan(d.orientation.y)
          || std::isnan(d.orientation.z)
          || std::isnan(d.orientation.w));
}

template <>
inline bool isnan(const irob_msgs::ToolPose& d)
{
  return (std::isnan(d.transform.translation.x)
          || std::isnan(d.transform.translation.y)
          || std::isnan(d.transform.translation.z)
          || std::isnan(d.transform.rotation.x)
          || std::isnan(d.transform.rotation.y)
          || std::isnan(d.transform.rotation.z)
          || std::isnan(d.transform.rotation.w)
          || std::isnan(d.jaw));
}

template <>
inline bool isnan(const geometry_msgs::Point& d)
{
  return (std::isnan(d.x)
          || std::isnan(d.y)
          || std::isnan(d.z) );
}

template <>
inline bool isnan(const geometry_msgs::Transform& d)
{
  return (std::isnan(d.translation.x)
          || std::isnan(d.translation.y)
          || std::isnan(d.translation.z)
          || std::isnan(d.rotation.x)
          || std::isnan(d.rotation.y)
          || std::isnan(d.rotation.z)
          || std::isnan(d.rotation.w));
}

template <>
inline bool isnan(const geometry_msgs::Quaternion& d)
{
  return (std::isnan(d.x)
          || std::isnan(d.y)
          || std::isnan(d.z)
          || std::isnan(d.w));
}

// Unit vector + rotation to quat
template<typename QuatT, typename VecT>
inline QuatT vecToQuat(const VecT& vec, double angle);

template <>
inline Eigen::Quaterniond vecToQuat(const Eigen::Vector3d& vec,
                                           double angle){
  Eigen::Quaterniond quat_start(0.0, 0.707107, 0.707106, 0.0);
  double angle_rad = (angle / 180.0) * M_PI;
  Eigen::Matrix3d R1m;
  R1m = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX())
      * Eigen::AngleAxisd(0.0,  Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond R1(R1m);

  Eigen::Quaterniond ret = R1 * quat_start;

  Eigen::Vector3d vec_start(0.0, 0.0, -1.0);
  Eigen::Quaterniond R2 =
      Eigen::Quaterniond::FromTwoVectors(vec_start, vec);
  ret = R2 * ret;
  return ret;
}

// Quat to unit vector
template<typename QuatT, typename VecT>
inline VecT quatToVec(const QuatT& quat);

template <>
inline Eigen::Vector3d quatToVec(const Eigen::Quaterniond& quat){

  Eigen::Quaterniond quat_start(0.0, 0.707107, 0.707106, 0.0);

  Eigen::Quaterniond R = quat * quat_start.inverse();

  Eigen::Vector3d vec_start(0.0, 0.0, -1.0);
  Eigen::Vector3d ret = R * vec_start;
  return ret;
}

}

#endif /* DVRK_UTILS_HPP_ */


#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/adjacency_list.hpp>
//using namespace std;
//extern std::istream cin;
//extern std::ostream cout;
int main (){
	using namespace boost;
	{
		typedef adjacency_list < vecS, vecS, undirectedS > Graph;
		Graph G;
		long int a, b, j = 0;
		std::cin>>a;std::cin>>b;
		while (!std::cin.eof()){
			add_edge (a, b, G);
			std::cin>>a;std::cin>>b;
			if (! ((j++)%100000000)) std::cerr << j << std::endl;
		}
		std::cerr << "done reading:" << j << std::endl;

		std::vector <long int> component (num_vertices(G));
		std::cerr << "Created graph" << std::endl;

		int num = connected_components (G, &component[0]);
		std::cerr << "Connected graph" << std::endl;

		std::vector <long int>::size_type i;
		for (i = 0; i != component.size(); ++i){
			std::cout << i << ";" << component[i] << std::endl;
		}
	}
	return 0;
}


#include <algorithm>
#include <boost/functional/hash.hpp>
#include <iostream>
#include <sstream>
#include <unordered_set>
#include <vector>

size_t checksum(const std::vector<int>& nums) {
    size_t seed = 0;
    for (auto n : nums) {
        boost::hash_combine(seed, n);
    }
    return seed;
}

int main() {
    std::vector<int> nums;
    {
        std::string line;
        std::getline(std::cin, line);
        std::stringstream ss{line};
        while (!ss.eof()) {
            int x;
            ss >> x;
            nums.push_back(x);
        }
    }

    std::unordered_set<size_t> hashes;
    hashes.insert(checksum(nums));

    int steps = 0;
    for(;;) {
        auto it = std::max_element(std::begin(nums), std::end(nums));
        if (it == std::end(nums)) {
            std::cerr << "NO MAX!\n";
            exit(1);
        }
        int val = 0;
        std::swap(val, *it);
        for(++it; val > 0; ++it, --val) {
            if (it == std::end(nums)) {
                it = std::begin(nums);
            }
            (*it)++;
        }
        ++steps;
        if (!hashes.insert(checksum(nums)).second) {
            break;
        }
    }

    std::cout << "steps: " << steps << "\n";
}


//Authors: Dario Cattaruzza, Alessandro Abate, Peter Schrammel, Daniel Kroening
//University of Oxford 2016
//This code is supplied under the BSD license agreement (see license.txt)

#include <streambuf>
#include <iostream>
#include <fstream>
#include <math.h>

#include <boost/timer.hpp>

#include "Polyhedra.h"
#include "VertexEnumerator.h"

namespace abstract{

using std::max;

template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::ms_emptyMatrix(0,0);

template <class scalar>
Eigen::JacobiSVD<typename Tableau<scalar>::MatrixS> Polyhedra<scalar>::ms_svd;

template <class scalar>
JordanMatrix<scalar>* Polyhedra<scalar>::ms_pJordan(NULL);

template <class scalar>
traceVertices_t Polyhedra<scalar>::ms_trace_vertices=eTraceNoVertex;

template <class scalar>
traceDynamics_t Polyhedra<scalar>::ms_trace_dynamics=eTraceNoDynamics;

template <class scalar>
bool Polyhedra<scalar>::ms_auto_make_vertices=true;

/// Constructs an empty buffer
template <class scalar>
Polyhedra<scalar>::Polyhedra(int dimension) :
  DualSimplex<scalar>(0,dimension),
  m_isCentralised(false),
  m_vertices(0,dimension),
  m_tag(0),
  m_loadTime(0),
  m_transformTime(0),
  m_enumerationTime(0),
  m_calculationTime(0)
{}

/// Constructs transformed polyhedra
template <class scalar>
Polyhedra<scalar>::Polyhedra(const Polyhedra &source,const MatrixS &transform,const MatrixS &inverse) :
  DualSimplex<scalar>(source.m_size,source.getDimension()),
  m_isCentralised(false),
  m_vertices(0,getDimension()),
  m_tag(0),
  m_loadTime(0),
  m_transformTime(0),
  m_enumerationTime(0),
  m_calculationTime(0)
{
  copy(source);
  if (transform.rows()>0) this->transform(transform,inverse);
}

/// Calculates the convex hull of a point cloud
template <class scalar>
bool Polyhedra<scalar>::convexHull(const MatrixS &points,const MatrixS &vectors)
{
  MatrixS supports=points*vectors;
  m_faces=vectors.transpose();
  m_supports=supports.transpose().rowwise().maxCoeff();
  return load(m_faces,m_supports);
}

/// Loads a polyhedral description from file
template <class scalar>
int Polyhedra<scalar>::loadData(const std::string &data,size_t pos,const bool vertices)
{
  size_t result=0;
  boost::timer timer;
  this->m_isNormalised=false;
  m_vertices.resize(0,m_vertices.cols());
  if ((result=data.find("cube<",pos)) >= 0) {
    int dimension=getDimension();
    m_faces.resize(2*dimension,dimension);
    m_faces.block(0,0,dimension,dimension)=MatrixS::Identity(dimension,dimension);
    m_faces.block(dimension,0,dimension,dimension)=-MatrixS::Identity(dimension,dimension);
    m_supports.resize(m_faces.rows(),1);
    result+=5;
    m_supports.coeffRef(0,0)=ms_logger.getNumber(data,result);
    for (int row=1;row<m_faces.rows();row++) {
      m_supports.coeffRef(row,0)=m_supports.coeff(row-1,0);
    }
    load(m_faces,m_supports);
    m_loadTime=timer.elapsed()*1000;
    return result;
  }
  int lines=MatToStr<scalar>::ms_defaultLogger.lines(data,pos);
  if (lines==0) {
    result=ms_logger.StringToMat(m_faces,data,pos);
    clear();
    return result;
  }
  if (vertices) {
    result=ms_logger.StringToMat(m_vertices,data,pos);
    if (result>0) makeFaces();
    m_loadTime=timer.elapsed()*1000;
    return result;
  }
  if (getDimension()==0) {
    int cols=MatToStr<scalar>::ms_defaultLogger.cols(data,pos);
    changeDimension(cols);
  }
  m_supports.resize(lines,1);
  m_faces.resize(lines,getDimension());
  result=ms_logger.StringToMat(m_faces,m_supports,data,pos);
  if (result>0) {
    load(m_faces,m_supports);
    m_loadTime=timer.elapsed()*1000;
    if (ms_trace_tableau>=eTraceTableau) logTableau("loaded ");
    if (ms_auto_make_vertices && (m_faces.rows()>m_dimension)) makeVertices();
  }
  return result;
}

/// Loads a polyhedral description
template <class scalar>
bool Polyhedra<scalar>::loadVertices(const MatrixS &vertices)
{
  changeDimension(vertices.cols());
  m_vertices.conservativeResize(vertices.rows(),vertices.cols());
  m_vertices.block(0,0,vertices.rows(),vertices.cols())=vertices;
  if (makeFaces()) return true;
  if (ms_trace_errors) ms_logger.logData("Failed to load vertices");
  return false;
}

/// Changes the dimensionality of the polyhedra
template <class scalar>
void Polyhedra<scalar>::changeDimension(const int dimension,const bool keep)
{
  int cols=m_faces.cols();
  int rows=keep ? m_faces.rows() : 0;
  bool extend=keep && (dimension>cols);
  int newRows=extend ? 2*(dimension-cols) : 0;
  if (dimension!=cols) {
    m_faces.conservativeResize(rows+newRows,dimension);
    m_supports.conservativeResize(rows+newRows,1);
    if (extend) {
      m_faces.block(0,cols,rows,dimension-cols)=MatrixS::Zero(rows,dimension-cols);
      m_faces.block(rows,0,newRows,cols)=MatrixS::Zero(newRows,cols);
      m_faces.block(rows,cols,dimension-cols,dimension-cols)=MatrixS::Identity(dimension-cols,dimension-cols);
      m_faces.block(rows+dimension-cols,cols,dimension-cols,dimension-cols)=-MatrixS::Identity(dimension-cols,dimension-cols);
      m_supports.block(rows,0,newRows,1)=MatrixS::Zero(newRows,1);
    }
    load(m_faces,m_supports);
  }
}

/// Loads a polyhedral description from file
template <class scalar>
bool Polyhedra<scalar>::loadFromFile(const std::string &fileName)
{
  std::stringstream buffer;
  std::ifstream file;
  file.open(fileName.data());
  if (!file.is_open()) return false;
  buffer << file.rdbuf();
  file.close();
  std::string str=buffer.str();
  return loadData(str);
}

/// Returns a description of the polyhedra
template <class scalar>
std::string Polyhedra<scalar>::getDescription(displayType_t displayType,bool interval,bool useBrackets,MatrixS& templates)
{
  if (displayType==eVertices) return getVertices(",",interval,useBrackets);
  return getFaces(displayType==eNormalised,interval,useBrackets,templates);
}

/// Returns a description of the polyhedra
template <class scalar>
std::string Polyhedra<scalar>::getFaces(bool normalised,bool interval,bool useBrackets,MatrixS& templates)
{
  std::string result;
  MatrixS faces=m_faces;
  MatrixS supports=m_supports;
  if (templates.cols()>0) {
    faces=templates.transpose();
    maximiseAll(templates,supports);
  }
  if (supports.rows()!=faces.rows()) {
    int row=supports.rows();
    supports.conservativeResize(faces.cols(),1);
    for (;row<supports.rows();row++) supports.coeffRef(row,0)=func::ms_nan;
  }

  if (useBrackets) result+=ms_logger.IneToString(faces,supports,interval,normalised);
  else          result+=ms_decoder.IneToString(faces,supports,interval,normalised);
  if (m_isCentralised) {
    result+="\nc=";
    if (useBrackets)   result+=ms_logger.MatToString(m_centre,interval);
    else            result+=ms_decoder.MatToString(m_centre,interval);
  }
  return result;
}

/// Returns a description of the vertices of the polyhedra
template <class scalar>
std::string Polyhedra<scalar>::getVertices(std::string separator,bool interval,bool useBrackets)
{
  makeVertices();
  std::string result=ms_decoder.MatToString(m_vertices,interval);
  if (m_isCentralised) {
    result+="\nc=";
    result+=ms_decoder.MatToString(m_centre,interval);
  }
  return result;
}

/// Copies the polyhedra from another source
template <class scalar>
bool Polyhedra<scalar>::copy(const Polyhedra &source)
{
  if (!load(source.m_faces,source.m_supports)) return false;
  this->m_isNormalised=source.m_isNormalised;
  m_centre=source.m_centre;
  m_isCentralised=source.m_isCentralised;
  m_vertices.resize(source.m_vertices.rows(),source.m_vertices.cols());
  m_vertices.block(0,0,m_vertices.rows(),m_vertices.cols())=source.m_vertices;
  return true;
}

template <class scalar>
bool Polyhedra<scalar>::load(const MatrixS &faces,const MatrixS &supports,const bool transpose)
{
  boost::timer timer;
  this->m_dimension=faces.cols()+1;
  m_vertices.resize(0,getDimension());
  if (!DualSimplex<scalar>::load(faces,supports,transpose)) return false;
  if (&faces!=&m_faces) {
    m_centre=MatrixS::Zero(1,getDimension());
    m_isCentralised=false;
  }
  m_loadTime=timer.elapsed()*1000;
  return true;
}

/// Indicates if the referenced polyhedra is contained inside this one
template <class scalar>
bool Polyhedra<scalar>::contains(Polyhedra<scalar>& polyhedra)
{
  MatrixS supports(m_faces.rows(),1);
  MatrixS matrix=m_faces.transpose();
  polyhedra.maximiseAll(matrix,supports,this->eUnderAprox);
  supports-=m_supports;
  for (int i=0;i<supports.rows();i++) {
    if (func::isPositive(supports.coeff(i,0))) {
      return false;
    }
  }
  return true;
}

/// Intersects the polyhedra with another polyhedra
template <class scalar>
bool Polyhedra<scalar>::pseudoIntersect(const Polyhedra &polyhedra)
{
  if (polyhedra.m_dimension!=this->m_dimension) return false;
  this->m_isNormalised=false;
  int count=m_supports.rows();
  int remoteCount=polyhedra.m_supports.rows();
  m_faces.conservativeResize(count+remoteCount,m_faces.cols());
  m_faces.block(count,0,remoteCount,m_faces.cols())=polyhedra.m_faces;
  m_supports.conservativeResize(count+remoteCount,1);
  m_supports.block(count,0,remoteCount,1)=polyhedra.m_supports;
  load(m_faces,m_supports);
  return true;
}

/// Calculates the pseudoinverse of a matrix
template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::pseudoInverseEigen(const MatrixS &matrix,bool &hasInverse)
{
  if (matrix.rows()!=matrix.cols()) return pseudoInverseSVD(matrix,hasInverse);
  // We only care about the directions, so errors are acceptable (they only cause wrapping
  MatrixR refMatrix(matrix.rows(),matrix.cols());
  for (int row=0;row<matrix.rows();row++) {
    for (int col=0;col<matrix.cols();col++) {
      refMatrix.coeffRef(row,col)=func::toCentre(matrix.coeff(row,col));
    }
  }
  Eigen::EigenSolver<MatrixR> eigenSpace(refMatrix);
  if (eigenSpace.info()!=Eigen::Success) return this->getJordanSolver()->getSVDpseudoInverse(matrix,hasInverse);
  MatrixRC eigenvalues=eigenSpace.eigenvalues().asDiagonal();
  for (int row=0;row<matrix.rows();row++)
  {
    if (func::isZero(func::norm2(eigenvalues.coeff(row,row)),func::ms_weakZero)) hasInverse=false;
    else eigenvalues.coeffRef(row,row)=refScalar(1)/eigenvalues.coeff(row,row);
  }
  if (hasInverse) return matrix.inverse();
  MatrixRC eigenVectors=eigenSpace.eigenvectors();
  eigenvalues=eigenVectors.inverse()*eigenvalues*eigenVectors;
  MatrixS result(matrix.rows(),matrix.cols());
  for (int row=0;row<matrix.rows();row++) {
    for (int col=0;col<matrix.cols();col++) {
      result.coeffRef(row,col)=eigenvalues.coeff(row,col).real();
    }
  }
  if (ms_trace_dynamics>=eTraceDynamics) {
    ms_logger.logData(matrix,"Matrix:");
    ms_logger.logData(result,"Inverse:");
  }
  return result;
}

/// Calculates the pseudoinverse of a matrix
template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::pseudoInverseSVD(const MatrixS &matrix,bool &hasInverse)
{
  // We only care about the directions, so errors are acceptable (they only cause wrapping
  MatrixR refMatrix(matrix.rows(),matrix.cols());
  for (int row=0;row<matrix.rows();row++) {
    for (int col=0;col<matrix.cols();col++) {
      refMatrix.coeffRef(row,col)=func::toCentre(matrix.coeff(row,col));
    }
  }
  return this->getJordanSolver()->getSVDpseudoInverse(matrix,hasInverse);
}

/// Calculates the pseudoinverse of a matrix
template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::pseudoInverseJordan(const MatrixS &matrix,bool &hasInverse)
{
  return this->getJordanSolver()->getPseudoInverse(matrix,hasInverse);
}

/// Intersects the polyhedra with another polyhedra
template <class scalar>
bool Polyhedra<scalar>::intersect(const Polyhedra &polyhedra,const bool over)
{
  if (polyhedra.isEmpty()) return true;
  if (pseudoIntersect(polyhedra)) {
    MatrixS supports(m_faces.rows(),1);
    bool redundant[supports.rows()];
    MatrixS matrix=m_faces.transpose();
    maximiseAll(matrix,supports);
    for (int i=0;i<supports.rows();i++) {
      scalar dif=m_supports.coeff(i,0)-supports.coeff(i,0);
      char sign=func::hardSign(dif);
      redundant[i]=(sign>0) || (over && (sign==0));
    }
    int pos=0;
    for (int i=0;i<supports.rows();i++) {
      if (!redundant[i]) {
        m_faces.row(pos)=m_faces.row(i);
        m_supports.coeffRef(pos++,0)=m_supports.coeff(i,0);
      }
    }
    m_faces.conservativeResize(pos,m_faces.cols());
    m_supports.conservativeResize(pos,1);
    load(m_faces,m_supports);
    return true;
  }
  return false;
}

/// Performs the union of another polyhedra with this one
template <class scalar>
bool Polyhedra<scalar>::merge(Polyhedra &polyhedra,const bool extend)
{
  boost::timer timer;
  if (polyhedra.isEmpty()) return true;
  MatrixS supports2(m_faces.rows(),1);
  MatrixS faceVectors2=m_faces.transpose();
  polyhedra.maximiseAll(faceVectors2,supports2);
  if (this->ms_trace_tableau>=eTraceTransforms) {
    ms_logger.logData(m_faces,m_supports,"Orig Set");
    ms_logger.logData(m_faces,supports2,"Merge Set");
  }
  if (extend) {
    MatrixS supports(polyhedra.m_faces.rows(),1);
    MatrixS faceVectors=polyhedra.m_faces.transpose();
    maximiseAll(faceVectors,supports);
    if (!pseudoIntersect(polyhedra)) return false;
    for (int i=0;i<supports2.rows();i++) m_supports.coeffRef(i,0)=max(m_supports.coeff(i,0),supports2.coeff(i,0));
    for (int i=0;i<supports.rows();i++) m_supports.coeffRef(i+supports2.rows(),0)=max(polyhedra.m_supports.coeff(i,0),supports.coeff(i,0));
    this->removeRedundancies();
  }
  else {
    for (int i=0;i<supports2.rows();i++) m_supports.coeffRef(i,0)=max(m_supports.coeff(i,0),supports2.coeff(i,0));
  }
  if (this->ms_trace_tableau>=eTraceTransforms) {
    ms_logger.logData(m_faces,m_supports,"Merged Set");
  }
  if (this->ms_trace_time) {
    int elapsed=timer.elapsed()*1000;
    ms_logger.logData(elapsed," Merge time",true);
  }
  return true;
}

template <class scalar>
bool Polyhedra<scalar>::concatenate(Polyhedra &polyhedra)
{
  return concatenate(polyhedra.m_faces,polyhedra.m_supports);
}

template <class scalar>
bool Polyhedra<scalar>::concatenate(MatrixS &faces,MatrixS &supports)
{
  int oldRows=m_faces.rows();
  int oldCols=m_faces.cols();
  m_faces.conservativeResize(oldRows+faces.rows(),oldCols+faces.cols());
  m_supports.conservativeResize(oldRows+supports.rows(),1);
  m_faces.block(0,oldCols,oldRows,faces.cols())=MatrixS::Zero(oldRows,faces.cols());
  m_faces.block(oldRows,0,faces.rows(),oldCols)=MatrixS::Zero(faces.rows(),oldCols);
  m_faces.block(oldRows,oldCols,faces.rows(),faces.cols())=faces;
  m_supports.block(oldRows,0,supports.rows(),1)=supports;
  load(m_faces,m_supports);
  return true;
}

/// Performs the Minkowski sum of this polyhedra to another
template <class scalar>
bool Polyhedra<scalar>::add(Polyhedra &polyhedra,const bool extended)
{
  if (polyhedra.isEmpty()) return true;
  MatrixS supports2(m_faces.rows(),1);
  MatrixS faceVectors2=m_faces.transpose();
  polyhedra.maximiseAll(faceVectors2,supports2);
  if (!extended) {
    for (int i=0;i<supports2.rows();i++) {
      m_supports.coeffRef(i,0)=m_supports.coeff(i,0)+supports2.coeff(i,0);
    }
    return true;
  }
  MatrixS supports(polyhedra.m_faces.rows(),1);
  MatrixS faceVectors=polyhedra.m_faces.transpose();
  maximiseAll(faceVectors,supports);
  if (!pseudoIntersect(polyhedra)) return false;
  for (int i=0;i<supports2.rows();i++) m_supports.coeffRef(i,0)=m_supports.coeff(i,0)+supports2.coeff(i,0);
  int j=0;
  for (int i=supports2.rows();i<m_supports.rows();i++,j++) m_supports.coeffRef(i,0)=m_supports.coeff(i,0)+supports.coeff(j,0);
  this->removeRedundancies();
  return true;
}

/// Performs the Minkowski difference of this polyhedra with another
template <class scalar>
bool Polyhedra<scalar>::erode(Polyhedra &polyhedra)
{
  MatrixS supports(polyhedra.m_faces.rows(),1);
  MatrixS supports2(m_faces.rows(),1);
  MatrixS faceVectors=polyhedra.m_faces.transpose();
  maximiseAll(faceVectors,supports);
  MatrixS faceVectors2=m_faces.transpose();
  polyhedra.maximiseAll(faceVectors2,supports2);
  if (!pseudoIntersect(polyhedra)) return false;
  for (int i=0;i<supports2.rows();i++) m_supports.coeffRef(i,0)=m_supports.coeff(i,0)-supports2.coeff(i,0);
  int j=0;
  for (int i=supports2.rows();i<m_supports.rows();i++,j++) m_supports.coeffRef(i,0)=m_supports.coeff(i,0)-supports.coeff(j,0);
  this->removeRedundancies();
  return true;
}

/// Adds a number of directions to the template of the polhedra
template <class scalar>
bool Polyhedra<scalar>::addDirection(const MatrixS &directions)
{
  MatrixS supports(directions.cols(),1);
  if (directions.rows()!=getDimension()) return false;
  maximiseAll(directions,supports);
  return addDirection(directions,supports);
}

/// Adds a number of directions to the template of the polhedra
template <class scalar>
bool Polyhedra<scalar>::addDirection(const MatrixS &directions,MatrixS &supports)
{
  if (directions.rows()!=getDimension()) return false;
  this->m_isNormalised=false;
  int count=m_faces.rows();
  m_faces.conservativeResize(count+directions.cols(),getDimension());
  m_faces.block(count,0,directions.cols(),m_faces.cols())=directions.transpose();
  m_supports.conservativeResize(count+directions.cols(),1);
  m_supports.block(count,0,directions.cols(),1)=supports;
  this->removeRedundancies();
  m_centre.resize(0,m_centre.cols());
  m_vertices.resize(0,m_vertices.cols());
  return true;
}

/// Retrieves a copy of this polyhedra transformed by the given matrix
template <class scalar>
Polyhedra<scalar>& Polyhedra<scalar>::getTransformedPolyhedra(Polyhedra& polyhedra,const MatrixS& transform,const MatrixS& inverse,const MatrixS &templates)
{
  if (!polyhedra.copy(*this)) {
    ms_logger.logData(m_name,false);
    ms_logger.logData(" loading Error");
    throw loadError;
  }
  if ((transform.rows()>0) || (inverse.rows()>0)) polyhedra.transform(transform,inverse);
  if (templates.cols()>0) polyhedra.retemplate(templates);
  return polyhedra;
}

/// linearly transofrm the polyhedra (rotate, translate, stretch)
template <class scalar>
bool Polyhedra<scalar>::transform(const MatrixS &transform,const MatrixS& inverse)
{
  bool hasInverse=true;
  if (this->isEmpty()) return true;
  boost::timer timer;
  if ((transform.rows()<=0) && (inverse.rows()>0)) {
    this->m_isNormalised=false;
    if (m_faces.cols()==inverse.rows()) m_faces*=inverse;
    else m_faces*=inverse.transpose();
    m_centre.resize(0,m_centre.cols());
    m_vertices.resize(0,m_vertices.cols());
  }
  else {
    if (transform.cols()!=getDimension()) return false;
    if (transform.rows()>transform.cols()) {
      int rows=transform.rows();
      int cols=transform.cols();
      MatrixS newTransform(rows,rows);
      newTransform.block(0,0,rows,cols)=transform;
      newTransform.block(0,cols,rows,rows-cols)=MatrixS::Zero(rows,rows-cols);
      changeDimension(rows,true);
      return this->transform(newTransform);
    }
    if (m_centre.rows()>0) {
      if (m_centre.cols()==transform.cols()) {
        m_centre=m_centre*transform.transpose();
        if (m_isCentralised) m_isCentralised=(func::isPositive(m_centre.norm()));
      }
    }
    if (m_vertices.rows()>0) m_vertices*=transform.transpose();
    this->m_isNormalised=false;
    if (inverse.rows()>0) m_faces*=inverse;
    else {
      MatrixS matrix=pseudoInverseEigen(transform,hasInverse);
      if (ms_trace_dynamics>=eTraceDynamics) {
        ms_logger.logData(transform,"Transform:");
        ms_logger.logData(matrix,"Pseudo Inverse:");
      }
      if (hasInverse) m_faces*=matrix;// The normal gets multiplied by A^-1^T
      else {
        MatrixS faces(2*m_faces.rows(),getDimension());
        faces.block(0,0,m_faces.rows(),getDimension())=m_faces;
        faces.block(m_faces.rows(),0,m_faces.rows(),getDimension())=m_faces*matrix;
        MatrixS supports;
        MatrixS vectors=transform.transpose()*faces.transpose();
        if (this->ms_trace_tableau>=eTraceTableau) this->logTableau();
        maximiseAll(vectors,supports);
        m_faces=faces;
        m_supports=supports;
        return this->removeRedundancies();
      }
    }
  }
  bool result=DualSimplex<scalar>::load(m_faces,m_supports);
  m_transformTime=timer.elapsed()*1000;
  if (this->ms_trace_time) {
    if (m_transformTime>1) {
      ms_logger.logData(m_name,false);
      ms_logger.logData(m_transformTime," Transform:",true);
    }
  }
  return result;
}

/// templates the polyhedra in the given directions
template <class scalar>
bool Polyhedra<scalar>::retemplate(const MatrixS& templates,refScalar aprox)
{
  MatrixS supports(templates.cols(),1);
  decentralize();
  this->m_isNormalised=false;
  if (aprox<0) {
    refScalar threshold=1+aprox;
    this->removeRedundancies();
    if (!makeVertices()) return false;
    for (int dir=0;dir<templates.cols();dir++) {
      scalar support=-func::ms_infinity;
      supports.coeffRef(dir,0)=support;
      for (int point=0;point<m_vertices.rows();point++) {
        MatrixS thisSupport=m_vertices.row(point)*templates.block(0,dir,m_vertices.cols(),1);
        if (thisSupport.coeff(0,0)>support) support=thisSupport.coeff(0,0);
      }
      supports.coeffRef(dir,0)=support;
      for (int point=0;point<m_vertices.rows();point++) {
        MatrixS thisSupport=m_vertices.row(point)*templates.block(0,dir,m_vertices.cols(),1);
        if ((thisSupport.coeff(0,0)>support*threshold) && (thisSupport.coeff(0,0)<supports.coeffRef(dir,0)))
        {
          supports.coeffRef(dir,0)=thisSupport.coeff(0,0);
        }
      }
    }
    addDirection(templates,supports);
    logTableau();//templog
    this->removeRedundancies();
    //m_vertices.resize(0,m_vertices.cols());
    makeVertices(true);
    if (!makeFaces()) return false;
    logTableau();//templog
    for (int i=0;i<supports.rows();i++) supports.coeffRef(i,0)=func::toLower(supports.coeffRef(i,0));
    return true;
  }
  maximiseAll(templates,supports);
  m_faces=templates.transpose();
  m_supports=supports;
  return load(m_faces,m_supports);
}

/// linearly transofrm the polyhedra (rotate, translate, stretch)
template <class scalar>
void Polyhedra<scalar>::transform(const scalar &coefficient)
{
  this->m_isNormalised=false;
  decentralize();
  if (m_vertices.rows()>0) m_vertices*=coefficient;
  m_supports*=coefficient;
  m_centre*=coefficient;
  m_isCentralised=(m_centre.norm()>this->m_zero);
  load(m_faces,m_supports);
}

/// linearly transofrm the polyhedra through vertex enumeration
template <class scalar>
bool Polyhedra<scalar>::vertexTransform(const MatrixS &transform,const MatrixS &templates)
{
  if (!makeVertices()) return false;
  MatrixS vertices=m_vertices*transform.transpose();
  return convexHull(vertices,templates);
}

/// finds the inequalities of the polyhedra and stores them in a matrix
template <class scalar>
bool Polyhedra<scalar>::makeFaces()
{
  if (m_vertices.rows()<=0) return true;
  decentralize();
  MatrixS supports=MatrixS::Ones(m_vertices.rows(),1);
  MatrixS centre=m_vertices.colwise().sum()/m_vertices.rows();
  for (int row=0;row<m_vertices.rows();row++) m_vertices.row(row)-=centre;
  VertexEnumerator<scalar> enumerator(m_vertices.rows(),m_vertices.cols());
  typename VertexEnumerator<scalar>::RayList& rayList=enumerator.findVertices(m_vertices,supports,true);
  int numFaces=rayList.size();
  if (numFaces==0) return false;
  m_faces.resize(numFaces,getDimension());
  m_supports.resize(numFaces,1);
  int row=0;
  for (typename VertexEnumerator<scalar>::RayList::iterator it=rayList.begin();it!=rayList.end();it++,row++) {
    m_faces.row(row)=it->data.block(0,1,1,m_faces.cols());
    m_supports.coeffRef(row,0)=it->data.coeff(0,0);
  }
  centre=-centre;
  m_supports-=m_faces*centre.transpose();
  load(m_faces,m_supports);
  return true;
}

template <class scalar>
void Polyhedra<scalar>::logVertices(bool force)
{
  if ((ms_trace_vertices>=eTraceVertices) || force) {
    makeVertices(force);
    ms_logger.logData(m_name,false);
    ms_logger.logData(m_vertices," Vertices:");
  }
}

template <class scalar>
void Polyhedra<scalar>::logPolyhedra(std::string parameters)
{
  if (parameters.length()>0) {
    std::stringstream stream;
    stream << getName();
    stream << ": " << parameters;
    ms_logger.logData(m_faces,m_supports,stream.str());
  }
  else ms_logger.logData(m_faces,m_supports,getName());
  if (ms_trace_vertices>=eTraceVertices) {
    makeVertices();
    ms_logger.logData(m_vertices," Vertices:");
  }
}

/// finds the vertices of the polyhedra and stores them in a matrix
template <class scalar>
bool Polyhedra<scalar>::makeVertices(bool force)
{
  if (!force && (m_vertices.rows()>0)) return true;
  if (m_faces.rows()==0) {
    return false;
  }
  boost::timer timer;
  this->normalise();
  VertexEnumerator<scalar> enumerator(m_faces.rows(),m_faces.cols());
  typename VertexEnumerator<scalar>::RayList& rayList=enumerator.findVertices(m_faces,m_supports,false);
  enumerator.logRays();

  int numVertices=rayList.size();
  if (numVertices==0) {
    if (this->ms_trace_time) {
      ms_logger.logData(m_name,false);
      ms_logger.logData(timer.elapsed()*1000," Make Vertices Time:",true);
    }
    if (ms_trace_errors) {
      ms_logger.logData(m_name, false);
      ms_logger.logData(": Failed to make vertices");
      logTableau();
    }
    return false;
  }
  m_vertices.resize(numVertices,getDimension());

  int row=0;
  for (typename VertexEnumerator<scalar>::RayList::iterator it=rayList.begin();it!=rayList.end();it++,row++) {
    scalar scale = abs(it->data.coeff(0,0));
    if (func::isZero(scale)) scale=1;
    for (int col=0;col<m_vertices.cols();col++) m_vertices.coeffRef(row,col)=it->data.coeff(0,col+1)/scale;
  }
  m_enumerationTime=timer.elapsed()*1000;
  if (ms_trace_vertices>=eTraceVertices) {
    ms_logger.logData(m_name,false);
    ms_logger.logData(" Find Vertices:");
    this->logTableau();
  }
  if (this->ms_trace_time) {
    ms_logger.logData(m_name,false);
    ms_logger.logData(m_enumerationTime," Make Vertices Time:",true);
  }
  return true;
}

/// Removes any existing faces
template <class scalar>
void Polyhedra<scalar>::clear()
{
  m_faces.resize(0,getDimension());
  m_supports.resize(0,1);
  load(m_faces,m_supports);
}

/// Retrieves the (hyper-rectangular) center of the polyhedra
template <class scalar>
typename Tableau<scalar>::MatrixS& Polyhedra<scalar>::getCentre()
{
  if (!m_isCentralised) {
    MatrixS positiveDirections=MatrixS::Identity(getDimension(),getDimension());
    MatrixS positiveSupports;
    maximiseAll(positiveDirections,positiveSupports);
    MatrixS negativeDirections=-positiveDirections;
    MatrixS negativeSupports;
    maximiseAll(negativeDirections,negativeSupports);
    m_centre=(positiveSupports-negativeSupports).transpose()/2;
  }
  return m_centre;
}

/// Finds the (hyper-rectangular) center of the polyhedra, and moves it to the origin
template <class scalar>
void Polyhedra<scalar>::centralize()
{
  if (m_isCentralised) return;
  m_isCentralised=false;
  translate(getCentre());
}

/// Translates the polyhedra in the direction of vector
template <class scalar>
void Polyhedra<scalar>::translate(const MatrixS &vector,const bool storeOffset)
{
  if (storeOffset) {
    if (m_isCentralised)  m_centre+=vector;
    else                  m_centre=vector;
    m_isCentralised=(func::toLower(m_centre.norm())>this->m_zero);
  }
  for (int row=0;row<m_vertices.rows();row++) m_vertices.row(row)-=vector;
  MatrixS matrix=m_faces*vector.transpose();
  m_supports-=matrix;
}

/// Moves the polyhedra by the vector indicated in centre.
template <class scalar>
void Polyhedra<scalar>::decentralize()
{
  if (!m_isCentralised) return;
  MatrixS minCentre=-m_centre;
  translate(minCentre);
  m_isCentralised=false;
}

/// Indicates if there is a selected non-zero central point
template <class scalar>
bool Polyhedra<scalar>::isCentralized()
{
  return m_isCentralised;//(m_centre.norm()>this->m_zero);
}

/// Indicates if a set of points is inside the Polyhedra
template <class scalar>
bool Polyhedra<scalar>::isInside(const MatrixS &points)
{
  if (points.cols()!=getDimension()) return false;
  MatrixS supports=m_faces*points.transpose();
  for (int col=0;col<supports.cols();col++) {
    supports.col(col)-=m_supports;
    for (int row=0;row<supports.rows();row++) {
      if (func::toUpper(supports.coeff(row,col))>0) {
        /*ms_logger.logData(points,"Points");
        ms_logger.logData(supports,"Supports");
        logTableau("True Supports",true);*/
        return false;
      }
    }
  }
  return true;
}

template <class scalar>
void Polyhedra<scalar>::ComputeVertexOrderVector()
{
  m_vertices.QuickAngleSort();
}

template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::vertexMaximize(const MatrixS &vectors,const bool all)
{
  if (!makeVertices()) return MatrixS(0,0);
  if (m_vertices.cols()!=vectors.cols()) return MatrixS::Zero(vectors.cols(),1);
  MatrixS result=(m_vertices*vectors).transpose();
  if (all) return result;
  MatrixS supports=result.rowwise().maxCoeff();
  return supports;
}

template <class scalar>
typename Tableau<scalar>::MatrixS Polyhedra<scalar>::boundingHyperBox()
{
  int dimension=getDimension();
  MatrixS hyperbox(2*dimension,2);
  makeVertices();
  for (int i=0;i<dimension;i++) {
    hyperbox.coeffRef(i,0)=m_vertices.coeff(0,i);
    hyperbox.coeffRef(i,1)=m_vertices.coeff(0,i);
  }
  for  (int row=1;row<m_vertices.rows();row++) {
    for (int i=0;i<dimension;i++) {
      if (m_vertices.coeff(row,i)<hyperbox.coeff(i,0)) hyperbox.coeffRef(i,0)=m_vertices.coeff(0,i);
      if (m_vertices.coeff(row,i)>hyperbox.coeff(i,1)) hyperbox.coeffRef(i,1)=m_vertices.coeff(0,i);
    }
  }
  return hyperbox;
}

template <class scalar>
JordanMatrix<scalar>* Polyhedra<scalar>::getJordanSolver()
{
  if (!ms_pJordan) ms_pJordan=new JordanMatrix<scalar>(0);
  return ms_pJordan;
}

#ifdef USE_LDOUBLE
  #ifdef USE_SINGLES
    template class Polyhedra<long double>;
  #endif
  #ifdef USE_INTERVALS
    template class Polyhedra<ldinterval>;
  #endif
#endif
#ifdef USE_MPREAL
  #ifdef USE_SINGLES
    template class Polyhedra<mpfr::mpreal>;
  #endif
  #ifdef USE_INTERVALS
    template class Polyhedra<mpinterval>;
  #endif
#endif

}