// Copyright 2019 Autoware Foundation. All rights reserved.
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef LANE_CHANGE_PLANNER__UTILITIES_HPP_
#define LANE_CHANGE_PLANNER__UTILITIES_HPP_

#include "lane_change_planner/route_handler.hpp"

#include <rclcpp/rclcpp.hpp>

#include <autoware_perception_msgs/msg/dynamic_object_array.hpp>
#include <autoware_planning_msgs/msg/path.hpp>
#include <autoware_planning_msgs/msg/path_with_lane_id.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometry.hpp>

#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_routing/Route.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_routing/RoutingGraphContainer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <limits>
#include <string>
#include <vector>

namespace lane_change_planner
{
namespace util
{
using Point = boost::geometry::model::d2::point_xy<double>;
using Polygon = boost::geometry::model::polygon<Point>;
using LineString = boost::geometry::model::linestring<Point>;
struct FrenetCoordinate3d
{
  double length;
  double distance;
  FrenetCoordinate3d() : length(0), distance(0) {}
};

double normalizeRadian(const double radian);
double l2Norm(const geometry_msgs::msg::Vector3 vector);

Eigen::Vector3d convertToEigenPt(const geometry_msgs::msg::Point geom_pt);
std::vector<geometry_msgs::msg::Point> convertToGeometryPointArray(
  const autoware_planning_msgs::msg::PathWithLaneId & path);
geometry_msgs::msg::PoseArray convertToGeometryPoseArray(
  const autoware_planning_msgs::msg::PathWithLaneId & path);

autoware_perception_msgs::msg::PredictedPath convertToPredictedPath(
  const autoware_planning_msgs::msg::PathWithLaneId & path,
  const geometry_msgs::msg::Twist & vehicle_twist, const geometry_msgs::msg::Pose & vehicle_pose,
  const double duration, const double resolution, const double acceleration,
  const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock);
autoware_perception_msgs::msg::PredictedPath resamplePredictedPath(
  const autoware_perception_msgs::msg::PredictedPath & input_path, const double resolution,
  const double duration, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr & clock);

bool convertToFrenetCoordinate3d(
  const std::vector<geometry_msgs::msg::Point> & linestring,
  const geometry_msgs::msg::Point search_point_geom, FrenetCoordinate3d * frenet_coordinate);

geometry_msgs::msg::Pose lerpByPose(
  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t);

geometry_msgs::msg::Point lerpByLength(
  const std::vector<geometry_msgs::msg::Point> & array, const double length);
bool lerpByTimeStamp(
  const autoware_perception_msgs::msg::PredictedPath & path, const rclcpp::Time & t,
  geometry_msgs::msg::Pose * lerped_pt, const rclcpp::Logger & logger,
  const rclcpp::Clock::SharedPtr & clock);

double getDistance3d(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2);
double getDistanceBetweenPredictedPaths(
  const autoware_perception_msgs::msg::PredictedPath & path1,
  const autoware_perception_msgs::msg::PredictedPath & path2, const double start_time,
  const double end_time, const double resolution, const rclcpp::Logger & logger,
  const rclcpp::Clock::SharedPtr & clock);

double getDistanceBetweenPredictedPathAndObject(
  const autoware_perception_msgs::msg::DynamicObject & object,
  const autoware_perception_msgs::msg::PredictedPath & path, const double start_time,
  const double end_time, const double resolution, const rclcpp::Logger & logger,
  const rclcpp::Clock::SharedPtr & clock);

std::vector<size_t> filterObjectsByLanelets(
  const autoware_perception_msgs::msg::DynamicObjectArray & objects,
  const lanelet::ConstLanelets & lanelets, const double start_arc_length,
  const double end_arc_length, const rclcpp::Logger & logger);

std::vector<size_t> filterObjectsByLanelets(
  const autoware_perception_msgs::msg::DynamicObjectArray & objects,
  const lanelet::ConstLanelets & target_lanelets, const rclcpp::Logger & logger);

bool calcObjectPolygon(
  const autoware_perception_msgs::msg::DynamicObject & object, Polygon * object_polygon,
  const rclcpp::Logger & logger);

std::vector<size_t> filterObjectsByPath(
  const autoware_perception_msgs::msg::DynamicObjectArray & objects,
  const std::vector<size_t> & object_indices,
  const autoware_planning_msgs::msg::PathWithLaneId & ego_path, const double vehicle_width,
  const rclcpp::Logger & logger);

const geometry_msgs::msg::Pose refineGoal(
  const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelet & goal_lanelet);

autoware_planning_msgs::msg::PathWithLaneId refinePath(
  const double search_radius_range, const double search_rad_range,
  const autoware_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Pose & goal,
  const int64_t goal_lane_id, const rclcpp::Logger & logger);
autoware_planning_msgs::msg::PathWithLaneId removeOverlappingPoints(
  const autoware_planning_msgs::msg::PathWithLaneId & input_path);

bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id);

nav_msgs::msg::OccupancyGrid generateDrivableArea(
  const lanelet::ConstLanelets & lanes, const geometry_msgs::msg::PoseStamped & current_pose,
  const double width, const double height, const double resolution, const double vehicle_length,
  const RouteHandler & route_handler);

double getDistanceToEndOfLane(
  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets);

double getDistanceToNextIntersection(
  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets);
double getDistanceToCrosswalk(
  const geometry_msgs::msg::Pose & current_pose, const lanelet::ConstLanelets & lanelets,
  const lanelet::routing::RoutingGraphContainer & overall_graphs);
double getSignedDistance(
  const geometry_msgs::msg::Pose & current_pose, const geometry_msgs::msg::Pose & goal_pose,
  const lanelet::ConstLanelets & lanelets);

std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets);

autoware_planning_msgs::msg::Path convertToPathFromPathWithLaneId(
  const autoware_planning_msgs::msg::PathWithLaneId & path_with_lane_id);

lanelet::Polygon3d getVehiclePolygon(
  const geometry_msgs::msg::Pose & vehicle_pose, const double vehicle_width,
  const double base_link2front);

autoware_planning_msgs::msg::PathPointWithLaneId insertStopPoint(
  double length, autoware_planning_msgs::msg::PathWithLaneId * path);

double getArcLengthToTargetLanelet(
  const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelet & target_lane,
  const geometry_msgs::msg::Pose & pose);

std::vector<Polygon> getTargetLaneletPolygons(
  const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & pose,
  const double check_length, const std::string & target_type);

std::vector<Polygon> filterObstaclePolygons(
  const std::vector<Polygon> & obstacle_polygons,
  const autoware_perception_msgs::msg::DynamicObjectArray & objects,
  const double static_obstacle_velocity_thresh);

double getDistanceToNearestObstaclePolygon(
  const std::vector<Polygon> & obstacle_polygons, const geometry_msgs::msg::Pose & pose);

class SplineInterpolate
{
  bool initialized_ = false;
  std::vector<double> a_;
  std::vector<double> b_;
  std::vector<double> c_;
  std::vector<double> d_;
  std::vector<double> h_;
  rclcpp::Logger logger_;

public:
  explicit SplineInterpolate(const rclcpp::Logger & logger);
  bool interpolate(
    const std::vector<double> & base_index, const std::vector<double> & base_value,
    const std::vector<double> & return_index, std::vector<double> & return_value);

private:
  double getValue(const double & query, const std::vector<double> & base_index) const;
  void generateSpline(
    const std::vector<double> & base_index, const std::vector<double> & base_value);
  bool isIncrease(const std::vector<double> & x) const;
  bool isValidInput(
    const std::vector<double> & base_index, const std::vector<double> & base_value,
    const std::vector<double> & return_index, std::vector<double> & return_value) const;
  std::vector<double> solveLinearSystem(const double omega, const size_t max_iter) const;
  bool isConvergeL1(
    const std::vector<double> & r1, const std::vector<double> & r2,
    const double converge_range) const;
};

}  // namespace util
}  // namespace lane_change_planner

#endif  // LANE_CHANGE_PLANNER__UTILITIES_HPP_


#ifndef POSE_EDGE_HPP
#define POSE_EDGE_HPP

#include <Eigen/Geometry>
#include <opencv2/core/core.hpp>
#include <g2o/core/block_solver.h>
#include <g2o/types/slam3d/types_slam3d.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/base_unary_edge.h>
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include "../helper.hpp"

class PoseEdge:public g2o::BaseUnaryEdge<1, Eigen::VectorXd, g2o::VertexSE3Expmap>
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  PoseEdge(const Eigen::Vector3d& point, const Eigen::Matrix3d& K, const cv::Mat& img)
  : start_point(point), fx(K(0,0)), fy(K(1,1)), cx(K(0,2)), cy(K(1,2)), dest_img(img)
  {}

  void computeError();

  void linearizeOplus();

    virtual bool read(std::istream& /*is*/)
    {
      std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
      return false;
    }

    virtual bool write(std::ostream& /*os*/) const
    {
      std::cerr << __PRETTY_FUNCTION__ << " not implemented yet" << std::endl;
      return false;
    }

private:

  Eigen::Vector3d start_point;
  double fx=0, fy=0, cx=0, cy=0;
  const cv::Mat& dest_img;
};

#endif


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

#include <unistd.h>
#include <cassert>
#include <cstddef>
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <memory>
#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>
#include <boost/shared_ptr.hpp>

#include "common/varint.h"
#include "epee/include/warnings.h"
#include "crypto.h"
#include "hash.h"

namespace {
  static void local_abort(const char *msg)
  {
    fprintf(stderr, "%s\n", msg);
#ifdef NDEBUG
    _exit(1);
#else
    abort();
#endif
  }
}

namespace crypto {

  using std::abort;
  using std::int32_t;
  using std::int64_t;
  using std::size_t;
  using std::uint32_t;
  using std::uint64_t;

  extern "C" {
#include "crypto-ops.h"
#include "random.h"
  }

  const crypto::public_key null_pkey = crypto::public_key{};
  const crypto::secret_key null_skey = crypto::secret_key{};

  static inline unsigned char *operator &(ec_point &point) {
    return &reinterpret_cast<unsigned char &>(point);
  }

  static inline const unsigned char *operator &(const ec_point &point) {
    return &reinterpret_cast<const unsigned char &>(point);
  }

  static inline unsigned char *operator &(ec_scalar &scalar) {
    return &reinterpret_cast<unsigned char &>(scalar);
  }

  static inline const unsigned char *operator &(const ec_scalar &scalar) {
    return &reinterpret_cast<const unsigned char &>(scalar);
  }

  void generate_random_bytes_thread_safe(size_t N, uint8_t *bytes)
  {
    static boost::mutex random_lock;
    boost::lock_guard<boost::mutex> lock(random_lock);
    generate_random_bytes_not_thread_safe(N, bytes);
  }

  static inline bool less32(const unsigned char *k0, const unsigned char *k1)
  {
    for (int n = 31; n >= 0; --n)
    {
      if (k0[n] < k1[n])
        return true;
      if (k0[n] > k1[n])
        return false;
    }
    return false;
  }

  void random32_unbiased(unsigned char *bytes)
  {
    // l = 2^252 + 27742317777372353535851937790883648493.
    // it fits 15 in 32 bytes
    static const unsigned char limit[32] = { 0xe3, 0x6a, 0x67, 0x72, 0x8b, 0xce, 0x13, 0x29, 0x8f, 0x30, 0x82, 0x8c, 0x0b, 0xa4, 0x10, 0x39, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0 };
    do
    {
      generate_random_bytes_thread_safe(32, bytes);
    } while (!sc_isnonzero(bytes) && !less32(bytes, limit)); // should be good about 15/16 of the time
    sc_reduce32(bytes);
  }
  /* generate a random 32-byte (256-bit) integer and copy it to res */
  static inline void random_scalar(ec_scalar &res) {
    random32_unbiased((unsigned char*)res.data);
  }

  void hash_to_scalar(const void *data, size_t length, ec_scalar &res) {
    cn_fast_hash(data, length, reinterpret_cast<hash &>(res));
    sc_reduce32(&res);
  }

  /*
   * generate public and secret keys from a random 256-bit integer
   * TODO: allow specifying random value (for wallet recovery)
   *
   */
  secret_key crypto_ops::generate_keys(public_key &pub, secret_key &sec, const secret_key& recovery_key, bool recover) {
    ge_p3 point;

    secret_key rng;

    if (recover)
    {
      rng = recovery_key;
    }
    else
    {
      random_scalar(rng);
    }
    sec = rng;
    sc_reduce32(&unwrap(sec));  // reduce in case second round of keys (sendkeys)

    ge_scalarmult_base(&point, &unwrap(sec));
    ge_p3_tobytes(&pub, &point);

    return rng;
  }

  bool crypto_ops::check_key(const public_key &key) {
    ge_p3 point;
    return ge_frombytes_vartime(&point, &key) == 0;
  }

  bool crypto_ops::secret_key_to_public_key(const secret_key &sec, public_key &pub) {
    ge_p3 point;
    if (sc_check(&unwrap(sec)) != 0) {
      return false;
    }
    ge_scalarmult_base(&point, &unwrap(sec));
    ge_p3_tobytes(&pub, &point);
    return true;
  }

  bool crypto_ops::generate_key_derivation(const public_key &key1, const secret_key &key2, key_derivation &derivation) {
    ge_p3 point;
    ge_p2 point2;
    ge_p1p1 point3;
    assert(sc_check(&key2) == 0);
    if (ge_frombytes_vartime(&point, &key1) != 0) {
      return false;
    }
    ge_scalarmult(&point2, &unwrap(key2), &point);
    ge_mul8(&point3, &point2);
    ge_p1p1_to_p2(&point2, &point3);
    ge_tobytes(&derivation, &point2);
    return true;
  }

  void crypto_ops::derivation_to_scalar(const key_derivation &derivation, size_t output_index, ec_scalar &res) {
    struct {
      key_derivation derivation;
      char output_index[(sizeof(size_t) * 8 + 6) / 7];
    } buf;
    char *end = buf.output_index;
    buf.derivation = derivation;
    tools::write_varint(end, output_index);
    assert(end <= buf.output_index + sizeof buf.output_index);
    hash_to_scalar(&buf, end - reinterpret_cast<char *>(&buf), res);
  }

  bool crypto_ops::derive_public_key(const key_derivation &derivation, size_t output_index,
    const public_key &base, public_key &derived_key) {
    ec_scalar scalar;
    ge_p3 point1;
    ge_p3 point2;
    ge_cached point3;
    ge_p1p1 point4;
    ge_p2 point5;
    if (ge_frombytes_vartime(&point1, &base) != 0) {
      return false;
    }
    derivation_to_scalar(derivation, output_index, scalar);
    ge_scalarmult_base(&point2, &scalar);
    ge_p3_to_cached(&point3, &point2);
    ge_add(&point4, &point1, &point3);
    ge_p1p1_to_p2(&point5, &point4);
    ge_tobytes(&derived_key, &point5);
    return true;
  }

  struct s_comm {
    hash h;
    ec_point key;
    ec_point comm;
  };

  struct s_comm_2 {
    hash msg;
    ec_point D;
    ec_point X;
    ec_point Y;
  };

  static void hash_to_ec(const public_key &key, ge_p3 &res) {
    hash h;
    ge_p2 point;
    ge_p1p1 point2;
    cn_fast_hash(std::addressof(key), sizeof(public_key), h);
    ge_fromfe_frombytes_vartime(&point, reinterpret_cast<const unsigned char *>(&h));
    ge_mul8(&point2, &point);
    ge_p1p1_to_p3(&res, &point2);
  }

  void crypto_ops::generate_key_image(const public_key &pub, const secret_key &sec, key_image &image) {
    ge_p3 point;
    ge_p2 point2;
    assert(sc_check(&sec) == 0);
    hash_to_ec(pub, point);
    ge_scalarmult(&point2, &unwrap(sec), &point);
    ge_tobytes(&image, &point2);
  }

PUSH_WARNINGS
DISABLE_VS_WARNINGS(4200)
  struct ec_point_pair {
    ec_point a, b;
  };
  struct rs_comm {
    hash h;
    struct ec_point_pair ab[];
  };
POP_WARNINGS

  static inline size_t rs_comm_size(size_t pubs_count) {
    return sizeof(rs_comm) + pubs_count * sizeof(ec_point_pair);
  }

}


#include "Format.hxx"
#include "../Boost_Float.hxx"

#include <El.hpp>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

void handle_arguments(const int &argc, char **argv, El::BigFloat &threshold,
                      El::BigFloat &mesh_threshold, Format &format,
                      boost::filesystem::path &input_path,
                      boost::filesystem::path &solution_dir,
                      boost::filesystem::path &output_path, bool &need_lambda)
{
  int precision;
  std::string threshold_string, mesh_threshold_string, format_string;

  namespace po = boost::program_options;

  po::options_description options("Basic options");
  options.add_options()("help,h", "Show this helpful message.");
  options.add_options()(
    "input,i", po::value<boost::filesystem::path>(&input_path)->required(),
    "Mathematica, JSON, or NSV file with SDP definition");
  options.add_options()(
    "solution", po::value<boost::filesystem::path>(&solution_dir)->required(),
    "SDPB output directory containing the solutions for y and x (e.g. "
    "'y.txt')");
  options.add_options()(
    "threshold", po::value<std::string>(&threshold_string)->required(),
    "Threshold for when a functional is considered to be zero.");
  options.add_options()(
    "meshThreshold",
    po::value<std::string>(&mesh_threshold_string)->default_value("0.001"),
    "Relative error threshold for when to refine a mesh when approximating a "
    "functional to look for zeros.");
  options.add_options()("format",
                        po::value<std::string>(&format_string)->required(),
                        "Format of input file: Either PVM (Polynomial Vector "
                        "Matrix), or PMP (Positive Matrix with Prefactor).");
  options.add_options()(
    "output,o", po::value<boost::filesystem::path>(&output_path)->required(),
    "Output file");
  options.add_options()(
    "precision", po::value<int>(&precision)->required(),
    "The precision, in the number of bits, for numbers in the "
    "computation. ");
  options.add_options()("lambda",
                        po::value<bool>(&need_lambda)->default_value(true),
                        "If true, compute Λ and its associated error.");

  po::positional_options_description positional;
  positional.add("precision", 1);
  positional.add("input", 1);
  positional.add("output", 1);

  po::variables_map variables_map;
  po::store(po::parse_command_line(argc, argv, options), variables_map);

  if(variables_map.count("help") != 0)
    {
      std::cout << options << '\n';
      exit(0);
    }

  po::notify(variables_map);

  if(!boost::filesystem::exists(input_path))
    {
      throw std::runtime_error("Input file '" + input_path.string()
                               + "' does not exist");
    }
  if(boost::filesystem::is_directory(input_path))
    {
      throw std::runtime_error("Input file '" + input_path.string()
                               + "' is a directory, not a file");
    }
  if(!boost::filesystem::exists(solution_dir))
    {
      throw std::runtime_error("Solution file '" + solution_dir.string()
                               + "' does not exist");
    }

  if(output_path.filename_is_dot())
    {
      throw std::runtime_error("Output file '" + output_path.string()
                               + "' is a directory");
    }
  if(boost::filesystem::exists(output_path)
     && boost::filesystem::is_directory(output_path))
    {
      throw std::runtime_error("Output file '" + output_path.string()
                               + "' exists and is a directory");
    }

  El::gmp::SetPrecision(precision);
  // El::gmp wants base-2 bits, but boost::multiprecision wants
  // base-10 digits.
  Boost_Float::default_precision(precision * log(2) / log(10));

  threshold = El::BigFloat(threshold_string);
  mesh_threshold = El::BigFloat(mesh_threshold_string);

  if(format_string == "PVM")
    {
      format = Format::Polynomial_Vector_Matrix;
    }
  else if(format_string == "PMP")
    {
      format = Format::Positive_Matrix_with_Prefactor;
    }
  else
    {
      throw std::runtime_error(
        "Unknown format.  Expected 'PVM' or 'PMP', but found '" + format_string
        + "'");
    }
}


/*
 *  Copyright 2013 Néstor Morales Hernández <nestor@isaatc.ull.es>
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include <iostream>
#include <stdio.h>
#include <fstream>
#include <boost/concept_check.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <omp.h>

#include "polarcalibration.h"

PolarCalibration::PolarCalibration() {
    m_hessianThresh = 50;
    m_stepSize = STEP_SIZE;
    m_showCommonRegion = false;
    m_showIterations = false;
}

PolarCalibration::~PolarCalibration() {

}

bool PolarCalibration::compute(const cv::Mat& img1distorted, const cv::Mat& img2distorted,
                               const cv::Mat & cameraMatrix1, const cv::Mat & distCoeffs1,
                               const cv::Mat & cameraMatrix2, const cv::Mat & distCoeffs2,
                               const uint32_t method) {

    cv::Mat img1(img1distorted.rows, img1distorted.cols, CV_8UC1);
    cv::Mat img2(img2distorted.rows, img2distorted.cols, CV_8UC1);

    cv::undistort(img1distorted, img1, cameraMatrix1, distCoeffs1);
    cv::undistort(img2distorted, img2, cameraMatrix2, distCoeffs2);

    return compute(img1, img2);
}

bool PolarCalibration::compute(const cv::Mat& img1, const cv::Mat& img2, cv::Mat F,
                               vector< cv::Point2f > points1, vector< cv::Point2f > points2, const uint32_t method)
{
//     cv::Mat F;
    cv::Point2d epipole1, epipole2;
    if (! findFundamentalMat(img1, img2, F, points1, points2, epipole1, epipole2, method))
        return false;

    // Determine common region
    vector<cv::Vec3f> initialEpilines, finalEpilines;
    vector<cv::Point2f> epipoles(2);
    epipoles[0] = epipole1;
    epipoles[1] = epipole2;
    determineCommonRegion(epipoles, cv::Size(img1.cols, img1.rows), F);

    getTransformationPoints(img1.size(), epipole1, epipole2, F);
    doTransformation(img1, img2, epipole1, epipole2, F);

    return true;
}

inline bool PolarCalibration::findFundamentalMat(const cv::Mat& img1, const cv::Mat& img2, cv::Mat & F,
                                                 vector<cv::Point2f> points1, vector<cv::Point2f> points2,
                                                 cv::Point2d& epipole1, cv::Point2d& epipole2, const uint32_t method)
{
    if (F.empty()) {
        switch(method) {
            case FMAT_METHOD_OFLOW:
                findPairsOFlow(img1, img2, points1, points2);
                break;
            case FMAT_METHOD_SURF:
                findPairsSURF(img1, img2, points1, points2);
                break;
        }

        if (points1.size() < 8)
            return false;

        F = cv::findFundamentalMat(points1, points2, CV_FM_8POINT);

        if (cv::countNonZero(F) == 0)
            return false;
    }

    // We obtain the epipoles
    getEpipoles(F, epipole1, epipole2);

    checkF(F, epipole1, epipole2, points1[0], points2[0]);

    /// NOTE: Remove. Just for debugging (begin)
//     {
//         cout << "***********************" << endl;
//         cv::FileStorage file("/home/nestor/Dropbox/KULeuven/projects/PolarCalibration/testing/lastMat_I.xml", cv::FileStorage::READ);
// //         cv::FileStorage file("/home/nestor/Dropbox/KULeuven/projects/PolarCalibration/testing/results/mats/lastMat_0925.xml", cv::FileStorage::READ);
// //         cv::FileStorage file("/tmp/results/mats/lastMat_0039.xml", cv::FileStorage::READ);
//
//         file["F"] >> F;
//         file.release();
//         cout << "F (from file)\n" << F << endl;
//         getEpipoles(F, epipole1, epipole2);
//         cout << "epipole1 " << epipole1 << endl;
//         cout << "epipole2 " << epipole2 << endl;
//     }
    /// NOTE: Remove. Just for debugging (end)

    /// NOTE: Remove. Just for debugging (begin)
//     cv::FileStorage file("/home/nestor/Dropbox/KULeuven/projects/PolarCalibration/testing/lastMat_O.xml", cv::FileStorage::WRITE);
//     file << "F" << F;
//     file.release();
    /// NOTE: Remove. Just for debugging (end)

    if (SIGN(epipole1.x) != SIGN(epipole2.x) &&
        SIGN(epipole1.y) != SIGN(epipole2.y)) {

        epipole2 *= -1;
    }

    return true;
}

inline void PolarCalibration::findPairsOFlow(const cv::Mat & img1, const cv::Mat & img2,
                                             vector<cv::Point2f> & outPoints1, vector<cv::Point2f> & outPoints2) {

    // We look for correspondences using Optical flow
    // vector of keypoints
    vector<cv::KeyPoint> keypoints1;
    cv::FastFeatureDetector fastDetector(50);
    fastDetector.detect(img1, keypoints1);

    if (keypoints1.size() == 0)
        return;

    vector<cv::Point2f> points1(keypoints1.size()), points2, points1B;
    {
        uint32_t idx = 0;
        for (vector<cv::KeyPoint>::iterator it = keypoints1.begin(); it != keypoints1.end(); it++, idx++) {
            points1[idx] = it->pt;
        }
    }
    // Optical flow
    vector<uint8_t> status, statusB;
    vector<float_t> error, errorB;

    cv::calcOpticalFlowPyrLK(img1, img2, points1, points2, status, error, cv::Size(3, 3), 3);
    cv::calcOpticalFlowPyrLK(img2, img1, points2, points1B, statusB, errorB, cv::Size(3, 3), 3);

    vector<cv::Point2f> pointsA(points1.size()), pointsB(points2.size());
    {
        uint32_t idx = 0;
        for (uint32_t i = 0; i < points1.size(); i++) {
            if ((status[i] == 1) && (statusB[i] == 1)) {
                if (cv::norm(points1[i] - points1B[i]) < 1.0) {
                    pointsA[idx] = points1[i];
                    pointsB[idx] = points2[i];
                }
            }
            idx++;
        }
        pointsA.resize(idx);
        pointsB.resize(idx);
    }

    outPoints1 = pointsA;
    outPoints2 = pointsB;

}

inline void PolarCalibration::findPairsSURF(const cv::Mat & img1, const cv::Mat & img2,
                                                     vector<cv::Point2f> & outPoints1, vector<cv::Point2f> & outPoints2) {

    // We look for correspondences using SURF

    // vector of keypoints
    vector<cv::KeyPoint> keypoints1, keypoints2;

    cv::SurfFeatureDetector surf(m_hessianThresh);
    surf.detect(img1, keypoints1);
    surf.detect(img2, keypoints2);

    // Descriptors are extracted
    cv::SurfDescriptorExtractor surfDesc;
    cv::Mat descriptors1, descriptors2;
    surfDesc.compute(img1, keypoints1, descriptors1);
    surfDesc.compute(img2, keypoints2, descriptors2);

    // Descriptors are matched
    cv::FlannBasedMatcher matcher;
    vector<cv::DMatch> matches;

    matcher.match(descriptors1, descriptors2, matches);

    nth_element(matches.begin(), matches.begin()+24, matches.end());
    matches.erase(matches.begin()+25, matches.end());

//     cv::Mat imageMatches;
//     cv::drawMatches(img1, keypoints1, img2, keypoints2, matches, imageMatches, cv::Scalar(0,0,255));
//     cv::namedWindow("Matched");
//     cv::imshow("Matched", imageMatches);

    // Fundamental matrix is found
    vector<cv::Point2f> points1, points2;

    points1.resize(matches.size());
    points2.resize(matches.size());

    {
        uint32_t idx2 = 0;
        for (int idx = 0; idx < matches.size(); idx++) {
            const cv::Point2f & p1 = keypoints1[matches[idx].queryIdx].pt;
            const cv::Point2f & p2 = keypoints2[matches[idx].trainIdx].pt;

            if (fabs(p1.x - p2.x < 10.0) && fabs(p1.y - p2.y < 10.0)) {
                points1[idx2] = p1;
                points2[idx2] = p2;
                idx2++;
            }
        }
        points1.resize(idx2);
        points2.resize(idx2);
    }

    outPoints1 = points1;
    outPoints2 = points2;
}

inline void PolarCalibration::getEpipoles(const cv::Mat & F, cv::Point2d & epipole1, cv::Point2d & epipole2) {
    cv::SVD svd(F);

    cv::Mat e1 = svd.vt.row(2);
    cv::Mat e2 = svd.u.col(2);

    epipole1 = cv::Point2d(e1.at<double>(0, 0) / e1.at<double>(0, 2), e1.at<double>(0, 1) / e1.at<double>(0, 2));
    epipole2 = cv::Point2d(e2.at<double>(0, 0) / e2.at<double>(2, 0), e2.at<double>(1, 0) / e2.at<double>(2, 0));
}

inline void PolarCalibration::checkF(cv::Mat & F, cv::Point2d & epipole1, cv::Point2d & epipole2, const cv::Point2d & m, const cv::Point2d & m1) {
    cv::Vec3f line = GET_LINE_FROM_POINTS(epipole1, m);
    vector<cv::Point2f> points(1);
    points[0] = epipole1;
    vector<cv::Vec3f> lines;
    cv::computeCorrespondEpilines(points, 1, F, lines);
    cv::Vec3f line1 = lines[0];

    cv::Mat L(1, 3, CV_64FC1);
    L.at<double>(0, 0) = line[0];
    L.at<double>(0, 1) = line[1];
    L.at<double>(0, 2) = line[2];

    cv::Mat L1(1, 3, CV_64FC1);
    L1.at<double>(0, 0) = line1[0];
    L1.at<double>(0, 1) = line1[1];
    L1.at<double>(0, 2) = line1[2];

    cv::Mat M(3, 1, CV_64FC1);
    M.at<double>(0, 0) = m.x;
    M.at<double>(1, 0) = m.y;
    M.at<double>(2, 0) = 1.0;

    cv::Mat M1(3, 1, CV_64FC1);
    M1.at<double>(0, 0) = m1.x;
    M1.at<double>(1, 0) = m1.y;
    M1.at<double>(2, 0) = 1.0;

    cv::Mat fl = L * M;
    cv::Mat fl1 = L1 * M1;

    if (SIGN(fl.at<double>(0,0)) != SIGN(fl1.at<double>(0,0))) {
        F = -F;

        getEpipoles(F, epipole1, epipole2);
    }
}

inline bool PolarCalibration::lineIntersectsSegment(const cv::Vec3d & line, const cv::Point2d & p1, const cv::Point2d & p2, cv::Point2d * intersection) {
    const cv::Vec3d segment = GET_LINE_FROM_POINTS(p1, p2);

    if (intersection != NULL)
        *intersection = cv::Point2d(std::numeric_limits<double>::max(), std::numeric_limits<double>::max());

    // Lines are represented as ax + by + c = 0, so
    // y = -(ax+c)/b. If y1=y2, then we have to obtain x, which is
    // x = (b1 * c2 - b2 * c1) / (b2 * a1 - b1 * a2)
    if ((segment[1] * line[0] - line[1] * segment[0]) == 0)
        return false;
    double x = (line[1] * segment[2] - segment[1] * line[2]) / (segment[1] * line[0] - line[1] * segment[0]);
    double y = -(line[0] * x + line[2]) / line[1];

    if (((int32_t)round(x) >= (int32_t)min(p1.x, p2.x)) && ((int32_t)round(x) <= (int32_t)max(p1.x, p2.x))) {
        if (((int32_t)round(y) >= (int32_t)min(p1.y, p2.y)) && ((int32_t)round(y) <= (int32_t)max(p1.y, p2.y))) {
            if (intersection != NULL)
                *intersection = cv::Point2d(x, y);

            return true;
        }
    }

    return false;
}

inline bool PolarCalibration::lineIntersectsRect(const cv::Vec3d & line, const cv::Size & imgDimensions, cv::Point2d * intersection) {
    return lineIntersectsSegment(line, cv::Point2d(0, 0), cv::Point2d(imgDimensions.width - 1, 0), intersection) ||
            lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, 0), cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), intersection) ||
            lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), cv::Point2d(0, imgDimensions.height - 1), intersection) ||
            lineIntersectsSegment(line, cv::Point2d(0, imgDimensions.height - 1), cv::Point2d(0, 0), intersection);
}

inline bool PolarCalibration::isTheRightPoint(const cv::Point2d & epipole, const cv::Point2d & intersection, const cv::Vec3d & line,
                                       const cv::Point2d * lastPoint)
{
    if (lastPoint != NULL) {
        cv::Vec3f v1(lastPoint->x - epipole.x, lastPoint->y - epipole.y, 0.0);
        v1 /= cv::norm(v1);
        cv::Vec3f v2(intersection.x - epipole.x, intersection.y - epipole.y, 0.0);
        v2 /= cv::norm(v2);

        if (fabs(acos(v1.dot(v2))) > CV_PI / 2.0)
            return false;
        else
            return true;
    } else {
        if ((line[0] > 0) && (epipole.y < intersection.y)) return false;
        if ((line[0] < 0) && (epipole.y > intersection.y)) return false;
        if ((line[1] > 0) && (epipole.x > intersection.x)) return false;
        if ((line[1] < 0) && (epipole.x < intersection.x)) return false;

        return true;
    }
    return false;
}

inline
void PolarCalibration::getBorderIntersections(const cv::Point2d & epipole, const cv::Vec3d & line, const cv::Size & imgDimensions,
                                                     vector<cv::Point2d> & intersections) {

    cv::Point2d intersection(-1, -1);
    intersections.reserve(2);

    if (lineIntersectsSegment(line, cv::Point2d(0, 0), cv::Point2d(imgDimensions.width - 1, 0), &intersection)) {
        intersections.push_back(intersection);
    }
    if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, 0), cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), &intersection)) {
        intersections.push_back(intersection);
    }
    if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), cv::Point2d(0, imgDimensions.height - 1), &intersection)) {
        intersections.push_back(intersection);
    }
    if (lineIntersectsSegment(line, cv::Point2d(0, imgDimensions.height - 1), cv::Point2d(0, 0), &intersection)) {
        intersections.push_back(intersection);
    }
}

inline
cv::Point2d PolarCalibration::getNearestIntersection(const cv::Point2d& oldEpipole, const cv::Point2d& newEpipole,
                                                    const cv::Vec3d& line, const cv::Point2d& oldPoint,
                                                    const cv::Size & imgDimensions)
{
    vector<cv::Point2d> intersections;
    getBorderIntersections(newEpipole, line, imgDimensions, intersections);

    double minAngle = std::numeric_limits<double>::max();
    cv::Point2d point(-1, -1);

    cv::Vec3d v1(oldPoint.x - oldEpipole.x, oldPoint.y - oldEpipole.y, 0.0);
    v1 /= cv::norm(v1);

    for (uint32_t i = 0; i < intersections.size(); i++) {
        cv::Vec3d v(intersections[i].x - newEpipole.x, intersections[i].y - newEpipole.y, 0.0);
        v /= cv::norm(v);

        const double & angle = fabs(acos(v.dot(v1)));

        if (angle < minAngle) {
            minAngle = angle;
            point = intersections[i];
        }
    }

    return point;
}

inline
cv::Point2d PolarCalibration::getBorderIntersection(const cv::Point2d & epipole, const cv::Vec3d & line, const cv::Size & imgDimensions,
                                                            const cv::Point2d * lastPoint) {

    cv::Point2d intersection(-1, -1);

    if (IS_INSIDE_IMAGE(epipole, imgDimensions)) {
        if (lineIntersectsSegment(line, cv::Point2d(0, 0), cv::Point2d(imgDimensions.width - 1, 0), &intersection)) {
            if (isTheRightPoint(epipole, intersection, line, lastPoint)) {

                return intersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, 0), cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), &intersection)) {
            if (isTheRightPoint(epipole, intersection, line, lastPoint)) {

                return intersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), cv::Point2d(0, imgDimensions.height - 1), &intersection)) {
            if (isTheRightPoint(epipole, intersection, line, lastPoint)) {

                return intersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(0, imgDimensions.height - 1), cv::Point2d(0, 0), &intersection)) {
            if (isTheRightPoint(epipole, intersection, line, lastPoint)) {

                return intersection;
            }
        }
    } else {
        double maxDist = std::numeric_limits<double>::min();
        cv::Point2d tmpIntersection(-1, -1);
        if (lineIntersectsSegment(line, cv::Point2d(0, 0), cv::Point2d(imgDimensions.width - 1, 0), &tmpIntersection)) {
            const double dist2 = (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x) +
                                 (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x);

            if (dist2 > maxDist) {
                maxDist = dist2;
                intersection = tmpIntersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, 0), cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), &tmpIntersection)) {
            const double dist2 = (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x) +
            (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x);

            if (dist2 > maxDist) {
                maxDist = dist2;
                intersection = tmpIntersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(imgDimensions.width - 1, imgDimensions.height - 1), cv::Point2d(0, imgDimensions.height - 1), &tmpIntersection)) {
            const double dist2 = (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x) +
            (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x);

            if (dist2 > maxDist) {
                maxDist = dist2;
                intersection = tmpIntersection;
            }
        }
        if (lineIntersectsSegment(line, cv::Point2d(0, imgDimensions.height - 1), cv::Point2d(0, 0), &tmpIntersection)) {
            const double dist2 = (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x) +
            (tmpIntersection.x - epipole.x) * (tmpIntersection.x - epipole.x);

            if (dist2 > maxDist) {
                maxDist = dist2;
                intersection = tmpIntersection;
            }
        }
        return intersection;
    }
}

inline void PolarCalibration::getExternalPoints(const cv::Point2d &epipole, const cv::Size imgDimensions,
                                                vector<cv::Point2f> &externalPoints) {

    if (epipole.y < 0) { // Cases 1, 2 and 3
        if (epipole.x < 0) { // Case 1
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(imgDimensions.width - 1, 0);
            externalPoints[1] = cv::Point2f(0, imgDimensions.height - 1);
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 2
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(imgDimensions.width - 1, 0);
            externalPoints[1] = cv::Point2f(0, 0);
        } else { // Case 3
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(imgDimensions.width - 1, imgDimensions.height - 1);
            externalPoints[1] = cv::Point2f(0, 0);
        }
    } else if (epipole.y <= imgDimensions.height - 1) { // Cases 4, 5 and 6
        if (epipole.x < 0) { // Case 4
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(0, 0);
            externalPoints[1] = cv::Point2f(0, imgDimensions.height - 1);
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 5
            externalPoints.resize(4);
            externalPoints[0] = cv::Point2f(0, 0);
            externalPoints[1] = cv::Point2f(imgDimensions.width - 1, 0);
            externalPoints[2] = cv::Point2f(imgDimensions.width - 1, imgDimensions.height - 1);
            externalPoints[3] = cv::Point2f(0, imgDimensions.height - 1);
        } else { // Case 6
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(imgDimensions.width - 1, imgDimensions.height - 1);
            externalPoints[1] = cv::Point2f(imgDimensions.width - 1, 0);
        }
    } else { // Cases 7, 8 and 9
        if (epipole.x < 0) { // Case 7
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(0, 0);
            externalPoints[1] = cv::Point2f(imgDimensions.width - 1, imgDimensions.height - 1);
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 8
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(0, imgDimensions.height - 1);
            externalPoints[1] = cv::Point2f(imgDimensions.width - 1, imgDimensions.height - 1);
        } else { // Case 9
            externalPoints.resize(2);
            externalPoints[0] = cv::Point2f(0, imgDimensions.height - 1);
            externalPoints[1] = cv::Point2f(imgDimensions.width - 1, 0);
        }
    }
}

inline void PolarCalibration::computeEpilines(const vector<cv::Point2f> & points, const uint32_t &whichImage,
                                            const cv::Mat & F, const vector <cv::Vec3f> & oldlines, vector <cv::Vec3f> & newLines) {

    cv::computeCorrespondEpilines(points, whichImage, F, newLines);

    for (uint32_t i = 0; i < oldlines.size(); i++) {
        if ((SIGN(oldlines[i][0]) != SIGN(newLines[i][0])) &&
            (SIGN(oldlines[i][1]) != SIGN(newLines[i][1]))) {
            newLines[i] *= -1;
        }
    }
}

inline cv::Point2d PolarCalibration::getBorderIntersectionFromOutside(const cv::Point2d & epipole, const cv::Vec3d & line, const cv::Size & imgDimensions)
{
    vector<cv::Point2d> intersections;
    getBorderIntersections(epipole, line, imgDimensions, intersections);
    double maxDist = std::numeric_limits<double>::min();
    cv::Point2d returnPoint;
    for (uint32_t i = 0; i < intersections.size(); i++) {
        const double dist = cv::norm(epipole - intersections[i]);
        if (dist > maxDist) {
            maxDist = dist;
            returnPoint = intersections[i];
        }
    }

    return returnPoint;
}

/**
 * This function is more easily understandable after reading section 3.4 of
 * ftp://cmp.felk.cvut.cz/pub/cmp/articles/matousek/Sandr-TR-2009-04.pdf
 * */
inline void PolarCalibration::determineCommonRegion(const vector<cv::Point2f> &epipoles,
                                             const cv::Size imgDimensions, const cv::Mat & F) {

    vector<cv::Point2f> externalPoints1, externalPoints2;
    getExternalPoints(epipoles[0], imgDimensions, externalPoints1);
    getExternalPoints(epipoles[1], imgDimensions, externalPoints2);

    determineRhoRange(epipoles[0], imgDimensions, externalPoints1, m_minRho1, m_maxRho1);
    determineRhoRange(epipoles[1], imgDimensions, externalPoints2, m_minRho2, m_maxRho2);

    if (!IS_INSIDE_IMAGE(epipoles[0], imgDimensions) && !IS_INSIDE_IMAGE(epipoles[1], imgDimensions)) {
        // CASE 1: Both outside
        const cv::Vec3f line11 = GET_LINE_FROM_POINTS(epipoles[0], externalPoints1[0]);
        const cv::Vec3f line12 = GET_LINE_FROM_POINTS(epipoles[0], externalPoints1[1]);

        const cv::Vec3f line23 = GET_LINE_FROM_POINTS(epipoles[1], externalPoints2[0]);
        const cv::Vec3f line24 = GET_LINE_FROM_POINTS(epipoles[1], externalPoints2[1]);

        vector <cv::Vec3f> inputLines(2), outputLines;
        inputLines[0] = line23;
        inputLines[1] = line24;
        computeEpilines(externalPoints2, 2, F, inputLines, outputLines);
        const cv::Vec3f line13 = outputLines[0];
        const cv::Vec3f line14 = outputLines[1];

        inputLines[0] = line11;
        inputLines[1] = line12;
        computeEpilines(externalPoints1, 1, F, inputLines, outputLines);
        const cv::Vec3f line21 = outputLines[0];
        const cv::Vec3f line22 = outputLines[1];

        // Beginning and ending lines
        m_line1B = lineIntersectsRect(line13, imgDimensions)? line13 : line11;
        m_line1E = lineIntersectsRect(line14, imgDimensions)? line14 : line12;
        m_line2B = lineIntersectsRect(line21, imgDimensions)? line21 : line23;
        m_line2E = lineIntersectsRect(line22, imgDimensions)? line22 : line24;

        // Beginning and ending lines intersection with the borders
        {
            vector<cv::Point2d> intersections;
            getBorderIntersections(epipoles[0], m_line1B, imgDimensions, intersections);
            double maxDist = std::numeric_limits<double>::min();
            for (uint32_t i = 0; i < intersections.size(); i++) {
                const cv::Point2f intersect = intersections[i];
                const double dist = cv::norm(epipoles[0] - intersect);
                if (dist > maxDist) {
                    maxDist = dist;
                    m_b1 = intersections[i];
                }
            }
        }
        {
            vector<cv::Point2d> intersections;
            getBorderIntersections(epipoles[1], m_line2B, imgDimensions, intersections);
            double maxDist = std::numeric_limits<double>::min();
            for (uint32_t i = 0; i < intersections.size(); i++) {
                const cv::Point2f intersect = intersections[i];
                const double dist = cv::norm(epipoles[1] - intersect);
                if (dist > maxDist) {
                    maxDist = dist;
                    m_b2 = intersections[i];
                }
            }
        }
        {
            vector<cv::Point2d> intersections;
            getBorderIntersections(epipoles[0], m_line1E, imgDimensions, intersections);
            double maxDist = std::numeric_limits<double>::min();
            for (uint32_t i = 0; i < intersections.size(); i++) {
                const cv::Point2f intersect = intersections[i];
                const double dist = cv::norm(epipoles[0] - intersect);
                if (dist > maxDist) {
                    maxDist = dist;
                    m_e1 = intersections[i];
                }
            }
        }
        {
            vector<cv::Point2d> intersections;
            getBorderIntersections(epipoles[1], m_line2E, imgDimensions, intersections);
            double maxDist = std::numeric_limits<double>::min();
            for (uint32_t i = 0; i < intersections.size(); i++) {
                const cv::Point2f intersect = intersections[i];
                const double dist = cv::norm(epipoles[1] - intersect);
                if (dist > maxDist) {
                    maxDist = dist;
                    m_e1 = intersections[i];
                }
            }
        }

        if (m_showCommonRegion) {
            showCommonRegion(epipoles[0], line11, line12, line13, line14, m_line1B, m_line1E, m_b1, m_e1, imgDimensions,
                            externalPoints1, std::string("leftCommonRegion"));
            showCommonRegion(epipoles[1], line23, line24, line21, line22, m_line2B, m_line2E, m_b2, m_e2, imgDimensions,
                            externalPoints2, std::string("rightCommonRegion"));
        }

    } else if (IS_INSIDE_IMAGE(epipoles[0], imgDimensions) && IS_INSIDE_IMAGE(epipoles[1], imgDimensions)) {
        // CASE 2: Both inside
        m_line1B = GET_LINE_FROM_POINTS(epipoles[0], externalPoints1[0]);
        m_line1E = m_line1B;

        vector <cv::Vec3f> inputLines(1), outputLines;
        inputLines[0] = m_line1B;
        computeEpilines(externalPoints1, 1, F, inputLines, outputLines);

        m_line2B = outputLines[0];
        m_line2E = outputLines[0];

        m_b1 = getBorderIntersection(epipoles[0], m_line1B, imgDimensions);
        m_e1 = getBorderIntersection(epipoles[0], m_line1E, imgDimensions);

        m_b2 = m_e2 = getNearestIntersection(epipoles[0], epipoles[1], m_line2B, m_b1, imgDimensions);

        if (m_showCommonRegion) {
            showCommonRegion(epipoles[0], m_line1B, m_line1E, m_line2B, m_line2E, m_line1B, m_line1E, m_b1, m_e1, imgDimensions,
                            externalPoints1, std::string("leftCommonRegion"));
            showCommonRegion(epipoles[1], m_line2B, m_line2E, m_line1B, m_line1E, m_line2B, m_line2E, m_b2, m_e2, imgDimensions,
                            externalPoints2, std::string("rightCommonRegion"));
        }
    } else {
        // CASE 3: One inside and one outside
        if (IS_INSIDE_IMAGE(epipoles[0], imgDimensions)) {
            // CASE 3.1: Only the first epipole is inside

            const cv::Vec3f line23 = GET_LINE_FROM_POINTS(epipoles[1], externalPoints2[0]);
            const cv::Vec3f line24 = GET_LINE_FROM_POINTS(epipoles[1], externalPoints2[1]);

            vector <cv::Vec3f> inputLines(2), outputLines;
            inputLines[0] = line23;
            inputLines[1] = line24;
            computeEpilines(externalPoints2, 2, F, inputLines, outputLines);
            const cv::Vec3f & line13 = outputLines[0];
            const cv::Vec3f & line14 = outputLines[1];

            m_line1B = line13;
            m_line1E = line14;
            m_line2B = line23;
            m_line2E = line24;

            m_b2 = getBorderIntersection(epipoles[1], m_line2B, imgDimensions);
            m_e2 = getBorderIntersection(epipoles[1], m_line2E, imgDimensions);

            m_b1 = getNearestIntersection(epipoles[1], epipoles[0], m_line1B, m_b2, imgDimensions);
            m_e1 = getNearestIntersection(epipoles[1], epipoles[0], m_line1E, m_e2, imgDimensions);

            if (m_showCommonRegion) {
                showCommonRegion(epipoles[0], line13, line14, line13, line14, m_line1B, m_line1E, m_b1, m_e1, imgDimensions,
                                externalPoints1, std::string("leftCommonRegion"));
                showCommonRegion(epipoles[1], line23, line24, line23, line24, m_line2B, m_line2E, m_b2, m_e2, imgDimensions,
                                externalPoints2, std::string("rightCommonRegion"));
            }
        } else {
            // CASE 3.2: Only the second epipole is inside
            const cv::Vec3f line11 = GET_LINE_FROM_POINTS(epipoles[0], externalPoints1[0]);
            const cv::Vec3f line12 = GET_LINE_FROM_POINTS(epipoles[0], externalPoints1[1]);

            vector <cv::Vec3f> inputLines(2), outputLines;
            inputLines[0] = line11;
            inputLines[1] = line12;
            computeEpilines(externalPoints1, 1, F, inputLines, outputLines);
            const cv::Vec3f & line21 = outputLines[0];
            const cv::Vec3f & line22 = outputLines[1];

            m_line1B = line11;
            m_line1E = line12;
            m_line2B = line21;
            m_line2E = line22;

            m_b1 = getBorderIntersection(epipoles[0], m_line1B, imgDimensions);
            m_e1 = getBorderIntersection(epipoles[0], m_line1E, imgDimensions);

            m_b2 = getNearestIntersection(epipoles[0], epipoles[1], m_line2B, m_b1, imgDimensions);
            m_e2 = getNearestIntersection(epipoles[0], epipoles[1], m_line2E, m_e1, imgDimensions);

            if (m_showCommonRegion) {
                showCommonRegion(epipoles[0], line11, line12, line11, line12, m_line1B, m_line1E, m_b1, m_e1, imgDimensions,
                                externalPoints1, std::string("leftCommonRegion"));
                showCommonRegion(epipoles[1], line21, line22, line21, line22, m_line2B, m_line2E, m_b2, m_e2, imgDimensions,
                                externalPoints2, std::string("rightCommonRegion"));
            }
        }
    }

    if (m_showCommonRegion) {
        cv::moveWindow("rightCommonRegion", imgDimensions.width + 10, 0);
    }
}

inline void PolarCalibration::determineRhoRange(const cv::Point2d &epipole, const cv::Size imgDimensions,
                       const vector<cv::Point2f> &externalPoints, double & minRho, double & maxRho) {
    if (epipole.y < 0) { // Cases 1, 2 and 3
        if (epipole.x < 0) { // Case 1
            minRho = sqrt(epipole.x * epipole.x + epipole.y * epipole.y);         // Point A
            maxRho = sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                          ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y));        // Point D
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 2
            minRho = -epipole.y;
            maxRho = max(sqrt(epipole.x * epipole.x +
                              ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y)),        // Point C
                         sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                              ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y))        // Point D
                        );
        } else { // Case 3
            minRho = sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                          epipole.y * epipole.y);        // Point B
            maxRho = sqrt(epipole.x * epipole.x +
                          ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y));        // Point C
        }
    } else if (epipole.y <= imgDimensions.height - 1) { // Cases 4, 5 and 6
        if (epipole.x < 0) { // Case 4
            minRho = -epipole.x;
            maxRho = max(
                         sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                              ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y)),        // Point D
                         sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                              epipole.y * epipole.y)        // Point B
                     );
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 5
            minRho = 0;
            maxRho = max(
                         max(
                             sqrt(epipole.x * epipole.x + epipole.y * epipole.y),        // Point A
                             sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                                  epipole.y * epipole.y)        // Point B
                         ),
                         max(
                             sqrt(epipole.x * epipole.x +
                                  ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y)),        // Point C
                             sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                                  ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y))        // Point D
                         )
                     );
        } else { // Case 6
            minRho = epipole.x - (imgDimensions.width - 1);
            maxRho = max(
                         sqrt(epipole.x * epipole.x + epipole.y * epipole.y),        // Point A
                         sqrt(epipole.x * epipole.x +
                              ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y))        // Point C
                     );
        }
    } else { // Cases 7, 8 and 9
        if (epipole.x < 0) { // Case 7
            minRho = sqrt(epipole.x * epipole.x +
                          ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y));        // Point C
            maxRho = sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                          epipole.y * epipole.y);        // Point B
        } else if (epipole.x <= imgDimensions.width - 1) { // Case 8
            minRho = epipole.y - (imgDimensions.height - 1);
            maxRho = max(
                         sqrt(epipole.x * epipole.x + epipole.y * epipole.y),        // Point A
                         sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                              epipole.y * epipole.y)        // Point B

                     );
        } else { // Case 9
            minRho = sqrt(((imgDimensions.width - 1) - epipole.x) * ((imgDimensions.width - 1) - epipole.x) +
                          ((imgDimensions.height - 1) - epipole.y) * ((imgDimensions.height - 1) - epipole.y));        // Point D
            maxRho = sqrt(epipole.x * epipole.x + epipole.y * epipole.y);        // Point A
        }
    }
}

inline void PolarCalibration::getNewPointAndLineSingleImage(const cv::Point2d epipole1, const cv::Point2d epipole2, const cv::Size & imgDimensions,
                                    const cv::Mat & F, const uint32_t & whichImage, const cv::Point2d & pOld1, const cv::Point2d & pOld2,
                                   cv::Vec3f & prevLine, cv::Point2d & pNew1, cv::Vec3f & newLine1,
                                   cv::Point2d & pNew2, cv::Vec3f & newLine2) {

    // We obtain vector v
    cv::Vec2f v;

    cv::Vec3f vBegin(m_b1.x - epipole1.x, m_b1.y - epipole1.y, 0.0);
    cv::Vec3f vCurr(pOld1.x - epipole1.x, pOld1.y - epipole1.y, 0.0);
    cv::Vec3f vEnd(m_e1.x - epipole1.x, m_e1.y - epipole1.y, 0.0);

    vBegin /= cv::norm(vBegin);
    vCurr /= cv::norm(vCurr);
    vEnd /= cv::norm(vEnd);

    if (IS_INSIDE_IMAGE(epipole1, imgDimensions)) {
        if (IS_INSIDE_IMAGE(epipole2, imgDimensions)) {
            v = cv::Vec2f(vCurr[1], -vCurr[0]);
        } else {
            vBegin = cv::Vec3f(m_b2.x - epipole2.x, m_b2.y - epipole2.y, 0.0);
            vCurr = cv::Vec3f(pOld2.x - epipole2.x, pOld2.y - epipole2.y, 0.0);
            vEnd = cv::Vec3f(m_e2.x - epipole2.x, m_e2.y - epipole2.y, 0.0);

            vBegin /= cv::norm(vBegin);
            vCurr /= cv::norm(vCurr);
            vEnd /= cv::norm(vEnd);

            const cv::Vec3f vCross = vBegin.cross(vEnd);

            v = cv::Vec2f(vCurr[1], -vCurr[0]);
            if (vCross[2] > 0.0) {
                v = -v;
            }
        }
    } else {
        const cv::Vec3f vCross = vBegin.cross(vEnd);

        v = cv::Vec2f(vCurr[1], -vCurr[0]);
        if (vCross[2] > 0.0) {
            v = -v;
        }
    }

    pNew1 = cv::Point2d(pOld1.x + v[0] * m_stepSize, pOld1.y + v[1] * m_stepSize);
    newLine1 = GET_LINE_FROM_POINTS(epipole1, pNew1);

    if (! IS_INSIDE_IMAGE(epipole1, imgDimensions)) {
        pNew1 = getBorderIntersection(epipole1, newLine1, imgDimensions, &pOld1);
    } else {
        pNew1 = getNearestIntersection(epipole1, epipole1, newLine1, pOld1, imgDimensions);
    }

    vector<cv::Point2f> points(1);
    points[0] = pNew1;
    vector<cv::Vec3f> inLines(1);
    inLines[0] = newLine1;
    vector<cv::Vec3f> outLines(1);
    computeEpilines(points, whichImage, F, inLines, outLines);
    newLine2 = outLines[0];

    if (! IS_INSIDE_IMAGE(epipole2, imgDimensions)) {
        cv::Point2d tmpPoint = getBorderIntersection(epipole2, newLine2, imgDimensions, &pOld2);
        pNew2 = tmpPoint;
    } else {
        vector <cv::Point2d> intersections;
        getBorderIntersections(epipole2, newLine2, imgDimensions, intersections);
        pNew2 = intersections[0];

        double minDist = std::numeric_limits<double>::max();
        for (uint32_t i = 0; i < intersections.size(); i++) {
            const double dist = (pOld2.x - intersections[i].x) * (pOld2.x - intersections[i].x) +
                                (pOld2.y - intersections[i].y) * (pOld2.y - intersections[i].y);
            if (minDist > dist) {
                minDist = dist;
                pNew2 = intersections[i];
            }
        }
    }

}

inline void PolarCalibration::getNewEpiline(const cv::Point2d epipole1, const cv::Point2d epipole2, const cv::Size & imgDimensions,
                                     const cv::Mat & F, const cv::Point2d pOld1, const cv::Point2d pOld2,
                                     cv::Vec3f prevLine1, cv::Vec3f prevLine2,
                                     cv::Point2d & pNew1, cv::Point2d & pNew2, cv::Vec3f & newLine1, cv::Vec3f & newLine2) {

    getNewPointAndLineSingleImage(epipole1, epipole2, imgDimensions, F, 1, pOld1, pOld2, prevLine1, pNew1, newLine1, pNew2, newLine2);

    //TODO If the distance is too big in image 2, we do it in the opposite sense
//     double distImg2 = (pOld2.x - pNew2.x) * (pOld2.x - pNew2.x) + (pOld2.y - pNew2.y) * (pOld2.y - pNew2.y);
//     if (distImg2 > m_stepSize * m_stepSize)
//         getNewPointAndLineSingleImage(epipole2, epipole1, imgDimensions, F, 2, pOld2, pOld1, prevLine2, pNew2, newLine2, pNew1, newLine1);

    if (m_showIterations) {
        showNewEpiline(epipole1, m_line1B, m_line1E, newLine1, pOld1, pNew1, imgDimensions, std::string("newEpiline1"));
        showNewEpiline(epipole2, m_line2B, m_line2E, newLine2, pOld2, pNew2, imgDimensions, std::string("newEpiline2"));
        cv::moveWindow("newEpiline2", imgDimensions.width +10, 0);
    }
}

inline void PolarCalibration::transformLine(const cv::Point2d& epipole, const cv::Point2d& p2, const cv::Mat& inputImage,
                                            const uint32_t & thetaIdx, const double &minRho, const double & maxRho,
                                            cv::Mat& mapX, cv::Mat& mapY, cv::Mat& inverseMapX, cv::Mat& inverseMapY)
{
    cv::Vec2f v(p2.x - epipole.x, p2.y - epipole.y);
    double maxDist = cv::norm(v);
    v /= maxDist;

    {
        uint32_t rhoIdx = 0;
        for (double rho = minRho; rho <= min(maxDist, maxRho); rho += 1.0, rhoIdx++) {
            cv::Point2d target(v[0] * rho + epipole.x, v[1] * rho + epipole.y);
            if ((target.x >= 0) && (target.x < inputImage.cols) &&
                (target.y >= 0) && (target.y < inputImage.rows)) {

                mapX.at<float>(thetaIdx, rhoIdx) = target.x;
                mapY.at<float>(thetaIdx, rhoIdx) = target.y;

                inverseMapX.at<float>(target.y, target.x) = rhoIdx;
                inverseMapY.at<float>(target.y, target.x) = thetaIdx;
            }
        }
    }
}

void PolarCalibration::getTransformationPoints(const cv::Size& imgDimensions, const cv::Point2d epipole1, const cv::Point2d epipole2, const cv::Mat& F)
{
    cv::Point2d p1 = m_b1, p2 = m_b2;
    cv::Vec3f line1 = m_line1B, line2 = m_line2B;

    m_thetaPoints1.clear();
    m_thetaPoints2.clear();
    m_thetaPoints1.reserve(2 * (imgDimensions.width + imgDimensions.height));
    m_thetaPoints2.reserve(2 * (imgDimensions.width + imgDimensions.height));

    int32_t crossesLeft = 0;
    if (IS_INSIDE_IMAGE(epipole1, imgDimensions) && IS_INSIDE_IMAGE(epipole2, imgDimensions))
        crossesLeft++;

    uint32_t thetaIdx = 0;
    double lastCrossProd = 0;

    while (true) {
        m_thetaPoints1.push_back(p1);
        m_thetaPoints2.push_back(p2);
//         transformLine(epipole1, p1, img1, thetaIdx, m_minRho1, m_maxRho1, m_mapX1, m_mapY1, m_inverseMapX1, m_inverseMapY1);
//         transformLine(epipole2, p2, img2, thetaIdx, m_minRho2, m_maxRho2, m_mapX2, m_mapY2, m_inverseMapX2, m_inverseMapY2);

        cv::Vec3f v0(p1.x - epipole1.x, p1.y - epipole1.y, 1.0);
        v0 /= cv::norm(v0);
        cv::Point2d oldP1 = p1;

        getNewEpiline(epipole1, epipole2, imgDimensions, F, p1, p2, line1, line2, p1, p2, line1, line2);

        // Check if we reached the end
        cv::Vec3f v1(p1.x - epipole1.x, p1.y - epipole1.y, 0.0);
        v1 /= cv::norm(v1);
        cv::Vec3f v2(m_e1.x - epipole1.x, m_e1.y - epipole1.y, 0.0);
        v2 /= cv::norm(v2);
        cv::Vec3f v3(oldP1.x - epipole1.x, oldP1.y - epipole1.y, 0.0);
        v3 /= cv::norm(v3);

        double crossProd = v1.cross(v2)[2];

        if (thetaIdx != 0) {
            if ((SIGN(lastCrossProd) != SIGN(crossProd)) || (fabs(acos(v1.dot(-v3))) < 0.01) || (p1 == cv::Point2d(-1, -1)))
                crossesLeft--;

            if ((crossesLeft < 0)) {
                break;
            }
        }
        lastCrossProd = crossProd;
        thetaIdx++;

        if (m_showIterations) {
            int keycode = cv::waitKey(0);

            // q: exit
            if (keycode == 113) {
                exit(0);
            }
            // 1: stepSize = 1
            if (keycode == 49) {
                m_stepSize = 1;
            }
            // 2: stepSize = 50
            if (keycode == 50) {
                m_stepSize = 10;
            }
            // 3: stepSize = 50
            if (keycode == 51) {
                m_stepSize = 50;
            }
            // 4: stepSize = 100
            if (keycode == 51) {
                m_stepSize = 100;
            }
            // n: next image
            if (keycode == 110) {
                break;
            }
            // r: reset current image
            if (keycode == 114) {
                p1 = m_b1;
                p2 = m_b2;
                line1 = m_line1B;
                line2 = m_line2B;
            }
        }
    }
    m_thetaPoints1.pop_back();
    m_thetaPoints2.pop_back();
}

void PolarCalibration::doTransformation(const cv::Mat& img1, const cv::Mat& img2, const cv::Point2d epipole1, const cv::Point2d epipole2, const cv::Mat & F) {

    const double rhoRange1 = m_maxRho1 - m_minRho1 + 1;
    const double rhoRange2 = m_maxRho2 - m_minRho2 + 1;

    const double rhoRange = max(rhoRange1, rhoRange2);

    m_mapX1 = cv::Mat::ones(m_thetaPoints1.size(), rhoRange, CV_32FC1) * -1;
    m_mapY1 = cv::Mat::ones(m_thetaPoints1.size(), rhoRange, CV_32FC1) * -1;
    m_mapX2 = cv::Mat::ones(m_thetaPoints2.size(), rhoRange, CV_32FC1) * -1;
    m_mapY2 = cv::Mat::ones(m_thetaPoints2.size(), rhoRange, CV_32FC1) * -1;

    m_inverseMapX1 = cv::Mat::ones(img1.rows, img1.cols, CV_32FC1) * -1;
    m_inverseMapY1 = cv::Mat::ones(img1.rows, img1.cols, CV_32FC1) * -1;
    m_inverseMapX2 = cv::Mat::ones(img1.rows, img1.cols, CV_32FC1) * -1;
    m_inverseMapY2 = cv::Mat::ones(img1.rows, img1.cols, CV_32FC1) * -1;

    for (uint32_t thetaIdx = 0; thetaIdx < m_thetaPoints1.size(); thetaIdx++) {
        transformLine(epipole1, m_thetaPoints1[thetaIdx], img1, thetaIdx, m_minRho1, m_maxRho1, m_mapX1, m_mapY1, m_inverseMapX1, m_inverseMapY1);
        transformLine(epipole2, m_thetaPoints2[thetaIdx], img2, thetaIdx, m_minRho2, m_maxRho2, m_mapX2, m_mapY2, m_inverseMapX2, m_inverseMapY2);
    }
}

void PolarCalibration::getRectifiedImages(const cv::Mat& img1, const cv::Mat& img2,
                                          cv::Mat& rectified1, cv::Mat& rectified2, int interpolation) {

    cv::remap(img1, rectified1, m_mapX1, m_mapY1, interpolation, cv::BORDER_TRANSPARENT);
    cv::remap(img2, rectified2, m_mapX2, m_mapY2, interpolation, cv::BORDER_TRANSPARENT);
}

void PolarCalibration::rectifyAndStoreImages(const cv::Mat& img1, const cv::Mat& img2, int interpolation)
{
    getRectifiedImages(img1, img2, m_rectified1, m_rectified2, interpolation);
}

bool PolarCalibration::getStoredRectifiedImages(cv::Mat& img1, cv::Mat& img2)
{
    img1 = m_rectified1;
    img2 = m_rectified2;
}

void PolarCalibration::getMaps(cv::Mat& mapX, cv::Mat& mapY, const uint8_t& whichImage)
{
    if (whichImage == 1) {
        mapX = m_mapX1;
        mapY = m_mapY1;
    } else if (whichImage == 2) {
        mapX = m_mapX2;
        mapY = m_mapY2;
    }
}

void PolarCalibration::getInverseMaps(cv::Mat& mapX, cv::Mat& mapY, const uint8_t& whichImage)
{
    if (whichImage == 1) {
        mapX = m_inverseMapX1;
        mapY = m_inverseMapY1;
    } else if (whichImage == 2) {
        mapX = m_inverseMapX2;
        mapY = m_inverseMapY2;
    }
}

void PolarCalibration::transformPoints(const vector< cv::Point2d >& points,
                                       vector< cv::Point2d >& transformedPoints, const uint8_t & whichImage)
{
    cv::Mat * pMapX, * pMapY;
    if (whichImage == 1) {
        pMapX = &m_inverseMapX1;
        pMapY = &m_inverseMapY1;
    } else if (whichImage == 2) {
        pMapX = &m_inverseMapX2;
        pMapY = &m_inverseMapY2;
    }

    transformedPoints.clear();
    transformedPoints.reserve(points.size());
    for (uint32_t i = 0; i < points.size(); i++) {
        const cv::Point2d & p = points[i];
        transformedPoints.push_back(
                    cv::Point2d(pMapX->at<float>(p.y, p.x),
                                pMapY->at<float>(p.y, p.x)));

//         cout << points[i] << " --> " << transformedPoints[i] << endl;
    }
//     exit(0);
}

/*inline
bool PolarCalibration::isVectorBetweenVectors(const cv::Vec3f& v, const cv::Vec3f& v1, const cv::Vec3f& v2)
{
    if (SIGN(v1.cross(v)[2]) == SIGN(v.cross(v2)[2])) {
        if (fabs(acos(v1.dot(v2))) > fabs(acos(v1.dot(v)))) {
            return true;
        }
    }
    return false;
}

bool PolarCalibration::findThetaIdx(const cv::Point2d& epipole, const cv::Point2d &queryPoint,
                                    const vector<cv::Point2d> & thetaPoints,
                                    const uint32_t& minIdx, const uint32_t& maxIdx, double & thetaIdx)
{
//     cout << "queryPoint " << queryPoint << endl;
//     cout << "minPoint " << thetaPoints[minIdx] << endl;
//     cout << "maxPoint " << thetaPoints[maxIdx] << endl;
//     cout << "m_b1 " << m_b1 << endl;
//     cout << "m_e1 " << m_e1 << endl;

    cv::Vec3f v(queryPoint.x - epipole.x, queryPoint.y - epipole.y, 0.0);
    cv::Vec3f v1(thetaPoints[minIdx].x - epipole.x, thetaPoints[minIdx].y - epipole.y, 0.0);
    cv::Vec3f v2(thetaPoints[maxIdx].x - epipole.x, thetaPoints[maxIdx].y - epipole.y, 0.0);

    v /= cv::norm(v);
    v1 /= cv::norm(v1);
    v2 /= cv::norm(v2);

    if (! isVectorBetweenVectors(v, v1, v2))
        return false;

    if (maxIdx - minIdx == 1) {
        const double & dist1 = cv::norm(queryPoint - thetaPoints[minIdx]);
        const double & dist2 = cv::norm(queryPoint - thetaPoints[maxIdx]);

//         cout << dist1 << endl;
//         cout << dist2 << endl;
//         cout << dist1 << endl;

        thetaIdx = minIdx + (dist1 / (dist1 + dist2));
//         if (dist1 < dist2)
//             thetaIdx = minIdx;
//         else
//             thetaIdx = maxIdx;

        return true;
    }

    const uint32_t minIdx1 = minIdx;
    const uint32_t maxIdx1 = minIdx + ((maxIdx - minIdx + 1) / 2);
    const uint32_t minIdx2 = maxIdx1;
    const uint32_t maxIdx2 = maxIdx;

    if (findThetaIdx(epipole, queryPoint, thetaPoints, minIdx1, maxIdx1, thetaIdx))
        return true;
    if (findThetaIdx(epipole, queryPoint, thetaPoints, minIdx2, maxIdx2, thetaIdx))
        return true;
}

bool PolarCalibration::transformPoints(const cv::Mat& Fin, const vector< cv::Point2f >& ctrlPoints1,
                                       const vector< cv::Point2f >& ctrlPoints2, const cv::Size & imgDimensions,
                                       const vector< cv::Point2d >& points1, const vector< cv::Point2d >& points2,
                                       vector< cv::Point2d >& transformedPoints1, vector< cv::Point2d >& transformedPoints2)
{
    cv::Point2d epipole1, epipole2;
    const cv::Mat img1(2, 2, CV_8UC3), img2(2, 2, CV_8UC3);
    cv::Mat F = Fin;
    if (! findFundamentalMat(img1, img2, F, ctrlPoints1, ctrlPoints2, epipole1, epipole2, FMAT_METHOD_OFLOW))
        return false;

//     Determine common region
    vector<cv::Vec3f> initialEpilines, finalEpilines;
    vector<cv::Point2f> epipoles(2);
    epipoles[0] = epipole1;
    epipoles[1] = epipole2;
    determineCommonRegion(epipoles, imgDimensions, F);

    getTransformationPoints(img1.size(), epipole1, epipole2, F);

    transformedPoints1.reserve(points1.size());
    transformedPoints2.reserve(points2.size());

    for (uint32_t i = 0; i < points1.size(); i++) {
        double thetaIdx;
        if (! findThetaIdx(epipole1, points1[i], m_thetaPoints1, 0, m_thetaPoints1.size() / 2, thetaIdx))
            findThetaIdx(epipole1, points1[i], m_thetaPoints1, m_thetaPoints1.size() / 2, m_thetaPoints1.size() - 1, thetaIdx);
        uint32_t rhoIdx = round(cv::norm(epipole1 - points1[i]) - m_minRho1);

        cout << "p " << points1[i] << endl;
        cout << "e " << epipole1 << endl;
        cout << "d " << cv::norm(epipole1 - points1[i]) << endl;
        cout << "r " << m_minRho1 << endl;

//         exit(0);

//         transformedPoints1.push_back(cv::Point2d(rhoIdx, thetaIdx + 1));
        transformedPoints1.push_back(cv::Point2d(cv::norm(epipole1 - points1[i]) - m_minRho1, thetaIdx + 1));
    }

    return true;

}*/


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      100910    K. Kumar          First creation of code.
 *      111115    K. Kumar          Altered tests to override default mass values; corrected
 *                                  bool-variable name; corrected cerr-statement.
 *      111122    K. Kumar          Altered tests to use reference data.
 *      111206    K. Kumar          Updated synodic period test and comments.
 *      120127    D. Dirkx          Moved to Tudat core.
 *      120127    K. Kumar          Transferred unit tests over to Boost unit test framework.
 *      120128    K. Kumar          Changed BOOST_CHECK to BOOST_CHECK_CLOSE_FRACTION for unit test
 *                                  comparisons.
 *
 *    References
 *      Wikipedia. Geostationary orbit, http://en.wikipedia.org/wiki/Geostationary_orbit, last
 *          accessed: 22nd November, 2011.
 *      Keefe, T.J. Synodic Period Calculator, http://www.ccri.edu/physics/keefe/synodic_calc.htm,
 *          last accessed: 6th December, 2011, last modified: 18th November, 2011.
 *
 *    Notes
 *      The tests need to be updated to check benchmark values from literature.
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>
#include <limits>

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

#include "Tudat/Astrodynamics/BasicAstrodynamics/astrodynamicsFunctions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"

namespace tudat
{
namespace unit_tests
{

//! Test suite for astrodynamics functions.
BOOST_AUTO_TEST_SUITE( test_astrodynamics_functions )

//! Test if the orbital period of a Kepler orbit is computed correctly.
BOOST_AUTO_TEST_CASE( testKeplerOrbitalPeriod )
{
    // Declare and set satellite mass [kg].
    double satelliteMass = 1.0e3;

    // Declare and set gravitational parameter of Earth [m^3 s^-2].
    double earthGravitationalParameter
            = physical_constants::GRAVITATIONAL_CONSTANT * 5.9736e24;

    // Declare and set distance between Earth center and satellite.
    double distanceBetweenSatelliteAndEarth = 4.2164e7;

    // Compute orbital period of satellite.
    double orbitalPeriod = basic_astrodynamics::computeKeplerOrbitalPeriod(
                distanceBetweenSatelliteAndEarth, earthGravitationalParameter, satelliteMass );

    // Declare and set expected orbital period [s].
    double expectedOrbitalPeriod = 86164.09054;

    // Check if computed orbital period matches expected orbital period.
    BOOST_CHECK_CLOSE_FRACTION( orbitalPeriod, expectedOrbitalPeriod, 1.0e-5 );
}

//! Test if the orbital angular momentum of a kepler orbit is computed correctly.
BOOST_AUTO_TEST_CASE( testKeplerAngularMomentum )
{
    // Reference: http://en.wikipedia.org/wiki/Geostationary_orbit.
    // Declare and set satellite mass [kg].
    double satelliteMass = 1.0e3;

    // Declare and set gravitational parameter of Earth [m^3 s^-2].
    double earthGravitationalParameter
            = physical_constants::GRAVITATIONAL_CONSTANT * 5.9736e24;

    // Declare and set distance between Earth center and satellite.
    double distanceBetweenSatelliteAndEarth = 4.2164e7;

    // Declare and set eccentricity of satellite orbit.
    double eccentricityOfSatelliteOrbit = 0.0;

    // Compute Kepler angular momentum.
    double angularMomentum = basic_astrodynamics::computeKeplerAngularMomentum(
                distanceBetweenSatelliteAndEarth, eccentricityOfSatelliteOrbit,
                earthGravitationalParameter, satelliteMass );

    // Declare and set expected angular momentum.
    // The expected angular momentum is computed using the fact that for a circular orbit,
    // H = mRV. This is an independent check of the code, which computes angular
    // momentum differently.
    double expectedAngularMomentum = satelliteMass * distanceBetweenSatelliteAndEarth
            * std::sqrt( earthGravitationalParameter / distanceBetweenSatelliteAndEarth );

    // Check if computed angular momentum matches expected angular momentum.
    BOOST_CHECK_CLOSE_FRACTION( angularMomentum, expectedAngularMomentum,
                                std::numeric_limits< double >::epsilon( ) );
}

//! Test if the mean motion of a kepler orbit is computed correctly.
BOOST_AUTO_TEST_CASE( testMeanMotion )
{
    // Declare and set satellite mass [kg].
    double satelliteMass = 1.0e3;

    // Declare and set gravitational parameter of Earth [m^3 s^-2].
    double earthGravitationalParameter
            = physical_constants::GRAVITATIONAL_CONSTANT * 5.9736e24;

    // Declare and set distance between Earth center and satellite.
    double distanceBetweenSatelliteAndEarth = 4.2164e7;

    // Reference: http://en.wikipedia.org/wiki/Geostationary_orbit.
    double meanMotion = basic_astrodynamics::computeKeplerMeanMotion(
                distanceBetweenSatelliteAndEarth, earthGravitationalParameter, satelliteMass );

    // Declare and set expected mean motion [rad/s].
    double expectedMeanMotion = 7.2921e-5;

    // Check if computed mean motion matches expected mean motion.
    BOOST_CHECK_CLOSE_FRACTION( meanMotion, expectedMeanMotion, 1.0e-7 );
}

//! Test if the orbital energy of a Kepler orbit is computed correctly.
BOOST_AUTO_TEST_CASE( testKeplerEnergy )
{
    // Declare and set satellite mass [kg].
    double satelliteMass = 1.0e3;

    // Declare and set gravitational parameter of Earth [m^3 s^-2].
    double earthGravitationalParameter
            = physical_constants::GRAVITATIONAL_CONSTANT * 5.9736e24;

    // Declare and set distance between Earth center and satellite.
    double distanceBetweenSatelliteAndEarth = 4.2164e7;

    // Compute Kepler energy.
    double orbitalEnergy = basic_astrodynamics::computeKeplerEnergy(
                distanceBetweenSatelliteAndEarth, earthGravitationalParameter, satelliteMass );

    // Declare and set expected orbital energy.
    // The expected orbital energy is computed using the fact that for a circular orbit,
    // E = m ( V^2/2 - mu/R ). This is an independent check of the code, which computes orbital
    // energy differently.
    double expectedOrbitalEnergy = satelliteMass * (
                0.5 * earthGravitationalParameter / distanceBetweenSatelliteAndEarth
                -  earthGravitationalParameter / distanceBetweenSatelliteAndEarth );

    // Check if computed orbital energy matches expected orbital energy.
    BOOST_CHECK_CLOSE_FRACTION( orbitalEnergy, expectedOrbitalEnergy,
                                std::numeric_limits< double >::epsilon( ) );

}

//! Test if the synodic period between two orbits is computed correctly.
BOOST_AUTO_TEST_CASE( testSynodicPeriod )
{
    // Compute synodic period between Earth and Mars.
    double synodicPeriod = basic_astrodynamics::computeSynodicPeriod( 365.256378, 686.95 );

    // Declare and set expected synodic period.
    double expectedSynodicPeriod = 779.9746457736733;

    // Check if computed synodic period matches expected synodic period.
    BOOST_CHECK_CLOSE_FRACTION( synodicPeriod, expectedSynodicPeriod,
                                std::numeric_limits< double >::epsilon( ) );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/**
 * @brief defines branch class
 *
 * @file branch.hpp
 * @author Luiz Victor Linhares Rocha
 * @date 2018-09-22
 * @copyright 2018
 */

#ifndef REDE_BRANCH_HPP_
#define REDE_BRANCH_HPP_

#include <complex>
#include <boost/shared_ptr.hpp>
#include "neuralFluxDeclare.hpp"
#include "../util/complexutilsDeclare.h"
#include "../util/connection.hpp"
#include "../util/connector.hpp"
#include "../util/identifiable.hpp"
#include "../util/changeTrackable.hpp"

namespace neuralFlux {

class Branch : public Connection<Bar>, public ChangeTrackable {
 public:
    virtual ~Branch();
    static BranchPtr connectBranch(
        boost::shared_ptr<Bar> barK,
        boost::shared_ptr<Bar> barM,
        const double rkm,
        const double xkm,
        const double bshkm = 0,
        const double a = 1,
        const double phi = 0);

    const BarPtr getK() const;
    const BarPtr getM() const;
    double getTetaKm() const;

    // Getters
    const complexo getZkm() const;
    double getRkm() const;
    double getXkm() const;

    complexo getYkm() const;
    double getGkm() const;
    double getBkm() const;
    double getBshkm() const;

    const complexo getTrafo() const;
    double getA() const;
    double getPhi() const;

    // setters, set changed
    void setZkm(const complexo zkm);
    void setRkm(const double rkm);
    void setXkm(const double xkm);

    void setYkm(const complexo zkm);
    void setGkm(const double rkm);
    void setBkm(const double xkm);
    void setBshkm(const double bshkm);

    void setTrafo(const complexo trafo);
    void setA(const double a);
    void setPhi(const double phi);

    // calculators
    complexo getIkm();
    complexo getImk();

    uint64_t getId() const;
    static uint64_t calculateId(uint64_t kId, uint64_t mId);
    // operator, branch order defined by the bar's ids, being k the mos significant
    bool operator<(const Branch& branch) const;
    bool operator>(const Branch& branch) const;
    bool operator==(const Branch& branch) const;

    void setBranchOn(bool branchOn = true);
    bool isBranchOn();

 private:
    complexo m_zkm;   // impedancia da linha
    complexo m_tap;  // dados de possivel trafo
    double m_bshkm;
    bool m_branchOn;
    const double tolerance = 0.000001;

    Branch(
        const double rkm,
        const double xkm,
        const double bshkm = 0,
        const double a = 1,
        const double phi = 0);
};
}  // namespace neuralFlux

#endif  // REDE_BRANCH_HPP_


#include <boost/mpl/max_element.hpp>


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_INTERLEAVE_SECOND_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_SSE_SSE2_INTERLEAVE_SECOND_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT

#include <boost/simd/swar/functions/interleave_second.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<single_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<single_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(__m128 const a0, __m128 const a1) const
    {
      return _mm_unpackhi_ps(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<double_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<double_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(__m128d const a0, __m128d const a1) const
    {
      return _mm_unpackhi_pd(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type8_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type8_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return _mm_unpackhi_epi8(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type16_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type16_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return _mm_unpackhi_epi16(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type32_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type32_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(__m128i const a0, __m128i const a1) const
    {
      return _mm_unpackhi_epi32(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT         ( interleave_second_
                                   , boost::simd::tag::sse2_
                                   , (A0)(A1)
                                   , ((simd_<type64_<A0>,boost::simd::tag::sse_>))
                                     ((simd_<type64_<A1>,boost::simd::tag::sse_>))
                                   )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const& a1) const
    {
      return _mm_unpackhi_epi64(a0,a1);
    }
  };
} } }

#endif
#endif


#ifndef GRAPHLAB_EXTENSION_DATA_HPP
#define GRAPHLAB_EXTENSION_DATA_HPP

#include <graphlab/graph/distributed_graph.hpp>
#include <boost/variant.hpp>
#include <Eigen/Dense>
#include <string>
#include <map>
#include "../collaborative_filtering/eigen_serialization.hpp"
#include "MurmurHash3.h"

// Here I define the basic "var" variant type.
// which is basically a boost::variant around a double, string, vector and matrix

namespace graphlab {
namespace extension {

// the key here is that we are going to lock down the
// type system for GraphLab to a wrapped boost::variant
typedef boost::variant<double,
                       std::string,
                       Eigen::VectorXd,
                       Eigen::MatrixXd> var;

extern var& operator+=(var& value, const var& other);

/**
 * Gets a typechecked value from the variant var
 */
template <typename T>
inline const T& get(const var& v) {
  const T* val = boost::get<const T>(&v);
  if (val == NULL) {
    logger_once(LOG_ERROR, "Reading invalid type from var");
    static T t;
    return t;
  }
  return *val;
}

/**
 * Gets a typechecked value from the variant var
 */
template <typename T>
inline T& get(var& v) {
  T* val = boost::get<T>(&v);
  if (val == NULL) {
    logger_once(LOG_ERROR, "Reading invalid type from var");
    static T t;
    return t;
  }
  return *val;
}

//////////////////////////////////////////////////////////////
// Here we briefly escape out tp the global namespace       //
// to define serializers and deserializers for the var      //
//////////////////////////////////////////////////////////////
}}

BEGIN_OUT_OF_PLACE_SAVE(oarc, graphlab::extension::var, value) {
  if ( const double* val = boost::get<double>( &value) ) {
    oarc << char(1) << (*val);
  } else if ( const std::string* val = boost::get<std::string>( &value) ) {
    oarc << char(2) << (*val);
  } else if ( const Eigen::VectorXd* val = boost::get<Eigen::VectorXd>( &value) ) {
    oarc << char(3) << (*val);
  } else if ( const Eigen::MatrixXd* val = boost::get<Eigen::MatrixXd>( &value) ) {
    oarc << char(4) << (*val);
  }
} END_OUT_OF_PLACE_SAVE()

BEGIN_OUT_OF_PLACE_LOAD(iarc, graphlab::extension::var, value) {
  char content_type;
  iarc >> content_type;
  if (content_type == 1) {
    double val; iarc >> val; value = val;
  } else if (content_type == 2) {
    std::string val; iarc >> val; value = val;
  } else if (content_type == 3) {
    Eigen::VectorXd val; iarc >> val; value = val;
  } else if (content_type == 4) {
    Eigen::MatrixXd val; iarc >> val; value = val;
  }
} END_OUT_OF_PLACE_LOAD()
namespace graphlab { namespace extension {

//////////////////////////////////////////////////////////////
// Returning to your regular progamming                     //
//////////////////////////////////////////////////////////////

typedef uint32_t key_id_type;

/** vars is a dynamic struct with a mapping from string->var.
  * Internally, it is stored as key_id_type->var where the key is
  * a hash value of the string.
  * we assume that the murmurhash will never collide for the small
  * namespaces considered.
 */
inline key_id_type get_id_from_name(const char* key) {
  uint32_t ret = 0;
  MurmurHash3_x86_32((void*)key, strlen(key), 12345, (void*)(&ret));
  return ret;
}
// overload for string
inline key_id_type get_id_from_name(const std::string& key) {
  uint32_t ret = 0;
  MurmurHash3_x86_32((void*)key.c_str(), key.length(), 12345, (void*)(&ret));
  return ret;
}
// overload for int
inline key_id_type get_id_from_name(key_id_type key) {
  return key;
}

/**
 * A dynamic struct storing mappings from string->var where var
 * is a variant.
 * fields can be accessed with operator() or ".field()"
 */
struct vars {
  std::vector<std::pair<key_id_type, var*> > table;
  static var empty_var;
  simple_spinlock lock;
  vars() { }
  ~vars() {
    }

  void clear() {
    lock.lock();
    for(const std::pair<key_id_type, var*>& p: table) {
      delete p.second;
    }
    table.clear();
    lock.unlock();
  }

  vars& operator=(const vars& v) {
    clear();
    for (size_t i = 0;i < v.table.size(); ++i) {
      field(v.table[i].first) = *(v.table[i].second);
    }
    return *this;
  }

  void save(oarchive& oarc) const {
    lock.lock();
    oarc << (size_t)table.size();
    for(const std::pair<key_id_type, var*>& p: table) {
      oarc << p.first << (*p.second);
    }
    lock.unlock();
  }

  void load(iarchive& iarc) {
    size_t tsize;
    iarc >> tsize;
    for (size_t i = 0;i < tsize; ++i) {
      key_id_type key; iarc >> key;
      iarc >> field(key);
    }
  }

  var& operator()(const std::string& key) {
    return field(key);
  }
  const var& operator()(const std::string& key) const {
    return field(key);
  }
  var& operator()(const char* key) {
    return field(key);
  }
  const var& operator()(const char* key) const {
    return field(key);
  }
  var& operator()(key_id_type key) {
    return field(key);
  }
  const var& operator()(key_id_type key) const {
    return field(key);
  }

  var& field(const std::string& _key) {
    key_id_type key = get_id_from_name(_key);
    return field(key);
  }

  const var& field(const std::string& _key) const {
    key_id_type key = get_id_from_name(_key);
    return field(key);
  }

  var& field(const char* _key) {
    key_id_type key = get_id_from_name(_key);
    return field(key);
  }
  const var& field(const char* _key) const {
    key_id_type key = get_id_from_name(_key);
    return field(key);
  }

  var& field(key_id_type key) {
    lock.lock();

    for(std::pair<key_id_type , var*>& p: table) {
      if (p.first == key) {
        lock.unlock();
        return *(p.second);
      }
    }
    var* ret = new var;
    // force slow resize to limit memory usage
    // assume that field creation is not a common operation.
    table.reserve(table.size() + 1);
    table.push_back(std::make_pair(key, ret));
    lock.unlock();
    return *ret;
  }

  const var& field(key_id_type key) const {
    for(const std::pair<key_id_type , var*>& p: table) {
      if (p.first == key) {
        return *(p.second);
      }
    }
    return empty_var;
  }

};

typedef distributed_graph<vars, vars> internal_graph_type;

} // namespace extension
} // namespace graphlab

#endif


#ifndef PYTHONIC_INCLUDE_NUMPY_MAXIMUM_HPP
#define PYTHONIC_INCLUDE_NUMPY_MAXIMUM_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/types/numpy_broadcast.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/max.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME maximum
#define NUMPY_NARY_FUNC_SYM boost::simd::max
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#include "protocol.hpp"
#include <algorithm>
#include <boost/crc.hpp>
#include <spdlog/spdlog.h>

#define SUBSEQUENT_CORRUPTION_PROP 0.4

namespace protocol {
Protocol::Protocol(std::random_device &rd, std::string name)
    : m_rng(rd()), m_subsequentCorruption(SUBSEQUENT_CORRUPTION_PROP),
      m_name(name) {
  setMessageSize(12);
}
Protocol::~Protocol() {}
void Protocol::setMessageSize(std::size_t size) {
  m_currentMessage.resize(size);
  m_messageSize = size;
}
void Protocol::setErrorPropability(float p) {
  m_corruptBit = std::bernoulli_distribution(p);
}
void Protocol::activateSubsequentCorruptions(bool activate) {
  m_subsequentCorruptionsActivated = activate;
}
std::string protocol::Protocol::name() {
  return m_name + (m_subsequentCorruptionsActivated
                       ? "_with_subsequent_corruptions"
                       : "");
}
void Protocol::nextMessage(Message &msg) {
  m_currentMessage.resize(m_messageSize);
  msg.resize(m_messageSize);
  std::generate(m_currentMessage.begin(), m_currentMessage.end(), m_rng);

  // Begin new message transmission.
  msg = m_currentMessage;
  m_currentTripTime = 0;
}
void Protocol::simulateHardware(protocol::Protocol::Message &message) {
  m_currentTripTime += message.size();

  for (uint8_t &b : message) {
    for (uint8_t bit = 0; bit < 8; ++bit) {
      if (m_corruptBit(m_rng)) {
        b ^= 1u << bit;
        m_lastBitCorrupted = true;
      } else if (m_lastBitCorrupted && m_subsequentCorruptionsActivated &&
                 m_subsequentCorruption(m_rng)) {
        b ^= 1u << bit;
        m_lastBitCorrupted = true;
      } else {
        m_lastBitCorrupted = false;
      }
    }
  }
}
int16_t Protocol::getCrc16OfMessage(const Message &msg) {
  boost::crc_16_type result;
  result.process_bytes(msg.data(), msg.size());
  return result.checksum();
}
bool protocol::Protocol::checkMessage(protocol::Protocol::ProtocolStatus status,
                                      protocol::Protocol::Message &msg) {
  switch (status) {
  case ProtocolStatus::Ok:
    return std::equal(m_currentMessage.begin(), m_currentMessage.end(),
                      msg.begin(), msg.end());
  case ProtocolStatus::ErrorDetected:
    return !std::equal(m_currentMessage.begin(), m_currentMessage.end(),
                       msg.begin(), msg.end());
  }
  return false;
}

int Protocol::startTransfer(Message &tmpMsg1, Message &tmpMsg2) {
  // The ideal protocol would have a transfer time of 0.
  return 0;
}

} // namespace protocol