/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 *  @file  InitializePose3.h
 *  @author Luca Carlone
 *  @author Frank Dellaert
 *  @date   August, 2014
 */

#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>

#include <boost/math/special_functions.hpp>

using namespace std;

namespace gtsam {

static const Key kAnchorKey = symbol('Z', 9999999);

/* ************************************************************************* */
GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) {

  GaussianFactorGraph linearGraph;

  for(const auto& factor: g) {
    Matrix3 Rij;
    double rotationPrecision = 1.0;

    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between){
      Rij = pose3Between->measured().rotation().matrix();
      Vector precisions = Vector::Zero(6);
      precisions[0] = 1.0; // vector of all zeros except first entry equal to 1
      pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable
      rotationPrecision = precisions[0]; // rotations first
    }else{
      cout << "Error in buildLinearOrientationGraph" << endl;
    }

    const auto& keys = factor->keys();
    Key key1 = keys[0], key2 = keys[1];
    Matrix M9 = Z_9x9;
    M9.block(0,0,3,3) = Rij;
    M9.block(3,3,3,3) = Rij;
    M9.block(6,6,3,3) = Rij;
    linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision));
  }
  // prior on the anchor orientation
  linearGraph.add(
      kAnchorKey, I_9x9,
      (Vector(9) << 1.0, 0.0, 0.0, /*  */ 0.0, 1.0, 0.0, /*  */ 0.0, 0.0, 1.0)
          .finished(),
          noiseModel::Isotropic::Precision(9, 1));
  return linearGraph;
}

/* ************************************************************************* */
// Transform VectorValues into valid Rot3
Values InitializePose3::normalizeRelaxedRotations(
    const VectorValues& relaxedRot3) {
  gttic(InitializePose3_computeOrientationsChordal);

  Matrix ppm = Z_3x3; // plus plus minus
  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;

  Values validRot3;
  for(const auto& it: relaxedRot3) {
    Key key = it.first;
    if (key != kAnchorKey) {
      Matrix3 R;
      R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized

      // ClosestTo finds rotation matrix closest to H in Frobenius sense
      // Rot3 initRot = Rot3::ClosestTo(M.transpose());

      Matrix U, V; Vector s;
      svd(R.transpose(), U, s, V);
      Matrix3 normalizedRotMat = U * V.transpose();

      if(normalizedRotMat.determinant() < 0)
        normalizedRotMat = U * ppm * V.transpose();

      Rot3 initRot = Rot3(normalizedRotMat);
      validRot3.insert(key, initRot);
    }
  }
  return validRot3;
}

/* ************************************************************************* */
NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) {
  gttic(InitializePose3_buildPose3graph);
  NonlinearFactorGraph pose3Graph;

  for(const auto& factor: graph) {

    // recast to a between on Pose3
    const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between)
      pose3Graph.add(pose3Between);

    // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
    const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
    if (pose3Prior)
      pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
              pose3Prior->prior(), pose3Prior->noiseModel());
  }
  return pose3Graph;
}

/* ************************************************************************* */
Values InitializePose3::computeOrientationsChordal(
    const NonlinearFactorGraph& pose3Graph) {
  gttic(InitializePose3_computeOrientationsChordal);

  // regularize measurements and plug everything in a factor graph
  GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);

  // Solve the LFG
  VectorValues relaxedRot3 = relaxedGraph.optimize();

  // normalize and compute Rot3
  return normalizeRelaxedRotations(relaxedRot3);
}

/* ************************************************************************* */
Values InitializePose3::computeOrientationsGradient(
    const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
    const size_t maxIter, const bool setRefFrame) {
  gttic(InitializePose3_computeOrientationsGradient);

  // this works on the inverse rotations, according to Tron&Vidal,2011
  Values inverseRot;
  inverseRot.insert(kAnchorKey, Rot3());
  for(const auto& key_value: givenGuess) {
    Key key = key_value.key;
    const Pose3& pose = givenGuess.at<Pose3>(key);
    inverseRot.insert(key, pose.rotation().inverse());
  }

  // Create the map of edges incident on each node
  KeyVectorMap adjEdgesMap;
  KeyRotMap factorId2RotMap;

  createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);

  // calculate max node degree & allocate gradient
  size_t maxNodeDeg = 0;
  VectorValues grad;
  for(const auto& key_value: inverseRot) {
    Key key = key_value.key;
    grad.insert(key,Z_3x1);
    size_t currNodeDeg = (adjEdgesMap.at(key)).size();
    if(currNodeDeg > maxNodeDeg)
      maxNodeDeg = currNodeDeg;
  }

  // Create parameters
  double b = 1;
  double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
  double a = (M_PI*M_PI)/(2*f0);
  double rho = 2*a*b;
  double mu_max = maxNodeDeg * rho;
  double stepsize = 2/mu_max; // = 1/(a b dG)

  double maxGrad;
  // gradient iterations
  size_t it;
  for (it = 0; it < maxIter; it++) {
    //////////////////////////////////////////////////////////////////////////
    // compute the gradient at each node
    maxGrad = 0;
    for (const auto& key_value : inverseRot) {
      Key key = key_value.key;
      Vector gradKey = Z_3x1;
      // collect the gradient for each edge incident on key
      for (const size_t& factorId : adjEdgesMap.at(key)) {
        Rot3 Rij = factorId2RotMap.at(factorId);
        Rot3 Ri = inverseRot.at<Rot3>(key);
        auto factor = pose3Graph.at(factorId);
        const auto& keys = factor->keys();
        if (key == keys[0]) {
          Key key1 = keys[1];
          Rot3 Rj = inverseRot.at<Rot3>(key1);
          gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
        } else if (key == keys[1]) {
          Key key0 = keys[0];
          Rot3 Rj = inverseRot.at<Rot3>(key0);
          gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
        } else {
          cout << "Error in gradient computation" << endl;
        }
      }  // end of i-th gradient computation
      grad.at(key) = stepsize * gradKey;

      double normGradKey = (gradKey).norm();
      if(normGradKey>maxGrad)
        maxGrad = normGradKey;
    } // end of loop over nodes

    //////////////////////////////////////////////////////////////////////////
    // update estimates
    inverseRot = inverseRot.retract(grad);

    //////////////////////////////////////////////////////////////////////////
    // check stopping condition
    if (it>20 && maxGrad < 5e-3)
      break;
  } // enf of gradient iterations

  // Return correct rotations
  const Rot3& Rref = inverseRot.at<Rot3>(kAnchorKey); // This will be set to the identity as so far we included no prior
  Values estimateRot;
  for(const auto& key_value: inverseRot) {
    Key key = key_value.key;
    if (key != kAnchorKey) {
      const Rot3& R = inverseRot.at<Rot3>(key);
      if(setRefFrame)
        estimateRot.insert(key, Rref.compose(R.inverse()));
      else
        estimateRot.insert(key, R.inverse());
    }
  }
  return estimateRot;
}

/* ************************************************************************* */
void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
                         const NonlinearFactorGraph& pose3Graph) {
  size_t factorId = 0;
  for(const auto& factor: pose3Graph) {
    auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
    if (pose3Between){
      Rot3 Rij = pose3Between->measured().rotation();
      factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));

      Key key1 = pose3Between->key1();
      if (adjEdgesMap.find(key1) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key1).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
      }
      Key key2 = pose3Between->key2();
      if (adjEdgesMap.find(key2) != adjEdgesMap.end()){  // key is already in
        adjEdgesMap.at(key2).push_back(factorId);
      }else{
        vector<size_t> edge_id;
        edge_id.push_back(factorId);
        adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
      }
    }else{
      cout << "Error in createSymbolicGraph" << endl;
    }
    factorId++;
  }
}

/* ************************************************************************* */
Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
  Vector3 logRot = Rot3::Logmap(R1.between(R2));

  double th = logRot.norm();
  if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
    Rot3 R1pert = R1.compose( Rot3::Expmap(Vector3(0.01, 0.01, 0.01)) ); // some perturbation
    logRot = Rot3::Logmap(R1pert.between(R2));
    th = logRot.norm();
  }
  // exclude small or invalid rotations
  if (th > 1e-5 && th == th) {  // nonzero valid rotations
    logRot = logRot / th;
  } else {
    logRot = Z_3x1;
    th = 0.0;
  }

  double fdot = a*b*th*exp(-b*th);
  return fdot*logRot;
}

/* ************************************************************************* */
Values InitializePose3::initializeOrientations(const NonlinearFactorGraph& graph) {
  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  return computeOrientationsChordal(pose3Graph);
}

///* ************************************************************************* */
Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph,  Values& initialRot) {
  gttic(InitializePose3_computePoses);

  // put into Values structure
  Values initialPose;
  for (const auto& key_value : initialRot) {
    Key key = key_value.key;
    const Rot3& rot = initialRot.at<Rot3>(key);
    Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
    initialPose.insert(key, initializedPose);
  }

  // add prior
  noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
  initialPose.insert(kAnchorKey, Pose3());
  pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);

  // Create optimizer
  GaussNewtonParams params;
  bool singleIter = true;
  if (singleIter) {
    params.maxIterations = 1;
  } else {
    cout << " \n\n\n\n  performing more than 1 GN iterations \n\n\n" << endl;
    params.setVerbosity("TERMINATION");
  }
  GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
  Values GNresult = optimizer.optimize();

  // put into Values structure
  Values estimate;
  for (const auto& key_value : GNresult) {
    Key key = key_value.key;
    if (key != kAnchorKey) {
      const Pose3& pose = GNresult.at<Pose3>(key);
      estimate.insert(key, pose);
    }
  }
  return estimate;
}

/* ************************************************************************* */
Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
                  bool useGradient) {
  gttic(InitializePose3_initialize);
  Values initialValues;

  // We "extract" the Pose3 subgraph of the original graph: this
  // is done to properly model priors and avoiding operating on a larger graph
  NonlinearFactorGraph pose3Graph = buildPose3graph(graph);

  // Get orientations from relative orientation measurements
  Values orientations;
  if (useGradient)
    orientations = computeOrientationsGradient(pose3Graph, givenGuess);
  else
    orientations = computeOrientationsChordal(pose3Graph);

  // Compute the full poses (1 GN iteration on full poses)
  return computePoses(pose3Graph, orientations);
}

/* ************************************************************************* */
Values InitializePose3::initialize(const NonlinearFactorGraph& graph) {
  return initialize(graph, Values(), false);
}

} // namespace gtsam


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <algorithm>
#include <ostream>
#include <sstream>
#include <string>
#include <vector>

#include <boost/config.hpp>
#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/correct.hpp>
#include <boost/geometry/algorithms/within.hpp>
#include <boost/geometry/algorithms/intersection_segment.hpp>
#include <boost/geometry/io/wkt/aswkt.hpp>

static std::ostream & operator<<(std::ostream &s, const bg::intersection_result& r)
{
    switch(r)
    {
        case bg::is_intersect_no : s << "is_intersect_no"; break;
        case bg::is_intersect : s << "is_intersect"; break;
        case bg::is_parallel : s << "is_parallel"; break;
        case bg::is_collinear_no : s << "is_collinear_no"; break;
        case bg::is_collinear_one : s << "is_collinear_one"; break;
        case bg::is_collinear_connect : s << "is_collinear_connect"; break;
        case bg::is_collinear_overlap : s << "is_collinear_overlap"; break;
        case bg::is_collinear_overlap_opposite : s << "is_collinear_overlap_opposite"; break;
        case bg::is_collinear_connect_opposite : s << "is_collinear_connect_opposite"; break;

        // detailed connection results:
        case bg::is_intersect_connect_s1p1 : s << "is_intersect_connect_s1p1"; break;
        case bg::is_intersect_connect_s1p2 : s << "is_intersect_connect_s1p2"; break;
        case bg::is_intersect_connect_s2p1 : s << "is_intersect_connect_s2p1"; break;
        case bg::is_intersect_connect_s2p2 : s << "is_intersect_connect_s2p2"; break;
    }
    return s;
}

static std::string as_string(const bg::intersection_result& r)
{
    std::stringstream out;
    out << r;
    return out.str();
}

typedef bg::model::point<double> P;
typedef bg::const_segment<P> S;

static void test_intersection(double s1x1, double s1y1, double s1x2, double s1y2,
                       double s2x1, double s2y1, double s2x2, double s2y2,
                       // Expected results
                       bg::intersection_result expected_result,
                       int exptected_count, const P& exp_p1, const P& exp_p2)
{
    S s1(P(s1x1, s1y1), P(s1x2, s1y2));
    S s2(P(s2x1, s2y1), P(s2x2, s2y2));
    std::vector<P> ip;
    double ra, rb;
    bg::intersection_result r = bg::intersection_s(s1, s2, ra, rb, ip);
    r = bg::intersection_connect_result(r, ra, rb);

    BOOST_CHECK_EQUAL(ip.size(), exptected_count);
    BOOST_CHECK_EQUAL(as_string(expected_result), as_string(r));

    if (ip.size() == 2 && ip[0] != exp_p1)
    {
        // Swap results, second point is not as expected, swap results, order is not prescribed,
        // it might be OK.
        std::reverse(ip.begin(), ip.end());
    }

    if (ip.size() >= 1)
    {
        BOOST_CHECK_EQUAL(ip[0], exp_p1);
    }
    if (ip.size() >= 2)
    {
        BOOST_CHECK_EQUAL(ip[1], exp_p2);
    }

    /*
    std::cout << exptected_count << " " << r;
    if (exptected_count >= 1) std::cout << " " << ip[0];
    if (exptected_count >= 2) std::cout << " " << ip[1];
    std::cout << std::endl;
    */
}

//BOOST_AUTO_TEST_CASE( test1 )
int test_main( int , char* [] )
{
    // Identical cases
    test_intersection(0,0, 1,1,  0,0, 1,1,          bg::is_collinear_overlap, 2,  P(0,0), P(1,1));
    test_intersection(1,1, 0,0,  0,0, 1,1,          bg::is_collinear_overlap_opposite, 2,  P(1,1), P(0,0));
    test_intersection(0,1, 0,2,  0,1, 0,2,          bg::is_collinear_overlap, 2,  P(0,1), P(0,2)); // Vertical
    test_intersection(0,2, 0,1,  0,1, 0,2,          bg::is_collinear_overlap_opposite, 2,  P(0,2), P(0,1)); // Vertical
    // Overlap cases
    test_intersection(0,0, 1,1,  -0.5,-0.5, 2,2,    bg::is_collinear_overlap, 2,  P(0,0), P(1,1));
    test_intersection(0,0, 1,1,  0.5,0.5, 1.5,1.5,  bg::is_collinear_overlap, 2,  P(0.5,0.5), P(1,1));
    test_intersection(0,0, 0,1,  0,-10, 0,10,       bg::is_collinear_overlap, 2,  P(0,0), P(0,1)); // Vertical
    test_intersection(0,0, 0,1,  0,10, 0,-10,       bg::is_collinear_overlap_opposite, 2,  P(0,0), P(0,1)); // Vertical
    test_intersection(0,0, 1,1,  1,1, 2,2,          bg::is_collinear_connect, 1,  P(1,1), P(0,0)); // Single point
    // Colinear, non overlap cases
    test_intersection(0,0, 1,1,  1.5,1.5, 2.5,2.5,  bg::is_collinear_no, 0,  P(0,0), P(0,0));
    test_intersection(0,0, 0,1,  0,5, 0,6,          bg::is_collinear_no, 0,  P(0,0), P(0,0)); // Vertical
    // Parallel cases
    test_intersection(0,0, 1,1,  1,0, 2,1,       bg::is_parallel, 0,  P(0,0), P(0,1));
    // Intersect cases
    test_intersection(0,2, 4,2,  3,0, 3,4,       bg::is_intersect, 1,  P(3,2), P(0,0));
    // Non intersect cases

    // Single point cases
    test_intersection(0,0, 0,0,  1,1, 2,2,          bg::is_collinear_no, 0,  P(1,1), P(0,0)); // Colinear/no
    test_intersection(2,2, 2,2,  1,1, 3,3,          bg::is_collinear_one, 1,  P(2,2.01), P(0,0)); // On segment
    test_intersection(1,1, 3,3,  2,2, 2,2,          bg::is_collinear_one, 1,  P(2,2), P(0,0)); // On segment
    test_intersection(1,1, 3,3,  1,1, 1,1,          bg::is_collinear_one, 1,  P(1,1), P(0,0)); // On segment, start
    test_intersection(1,1, 3,3,  3,3, 3,3,          bg::is_collinear_one, 1,  P(3,3), P(0,0)); // On segment, end

    return 0;
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-trigonometric
    This function object returns the Computes the remainder modulo \f$\pi/2\f$.

    @par Header <boost/simd/function/rem_pio2_straight.hpp>

    @par Notes

    - @c rem_pio2_straight computes the remainder modulo \f$\pi/2\f$ with "straight" algorithm,
    and returns an angle quadrant which is always 1.
    This is a very quick version only correct if the input
    is in \f$[\pi/4,\pi/2]\f$.

    - In fact it only substracts \f$\pi/2\f$ to the input
    so it can be viewed as a specially accurate minuspio_2 function outside
    the interval in which it can be used as a substitute to @ref rem_pio2.

    - The reduction of the argument modulo \f$\pi/2\f$ is generally
    the most difficult part of trigonometric evaluations.
    The accurate algorithm over the whole floating point range
    is over costly and implies the knowledge
    of a few hundred \f$\pi\f$ decimals
    some simpler algorithms as this one
    can be used, but the precision is only insured on specific intervals.

    @see rem_pio2, rem_pio2_medium,rem_2pi, rem_pio2_cephes,

    @par Example:

      @snippet rem_pio2_straight.cpp rem_pio2_straight

    @par Possible output:

      @snippet rem_pio2_straight.txt rem_pio2_straight

  **/
  std::pair<IEEEValue, IEEEValue> rem_pio2_straight(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/rem_pio2_straight.hpp>
#include <boost/simd/function/simd/rem_pio2_straight.hpp>

#endif


// Copyright 2017 Peter Dimov.
//
// Distributed under the Boost Software License, Version 1.0.
//
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt

#include <boost/mp11/function.hpp>
#include <boost/mp11/integral.hpp>
#include <boost/core/lightweight_test_trait.hpp>
#include <type_traits>

int main()
{
    using boost::mp11::mp_max;
    using boost::mp11::mp_int;
    using boost::mp11::mp_size_t;

    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_max<mp_int<1>>, mp_int<1>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_max<mp_int<2>, mp_int<1>, mp_int<2>, mp_int<3>>, mp_int<3>>));
    BOOST_TEST_TRAIT_TRUE((std::is_same<mp_max<mp_int<-1>, mp_size_t<1>, mp_int<-2>, mp_size_t<2>>, mp_size_t<2>>));

    return boost::report_errors();
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP

#include <algorithm>

#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
#include <boost/geometry/strategies/geographic/azimuth.hpp>
#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/geometry/formulas/vincenty_direct.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

#include <boost/geometry/formulas/result_direct.hpp>
#include <boost/geometry/formulas/mean_radius.hpp>

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <boost/geometry/io/dsv/write.hpp>
#endif

#ifndef BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS
#define BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS 100
#endif

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
#include <iostream>
#endif

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief Strategy functor for distance point to segment calculation on ellipsoid
       Algorithm uses direct and inverse geodesic problems as subroutines.
       The algorithm approximates the distance by an iterative Newton method.
\ingroup strategies
\details Class which calculates the distance of a point to a segment, for points
on the ellipsoid
\see C.F.F.Karney - Geodesics on an ellipsoid of revolution,
      https://arxiv.org/abs/1102.1215
\tparam FormulaPolicy underlying point-point distance strategy
\tparam Spheroid is the spheroidal model used
\tparam CalculationType \tparam_calculation
\tparam EnableClosestPoint computes the closest point on segment if true
*/
template
<
    typename FormulaPolicy = strategy::andoyer,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void,
    bool EnableClosestPoint = false
>
class geographic_cross_track
{
public :
    template <typename Point, typename PointOfSegment>
    struct return_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point,
                      PointOfSegment,
                      CalculationType
                  >::type
          >
    {};

    struct distance_strategy
    {
        typedef geographic<FormulaPolicy, Spheroid, CalculationType> type;
    };

    inline typename distance_strategy::type get_distance_strategy() const
    {
        typedef typename distance_strategy::type distance_type;
        return distance_type(m_spheroid);
    }

    explicit geographic_cross_track(Spheroid const& spheroid = Spheroid())
        : m_spheroid(spheroid)
    {}

    template <typename Point, typename PointOfSegment>
    inline typename return_type<Point, PointOfSegment>::type
    apply(Point const& p, PointOfSegment const& sp1, PointOfSegment const& sp2) const
    {
        typedef typename coordinate_system<Point>::type::units units_type;

        return (apply<units_type>(get<0>(sp1), get<1>(sp1),
                                  get<0>(sp2), get<1>(sp2),
                                  get<0>(p), get<1>(p),
                                  m_spheroid)).distance;
    }

private :

    template <typename CT>
    struct result_distance_point_segment
    {
        result_distance_point_segment()
            : distance(0)
            , closest_point_lon(0)
            , closest_point_lat(0)
        {}

        CT distance;
        CT closest_point_lon;
        CT closest_point_lat;
    };

    template <typename CT>
    result_distance_point_segment<CT>
    static inline non_iterative_case(CT lon, CT lat, CT distance)
    {
        result_distance_point_segment<CT> result;
        result.distance = distance;

        if (EnableClosestPoint)
        {
            result.closest_point_lon = lon;
            result.closest_point_lat = lat;
        }
        return result;
    }

    template <typename CT>
    result_distance_point_segment<CT>
    static inline non_iterative_case(CT lon1, CT lat1, //p1
                                     CT lon2, CT lat2, //p2
                                     Spheroid const& spheroid)
    {
        CT distance = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                              ::apply(lon1, lat1, lon2, lat2, spheroid);

        return non_iterative_case(lon1, lat1, distance);
    }

    template <typename CT>
    CT static inline normalize(CT g4, CT& der)
    {
        CT const pi = math::pi<CT>();
        if (g4 < -1.25*pi)//close to -270
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to -270" << std::endl;
#endif
            return g4 + 1.5 * pi;
        }
        else if (g4 > 1.25*pi)//close to 270
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to 270" << std::endl;
#endif
            return - g4 + 1.5 * pi;
        }
        else if (g4 < 0 && g4 > -0.75*pi)//close to -90
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "g4=" << g4 <<  ", close to -90" << std::endl;
#endif
            der = -der;
            return -g4 - pi/2;
        }
        return g4 - pi/2;
    }

    template <typename Units, typename CT>
    result_distance_point_segment<CT>
    static inline apply(CT lon1, CT lat1, //p1
                        CT lon2, CT lat2, //p2
                        CT lon3, CT lat3, //query point p3
                        Spheroid const& spheroid)
    {
        typedef typename FormulaPolicy::template inverse<CT, true, false, false, true, true>
                inverse_distance_quantities_type;
        typedef typename FormulaPolicy::template inverse<CT, false, true, false, false, false>
                inverse_azimuth_type;
        typedef typename FormulaPolicy::template inverse<CT, false, true, true, false, false>
                inverse_azimuth_reverse_type;
        typedef typename FormulaPolicy::template direct<CT, true, false, false, false>
                direct_distance_type;

        CT const earth_radius = geometry::formula::mean_radius<CT>(spheroid);

        result_distance_point_segment<CT> result;

        // Constants
        //CT const f = geometry::formula::flattening<CT>(spheroid);
        CT const pi = math::pi<CT>();
        CT const half_pi = pi / CT(2);
        CT const c0 = CT(0);

        // Convert to radians
        lon1 = math::as_radian<Units>(lon1);
        lat1 = math::as_radian<Units>(lat1);
        lon2 = math::as_radian<Units>(lon2);
        lat2 = math::as_radian<Units>(lat2);
        lon3 = math::as_radian<Units>(lon3);
        lat3 = math::as_radian<Units>(lat3);

        if (lon1 > lon2)
        {
            std::swap(lon1, lon2);
            std::swap(lat1, lat2);
        }

        //segment on equator
        //Note: antipodal points on equator does not define segment on equator
        //but pass by the pole
        CT diff = geometry::math::longitude_distance_signed<geometry::radian>(lon1, lon2);

        typedef typename formula::elliptic_arc_length<CT> elliptic_arc_length;

        bool meridian_not_crossing_pole =
              elliptic_arc_length::meridian_not_crossing_pole(lat1, lat2, diff);

        bool meridian_crossing_pole =
              elliptic_arc_length::meridian_crossing_pole(diff);

        //bool meridian_crossing_pole = math::equals(math::abs(diff), pi);
        //bool meridian_not_crossing_pole = math::equals(math::abs(diff), c0);

        if (math::equals(lat1, c0) && math::equals(lat2, c0) && !meridian_crossing_pole)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Equatorial segment" << std::endl;
            std::cout << "segment=(" << lon1 * math::r2d<CT>();
            std::cout << "," << lat1 * math::r2d<CT>();
            std::cout << "),(" << lon2 * math::r2d<CT>();
            std::cout << "," << lat2 * math::r2d<CT>();
            std::cout << ")\np=(" << lon3 * math::r2d<CT>();
            std::cout << "," << lat3 * math::r2d<CT>() << ")\n";
#endif
            if (lon3 <= lon1)
            {
                return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
            }
            if (lon3 >= lon2)
            {
                return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
            }
            return non_iterative_case(lon3, lat1, lon3, lat3, spheroid);
        }

        if ( (meridian_not_crossing_pole || meridian_crossing_pole ) && lat1 > lat2)
        {
            std::swap(lat1,lat2);
        }

        if (meridian_crossing_pole)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Meridian segment" << std::endl;
#endif
            result_distance_point_segment<CT> d1 = apply<geometry::radian>(lon1, lat1, lon1, half_pi, lon3, lat3, spheroid);
            result_distance_point_segment<CT> d2 = apply<geometry::radian>(lon2, lat2, lon2, half_pi, lon3, lat3, spheroid);
            if (d1.distance < d2.distance)
            {
                return d1;
            }
            else
            {
                return d2;
            }
        }

        CT d1 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon1, lat1, lon3, lat3, spheroid);

        CT d3 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon1, lat1, lon2, lat2, spheroid);

        if (geometry::math::equals(d3, c0))
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "Degenerate segment" << std::endl;
            std::cout << "distance between points=" << d1 << std::endl;
#endif
            return non_iterative_case(lon1, lat2, d1);
        }

        CT d2 = geometry::strategy::distance::geographic<FormulaPolicy, Spheroid, CT>
                ::apply(lon2, lat2, lon3, lat3, spheroid);

        // Compute a12 (GEO)
        geometry::formula::result_inverse<CT> res12 =
                inverse_azimuth_reverse_type::apply(lon1, lat1, lon2, lat2, spheroid);
        CT a12 = res12.azimuth;
        CT a13 = inverse_azimuth_type::apply(lon1, lat1, lon3, lat3, spheroid).azimuth;

        CT a312 = a13 - a12;

        if (geometry::math::equals(a312, c0))
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "point on segment" << std::endl;
#endif
            return non_iterative_case(lon3, lat3, c0);
        }

        CT projection1 = cos( a312 ) * d1 / d3;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "segment=(" << lon1 * math::r2d<CT>();
        std::cout << "," << lat1 * math::r2d<CT>();
        std::cout << "),(" << lon2 * math::r2d<CT>();
        std::cout << "," << lat2 * math::r2d<CT>();
        std::cout << ")\np=(" << lon3 * math::r2d<CT>();
        std::cout << "," << lat3 * math::r2d<CT>();
        std::cout << ")\na1=" << a12 * math::r2d<CT>() << std::endl;
        std::cout << "a13=" << a13 * math::r2d<CT>() << std::endl;
        std::cout << "a312=" << a312 * math::r2d<CT>() << std::endl;
        std::cout << "cos(a312)=" << cos(a312) << std::endl;
#endif
        if (projection1 < 0.0)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "projection closer to p1" << std::endl;
#endif
            // projection of p3 on geodesic spanned by segment (p1,p2) fall
            // outside of segment on the side of p1
            return non_iterative_case(lon1, lat1, lon3, lat3, spheroid);
        }

        CT a21 = res12.reverse_azimuth - pi;
        CT a23 = inverse_azimuth_type::apply(lon2, lat2, lon3, lat3, spheroid).azimuth;

        CT a321 = a23 - a21;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "a21=" << a21 * math::r2d<CT>() << std::endl;
        std::cout << "a23=" << a23 * math::r2d<CT>() << std::endl;
        std::cout << "a321=" << a321 * math::r2d<CT>() << std::endl;
        std::cout << "cos(a321)=" << cos(a321) << std::endl;
#endif
        CT projection2 = cos( a321 ) * d2 / d3;

        if (projection2 < 0.0)
        {
#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "projection closer to p2" << std::endl;
#endif
            // projection of p3 on geodesic spanned by segment (p1,p2) fall
            // outside of segment on the side of p2
            return non_iterative_case(lon2, lat2, lon3, lat3, spheroid);
        }

        // Guess s14 (SPHERICAL)
        typedef geometry::model::point
                <
                    CT, 2,
                    geometry::cs::spherical_equatorial<geometry::radian>
                > point;

        point p1 = point(lon1, lat1);
        point p2 = point(lon2, lat2);
        point p3 = point(lon3, lat3);

        geometry::strategy::distance::cross_track<CT> cross_track(earth_radius);
        CT s34 = cross_track.apply(p3, p1, p2);

        geometry::strategy::distance::haversine<CT> str(earth_radius);
        CT s13 = str.apply(p1, p3);
        CT s14 = acos( cos(s13/earth_radius) / cos(s34/earth_radius) ) * earth_radius;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "s34=" << s34 << std::endl;
        std::cout << "s13=" << s13 << std::endl;
        std::cout << "s14=" << s14 << std::endl;
        std::cout << "===============" << std::endl;
#endif

        // Update s14 (using Newton method)
        CT prev_distance = 0;
        geometry::formula::result_direct<CT> res14;
        geometry::formula::result_inverse<CT> res34;

        int counter = 0; // robustness
        CT g4;
        CT delta_g4;

        do{
            prev_distance = res34.distance;

            // Solve the direct problem to find p4 (GEO)
            res14 = direct_distance_type::apply(lon1, lat1, s14, a12, spheroid);

            // Solve an inverse problem to find g4
            // g4 is the angle between segment (p1,p2) and segment (p3,p4) that meet on p4 (GEO)

            CT a4 = inverse_azimuth_type::apply(res14.lon2, res14.lat2,
                                                lon2, lat2, spheroid).azimuth;
            res34 = inverse_distance_quantities_type::apply(res14.lon2, res14.lat2,
                                                            lon3, lat3, spheroid);
            g4 = res34.azimuth - a4;

            CT M43 = res34.geodesic_scale; // cos(s14/earth_radius) is the spherical limit
            CT m34 = res34.reduced_length;
            CT der = (M43 / m34) * sin(g4);

            // normalize (g4 - pi/2)
            delta_g4 = normalize(g4, der);

            s14 = s14 - delta_g4 / der;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            std::cout << "p4=" << res14.lon2 * math::r2d<CT>() <<
                         "," << res14.lat2 * math::r2d<CT>() << std::endl;
            std::cout << "a34=" << res34.azimuth * math::r2d<CT>() << std::endl;
            std::cout << "a4=" << a4 * math::r2d<CT>() << std::endl;
            std::cout << "g4=" << g4 * math::r2d<CT>() << std::endl;
            std::cout << "delta_g4=" << delta_g4 * math::r2d<CT>()  << std::endl;
            std::cout << "der=" << der  << std::endl;
            std::cout << "M43=" << M43 << std::endl;
            std::cout << "spherical limit=" << cos(s14/earth_radius) << std::endl;
            std::cout << "m34=" << m34 << std::endl;
            std::cout << "new_s14=" << s14 << std::endl;
            std::cout << std::setprecision(16) << "dist     =" << res34.distance << std::endl;
            std::cout << "---------end of step " << counter << std::endl<< std::endl;
#endif
            result.distance = prev_distance;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
            if (g4 == half_pi)
            {
                std::cout << "Stop msg: g4 == half_pi" << std::endl;
            }
            if (res34.distance >= prev_distance && prev_distance != 0)
            {
                std::cout << "Stop msg: res34.distance >= prev_distance" << std::endl;
            }
            if (delta_g4 == 0)
            {
                std::cout << "Stop msg: delta_g4 == 0" << std::endl;
            }
            if (counter == 19)
            {
                std::cout << "Stop msg: counter" << std::endl;
            }
#endif

        } while (g4 != half_pi
                 && (prev_distance > res34.distance || prev_distance == 0)
                 && delta_g4 != 0
                 && ++counter < BOOST_GEOMETRY_DETAIL_POINT_SEGMENT_DISTANCE_MAX_STEPS ) ;

#ifdef BOOST_GEOMETRY_DEBUG_GEOGRAPHIC_CROSS_TRACK
        std::cout << "distance=" << res34.distance << std::endl;

        point p4(res14.lon2, res14.lat2);
        CT s34_sph = str.apply(p4, p3);

        std::cout << "s34(sph) =" << s34_sph << std::endl;
        std::cout << "s34(geo) ="
                  << inverse_distance_quantities_type::apply(get<0>(p4), get<1>(p4), lon3, lat3, spheroid).distance
                  << ", p4=(" << get<0>(p4) * math::r2d<double>() << ","
                              << get<1>(p4) * math::r2d<double>() << ")"
                  << std::endl;

        CT s31 = inverse_distance_quantities_type::apply(lon3, lat3, lon1, lat1, spheroid).distance;
        CT s32 = inverse_distance_quantities_type::apply(lon3, lat3, lon2, lat2, spheroid).distance;

        CT a4 = inverse_azimuth_type::apply(get<0>(p4), get<1>(p4), lon2, lat2, spheroid).azimuth;
        geometry::formula::result_direct<CT> res4 = direct_distance_type::apply(get<0>(p4), get<1>(p4), .04, a4, spheroid);
        CT p4_plus = inverse_distance_quantities_type::apply(res4.lon2, res4.lat2, lon3, lat3, spheroid).distance;

        geometry::formula::result_direct<CT> res1 = direct_distance_type::apply(lon1, lat1, s14-.04, a12, spheroid);
        CT p4_minus = inverse_distance_quantities_type::apply(res1.lon2, res1.lat2, lon3, lat3, spheroid).distance;

        std::cout << "s31=" << s31 << "\ns32=" << s32
                  << "\np4_plus=" << p4_plus << ", p4=(" << res4.lon2 * math::r2d<double>() << "," << res4.lat2 * math::r2d<double>() << ")"
                  << "\np4_minus=" << p4_minus << ", p4=(" << res1.lon2 * math::r2d<double>() << "," << res1.lat2 * math::r2d<double>() << ")"
                  << std::endl;

        if (res34.distance <= p4_plus && res34.distance <= p4_minus)
        {
            std::cout << "Closest point computed" << std::endl;
        }
        else
        {
            std::cout << "There is a closer point nearby" << std::endl;
        }
#endif

        return result;
    }

    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

//tags
template <typename FormulaPolicy>
struct tag<geographic_cross_track<FormulaPolicy> >
{
    typedef strategy_tag_distance_point_segment type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid> >
{
    typedef strategy_tag_distance_point_segment type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct tag<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_segment type;
};

//return types
template <typename FormulaPolicy, typename P, typename PS>
struct return_type<geographic_cross_track<FormulaPolicy>, P, PS>
    : geographic_cross_track<FormulaPolicy>::template return_type<P, PS>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename P,
        typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid>, P, PS>
    : geographic_cross_track<FormulaPolicy, Spheroid>::template return_type<P, PS>
{};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType,
        typename P,
        typename PS
>
struct return_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
    : geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>::template return_type<P, PS>
{};

//comparable types
template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct comparable_type<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef geographic_cross_track
        <
            FormulaPolicy, Spheroid, CalculationType
        >  type;
};

template
<
        typename FormulaPolicy,
        typename Spheroid,
        typename CalculationType
>
struct get_comparable<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> >
{
    typedef typename comparable_type
        <
            geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>
        >::type comparable_type;
public :
    static inline comparable_type
    apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& )
    {
        return comparable_type();
    }
};

template
<
    typename FormulaPolicy,
    typename P,
    typename PS
>
struct result_from_distance<geographic_cross_track<FormulaPolicy>, P, PS>
{
private :
    typedef typename geographic_cross_track
        <
            FormulaPolicy
        >::template return_type<P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_cross_track<FormulaPolicy> const& , T const& distance)
    {
        return distance;
    }
};

template
<
    typename FormulaPolicy,
    typename Spheroid,
    typename CalculationType,
    typename P,
    typename PS
>
struct result_from_distance<geographic_cross_track<FormulaPolicy, Spheroid, CalculationType>, P, PS>
{
private :
    typedef typename geographic_cross_track
        <
            FormulaPolicy, Spheroid, CalculationType
        >::template return_type<P, PS>::type return_type;
public :
    template <typename T>
    static inline return_type
    apply(geographic_cross_track<FormulaPolicy, Spheroid, CalculationType> const& , T const& distance)
    {
        return distance;
    }
};

template <typename Point, typename PointOfSegment>
struct default_strategy
    <
        point_tag, segment_tag, Point, PointOfSegment,
        geographic_tag, geographic_tag
    >
{
    typedef geographic_cross_track<> type;
};

template <typename PointOfSegment, typename Point>
struct default_strategy
    <
        segment_tag, point_tag, PointOfSegment, Point,
        geographic_tag, geographic_tag
    >
{
    typedef typename default_strategy
        <
            point_tag, segment_tag, Point, PointOfSegment,
            geographic_tag, geographic_tag
        >::type type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry
#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_DISTANCE_CROSS_TRACK_HPP


/*
 * poh_bfs_example.cpp
 * Author: Aven Bross
 *
 * An example using Poh with BFS to path 3-color a plane graph.
 */

// STL headers
#include <algorithm>
#include <iostream>
#include <vector>
#include <utility>

// Basic graph headers
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

// Planar graph headers
#include <boost/graph/boyer_myrvold_planar_test.hpp>

// Local project headers
#include "../../include/path_coloring/poh_color_bfs.hpp"
#include "../../include/path_coloring/incidence_list_helpers.hpp"

using namespace boost;

/*
 * -----------------------------------------------------------------------------
 *                         Template type definitions
 * -----------------------------------------------------------------------------
 */

// Define the graph type for all test graphs
typedef adjacency_list<
        vecS,
        vecS,
        undirectedS,
        property<vertex_index_t, std::size_t>,
        property<edge_index_t, std::size_t>
    > graph_t;

// Vertex and edge types
typedef typename graph_traits<graph_t>::vertex_descriptor vertex_t;
typedef typename graph_traits<graph_t>::edge_descriptor edge_t;

// Vertex and edge index map types
typedef typename property_map<graph_t, vertex_index_t>::const_type
    vertex_index_map_t;

// Vertex property map type for an integer property
typedef std::vector<int> integer_property_storage_t;
typedef iterator_property_map<
        typename integer_property_storage_t::iterator,
        vertex_index_map_t
    > integer_property_map_t;

// Vertex property map type for a planar embedding
typedef std::vector<std::vector<edge_t>> planar_embedding_storage_t;
typedef iterator_property_map<
        typename planar_embedding_storage_t::iterator,
        vertex_index_map_t
    > planar_embedding_t;

/*
 * -----------------------------------------------------------------------------
 *               Main: path 3-color a plane graph
 * -----------------------------------------------------------------------------
 */

int main() {
    // First we will construct a planar graph.
    graph_t graph(5);

    boost::add_edge(0, 1, graph);
    boost::add_edge(1, 2, graph);
    boost::add_edge(2, 0, graph);
    boost::add_edge(1, 3, graph);
    boost::add_edge(0, 3, graph);
    boost::add_edge(2, 3, graph);
    boost::add_edge(0, 4, graph);
    boost::add_edge(2, 4, graph);
    boost::add_edge(3, 4, graph);

    // We choose our outer face to be the triangle 012
    std::vector<vertex_t> path_1 = { 0 };
    std::vector<vertex_t> path_2 = { 1, 2 };

    // Create the planar embedding
    planar_embedding_storage_t planar_embedding_storage(num_vertices(graph));
    planar_embedding_t planar_embedding(
            planar_embedding_storage.begin(), get(vertex_index, graph)
        );
    for(std::size_t v=0; v < num_vertices(graph); ++v) {
        planar_embedding[v].reserve(out_degree(v, graph));
    }
    boyer_myrvold_planarity_test(
            boyer_myrvold_params::graph = graph,
            boyer_myrvold_params::embedding = planar_embedding
        );

    // Print embedding ordered adjacency list
    std::cout << "Embedding ordered adajacency lists:\n";
    for(std::size_t v=0; v < num_vertices(graph); ++v) {
        std::cout << "    Adj[" << v << "] = ";
        for(auto edge_iter = planar_embedding[v].begin();
            edge_iter != planar_embedding[v].end(); ++edge_iter)
        {
            if(edge_iter != planar_embedding[v].begin())
                std::cout << " -> ";
            std::cout << get_incident_vertex(v, *edge_iter, graph);
        }
        std::cout << "\n";
    }
    std::cout << "\n";

    // Create a vertex property map for the coloring
    integer_property_storage_t color_map_storage(num_vertices(graph));
    integer_property_map_t color_map(
            color_map_storage.begin(), get(vertex_index, graph)
        );

    // Call Poh with the given paths and structurs and color set { 1, 2, 3 }
    poh_color_bfs(
            graph,
            planar_embedding,
            path_1.begin(), path_1.end(),
            path_2.begin(), path_2.end(),
            1, 2, 3,
            color_map
        );

    // Print the coloring
    std::cout << "The path 3-coloring:\n";
    for(std::size_t v=0; v < num_vertices(graph); ++v) {
        std::cout << "    color[" << v << "] = " << color_map[v] << "\n";
    }

    return 0;
}


#include "Constraint/BoxConstraint.h"

#include <Eigen/Geometry>
#include <QtMath>

BoxConstraint::BoxConstraint(const QVector3D& position, const QVector3D& halfDimension, bool isBoundary) :
	m_position(position),
	m_halfDimension(halfDimension),
	m_isBoundary(isBoundary)
{
	setRotation(0.f, QVector3D(1.f, 0.f, 0.f));
}

BoxConstraint::~BoxConstraint()
{
}

void BoxConstraint::setPosition(const QVector3D& position)
{
	m_position = position;
}

void BoxConstraint::setHalfDimension(const QVector3D& halfDimension)
{
	m_halfDimension = halfDimension;
}

void BoxConstraint::setRotation(float angleDegrees, const QVector3D& axis)
{
	m_angle = angleDegrees;
	m_rotationAxis = axis;
	m_rotation = Eigen::AngleAxisf((angleDegrees / 180.f) * M_PI, Eigen::Vector3f(axis.x(), axis.y(), axis.z()));
}

void BoxConstraint::setIsBoundary(bool isBoundary)
{
	m_isBoundary = isBoundary;
}

const QVector3D& BoxConstraint::getPosition()
{
	return m_position;
}

const QVector3D& BoxConstraint::getHalfDimension()
{
	return m_halfDimension;
}

const Eigen::Matrix3f& BoxConstraint::getRotation()
{
	return m_rotation;
}

float BoxConstraint::getAngle()
{
	return m_angle;
}

const QVector3D& BoxConstraint::getRotationAxis()
{
	return m_rotationAxis;
}

bool BoxConstraint::getIsBoundary()
{
	return m_isBoundary;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2016 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle

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

#include <algorithms/test_perimeter.hpp>
#include <algorithms/perimeter/perimeter_polygon_cases.hpp>

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

template <typename P>
void test_all_default() //test the default strategy
{
    double const pi = boost::math::constants::pi<double>();

    for (std::size_t i = 0; i <= 2; ++i)
    {
        test_geometry<bg::model::polygon<P> >(poly_data_sph[i], 2 * pi);
    }

    // Multipolygon
    test_geometry<bg::model::multi_polygon<bg::model::polygon<P> > >
                                            (multipoly_data[0], 3 * pi);

    // Geometries with length zero
    test_geometry<P>("POINT(0 0)", 0);
    test_geometry<bg::model::linestring<P> >("LINESTRING(0 0,3 4,4 3)", 0);
}

template <typename P>
void test_all_haversine(double const mean_radius)
{
    double const pi = boost::math::constants::pi<double>();
    bg::strategy::distance::haversine<double> haversine_strategy(mean_radius);

    for (std::size_t i = 0; i <= 2; ++i)
    {
        test_geometry<bg::model::polygon<P> >(poly_data_sph[i],
                                              2 * pi * mean_radius,
                                              haversine_strategy);
    }

    // Multipolygon
    test_geometry<bg::model::multi_polygon<bg::model::polygon<P> > >
                                            (multipoly_data[0],
                                             3 * pi * mean_radius,
                                             haversine_strategy);

    // Geometries with length zero
    test_geometry<P>("POINT(0 0)", 0, haversine_strategy);
    test_geometry<bg::model::linestring<P> >("LINESTRING(0 0,3 4,4 3)",
                                             0,
                                             haversine_strategy);
}

int test_main(int, char* [])
{
    //Earth radius estimation in Km
    //(see https://en.wikipedia.org/wiki/Earth_radius)
    double const mean_radius = 6371.0;

    test_all_default<bg::model::d2::point_xy<int,
            bg::cs::spherical_equatorial<bg::degree> > >();
    test_all_default<bg::model::d2::point_xy<float,
            bg::cs::spherical_equatorial<bg::degree> > >();
    test_all_default<bg::model::d2::point_xy<double,
            bg::cs::spherical_equatorial<bg::degree> > >();

    test_all_haversine<bg::model::d2::point_xy<int,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);
    test_all_haversine<bg::model::d2::point_xy<float,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);
    test_all_haversine<bg::model::d2::point_xy<double,
        bg::cs::spherical_equatorial<bg::degree> > >(mean_radius);

#if defined(HAVE_TTMATH)
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


#pragma once
#include <Eigen/Sparse>
#include <Eigen/SparseLU>
#include <vector>

#include "basis.hpp"
#include "triangulation_view.hpp"

namespace space {

struct OperatorOptions {
  // Options for all operators.
  bool dirichlet_boundary = true;

  // Whether or not to build the matrix for a forward operator.
  bool build_mat = false;

  // Options for stiff plus scaled mass operator.
  size_t time_level = 0;
  double alpha = 1;

  // Options for multigrid preconditioner.
  size_t mg_cycles = 3;

  OperatorOptions() = default;

  friend std::ostream &operator<<(std::ostream &os,
                                  const OperatorOptions &opts) {
    os << "OperatorOptions:" << std::endl;
    os << "\tdirichlet_boundary: "
       << (opts.dirichlet_boundary ? "true" : "false") << std::endl;
    os << "\tbuild_mat: " << (opts.build_mat ? "true" : "false") << std::endl;
    os << "\ttime_level: " << opts.time_level << std::endl;
    os << "\talpha: " << opts.alpha << std::endl;
    os << "\tcycles: " << opts.mg_cycles << std::endl;
    return os;
  }
};

class Operator {
 public:
  Operator(const TriangulationView &triang,
           OperatorOptions opts = OperatorOptions())
      : triang_(triang), opts_(std::move(opts)) {}

  virtual ~Operator() {}

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const = 0;

  // Does the given vertex correspond to a dof?
  inline bool IsDof(uint vertex) const {
    return !triang_.OnBoundary(vertex) || !opts_.dirichlet_boundary;
  }

  // Verify that the given vector satisfy the boundary conditions.
  bool FeasibleVector(const Eigen::VectorXd &vec) const;
  inline bool DirichletBoundary() const { return opts_.dirichlet_boundary; }

  // Overloads required to for Eigen.
  Eigen::VectorXd operator*(const Eigen::VectorXd &vec_in) const {
    Eigen::VectorXd result = vec_in;
    Apply(result);
    return result;
  }
  size_t rows() const { return triang_.V; }
  size_t cols() const { return triang_.V; }

  // Debug function for turning this operator into a mtrix.
  Eigen::MatrixXd ToMatrix() const;

 protected:
  const TriangulationView &triang_;
  OperatorOptions opts_;
};

template <class ForwardOp>
class ForwardOperator : public Operator {
 public:
  ForwardOperator(const TriangulationView &triang,
                  OperatorOptions opts = OperatorOptions());

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const final;

  // Apply the operator in single scale, to be implemented by derived.
  void ApplySingleScale(Eigen::VectorXd &vec_SS) const;

  const Eigen::SparseMatrix<double> &MatrixSingleScale() const {
    assert(matrix_.nonZeros());
    return matrix_;
  }

  // Hierarhical Basis Transformations from HB to SS, and its transpose.
  void ApplyHierarchToSingle(Eigen::VectorXd &vec_HB) const;
  void ApplyTransposeHierarchToSingle(Eigen::VectorXd &vec_SS) const;

 protected:
  void InitializeMatrixSingleScale();
  Eigen::SparseMatrix<double> matrix_;
};

class BackwardOperator : public Operator {
 public:
  BackwardOperator(const TriangulationView &triang,
                   OperatorOptions opts = OperatorOptions());

  // Apply the operator in the hierarchical basis.
  virtual void Apply(Eigen::VectorXd &vec_in) const final;

  // Apply the operator in single scale, to be implemented by derived.
  virtual void ApplySingleScale(Eigen::VectorXd &vec_SS) const = 0;

 protected:
  // Inverse Hierarhical Basis Transformations.
  void ApplyInverseHierarchToSingle(Eigen::VectorXd &vec_SS) const;
  void ApplyTransposeInverseHierarchToSingle(Eigen::VectorXd &vec_HB) const;

  // Vertex-to-DoF transformation matrices.
  Eigen::SparseMatrix<double> transform_;
  Eigen::SparseMatrix<double> transformT_;
};

/**
 *  Implementation of the actual operators.
 */
class MassOperator : public ForwardOperator<MassOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static Eigen::Matrix3d ElementMatrix(const Element2D *elem,
                                              const OperatorOptions &opts);
};

class StiffnessOperator : public ForwardOperator<StiffnessOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static const Eigen::Matrix3d &ElementMatrix(
      const Element2D *elem, const OperatorOptions &opts);
};

class StiffPlusScaledMassOperator
    : public ForwardOperator<StiffPlusScaledMassOperator> {
 public:
  // Inherit constructor.
  using ForwardOperator::ForwardOperator;

  // Returns the element matrix for the given element.
  inline static Eigen::Matrix3d ElementMatrix(const Element2D *elem,
                                              const OperatorOptions &opts);
};

template <typename ForwardOp>
class DirectInverse : public BackwardOperator {
 public:
  DirectInverse(const TriangulationView &triang,
                OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>>
      solver_;
};

template <typename ForwardOp>
class CGInverse : public BackwardOperator {
 public:
  CGInverse(const TriangulationView &triang,
            OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  Eigen::ConjugateGradient<Eigen::SparseMatrix<double>,
                           Eigen::Lower | Eigen::Upper>
      solver_;
};

template <typename ForwardOp>
class MultigridPreconditioner : public BackwardOperator {
 public:
  MultigridPreconditioner(const TriangulationView &triang,
                          OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

  inline void Prolongate(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[vertex] += 0.5 * vec_SS[gp];
  }

  inline void Restrict(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[gp] += 0.5 * vec_SS[vertex];
  }

  inline void RestrictInverse(uint vertex, Eigen::VectorXd &vec_SS) const {
    for (auto gp : triang_.Godparents(vertex))
      vec_SS[gp] -= 0.5 * vec_SS[vertex];
  }

 protected:
  // Initializes the static variable row_mat.
  void InitializeMultigridMatrix() const;

  // Returns a row of the _forward_ matrix on the given multilevel triang.
  // NOTE: The result might not be compressed.
  inline void RowMatrix(uint vertex,
                        std::vector<std::pair<uint, double>> &result) const;

  // Forward operator on the finest level.
  ForwardOp forward_op_;

  // Solver on the coarsest level.
  DirectInverse<ForwardOp> initial_triang_solver_;

  // (Static) variables reused for calculation of the multigrid matrix.
  static std::vector<std::vector<std::pair<uint, double>>> row_mat;
  static std::vector<std::vector<Element2D *>> patches;
  static std::vector<std::vector<uint>> vertices_relaxation;
};

template <template <typename> class InverseOp>
class XPreconditionerOperator : public BackwardOperator {
 public:
  XPreconditionerOperator(const TriangulationView &triang,
                          OperatorOptions opts = OperatorOptions());

  void ApplySingleScale(Eigen::VectorXd &vec_SS) const final;

 protected:
  StiffnessOperator stiff_op_;
  InverseOp<StiffPlusScaledMassOperator> inverse_op_;
};

extern template class ForwardOperator<MassOperator>;
extern template class ForwardOperator<StiffnessOperator>;
extern template class ForwardOperator<StiffPlusScaledMassOperator>;

extern template class DirectInverse<MassOperator>;
extern template class DirectInverse<StiffnessOperator>;
extern template class DirectInverse<StiffPlusScaledMassOperator>;

extern template class MultigridPreconditioner<MassOperator>;
extern template class MultigridPreconditioner<StiffnessOperator>;
extern template class MultigridPreconditioner<StiffPlusScaledMassOperator>;

extern template class XPreconditionerOperator<DirectInverse>;
extern template class XPreconditionerOperator<MultigridPreconditioner>;

}  // namespace space


/*
 * Copyright (c) 2012 Jonathan Perry
 * This code is released under the MIT license (see LICENSE file).
 */
#include "channels/MimoChannel.h"

#include <boost/numeric/ublas/operation.hpp>

MimoChannel::MimoChannel(
		unsigned int nTransmitter,
		unsigned int nReceiver,
		const std::vector<ComplexNumber> & channelMatrix)
  : m_nTransmitter(nTransmitter),
    m_nReceiver(nReceiver),
    m_channelMatrix(nReceiver, nTransmitter)
{
	for(unsigned int row = 0; row < nReceiver; row++) {
		for(unsigned int col = 0; col < nTransmitter; col++) {
			m_channelMatrix(row, col) = channelMatrix[row * nTransmitter + col];
		}
	}
}

void MimoChannel::seed(unsigned int *const , int )
{
	// no RNG in this class
	return;
}

void MimoChannel::process(
		const std::vector<ComplexSymbol> & inSymbols,
		std::vector<ComplexSymbol> & outSymbols)
{
	unsigned int numIQSymbols = inSymbols.size();

	unsigned int numChannelUsages = numIQSymbols / m_nTransmitter;

	if (inSymbols.size() != m_nTransmitter * numChannelUsages) {
		throw std::runtime_error("input symbols size should contain symbols for all transmit antenna");
	}

	unsigned int numOutputSymbols = numChannelUsages * m_nReceiver;
	outSymbols.clear();
	outSymbols.resize(numOutputSymbols);

	// Initialize a matrix with all symbols
	boost::numeric::ublas::matrix<ComplexSymbol> symbolMatrix(m_nTransmitter,
															  numChannelUsages);
	unsigned int inSymbolIter = 0;
	for(unsigned int col = 0; col < numChannelUsages; col++) {
		for(unsigned int row = 0; row < m_nTransmitter; row++) {
			symbolMatrix(row, col) = inSymbols[inSymbolIter++];
		}
	}

	// Multiply channel matrix by symbol matrix
	boost::numeric::ublas::matrix<ComplexSymbol> outputMatrix(m_nReceiver,
															  numChannelUsages);
	axpy_prod(m_channelMatrix, symbolMatrix, outputMatrix, true);

	// Read the output symbols from outputMatrix
	unsigned int outSymbolIter = 0;
	for(unsigned int col = 0; col < numChannelUsages; col++) {
		for(unsigned int row = 0; row < m_nReceiver; row++) {
			outSymbols[outSymbolIter++] = outputMatrix(row, col);
		}
	}

}

unsigned int MimoChannel::forecast(unsigned int numOutputs)
{
	unsigned int numChannelUsages = (numOutputs + m_nReceiver - 1) / m_nReceiver;
	return numChannelUsages * m_nTransmitter;
}


// (C) Copyright Andrew Sutton 2007
//
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0 (See accompanying file
// LICENSE_1_0.txt or http://www.boost.org/LICENSE_1_0.txt)

//[code_bron_kerbosch_clique_number
#include <iostream>

#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/bron_kerbosch_all_cliques.hpp>

#include "helper.hpp"

using namespace std;
using namespace boost;

// Declare the graph type and its vertex and edge types.
typedef undirected_graph<> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

int
main(int argc, char *argv[])
{
    // Create the graph and read it from standard input.
    Graph g;
    read_graph(g, cin);

    // Use the Bron-Kerbosch algorithm to find all cliques, and
    size_t c = bron_kerbosch_clique_number(g);
    cout << "clique number: " << c << endl;

    return 0;
}
//]


#include <iostream>
#include <vector>
#include <locale>
#include <omp.h>

#include <boost/multiprecision/cpp_int.hpp>
using namespace boost::multiprecision;

#include "ThreadPool.h"

// Included from collat_asm.asm (.o/.obj)
int16_t collatz(uint64_t *, int64_t);

struct record_t {
	int64_t number;
	int16_t steps;
};

int64_t overflowCollatz(uint64_t number_, int64_t bufferSize, int16_t &extraSteps) {
	cpp_int number = number_;

	while (number > bufferSize) {
		if (number % 2 == 0) {
			number = number / 2;
		}
		else {
			number = number * 3 + 1;
		}
		extraSteps++;
	}

	return number.convert_to<int64_t>();
}

void fillLUT(std::vector<int16_t> &LUT, record_t &largest) {

	std::vector<int64_t> sequence;
	int64_t LUTSum = 0;

	// Fill the lookup table with distances
	for (int64_t i = 3; i < LUT.size(); i++) {

		// If the number has already been added to the look up table
		if (LUT[i] != 0) {
			if (LUT[i] > largest.steps) {
				largest.steps = LUT[i];
				largest.number = i;
				// VT100 escape character for clearing line only works if running through visual studio
				std::cout << "\r                                                                                ";//"\33[2K";
				std::cout << "\r" << largest.number << " : " << largest.steps << "\n";
			}
			continue;
		}

		int64_t number = i;

		// Compute sequence until we find a number that has already been computed
		while (true) {
			if (number < LUT.size()) {
				if (LUT[number] != 0) break;
				sequence.push_back(number);
			}
			else {
				// Don't need to save the number if the sequence goes above the lookup table size
				// but 0 is used as a place holder to get the correct amount of steps
				sequence.push_back(0);
			}

			if (number % 2 == 0) {
				number = number / 2;
			}
			else {
				number = 3 * number + 1;
			}
		}

		// Iterate through the sequence and add all steps to the lookup table
		int16_t sequenceSteps = 0;
		int16_t LUTSteps = LUT[number];
		for (auto it = sequence.rbegin(); it != sequence.rend(); it++) {
			sequenceSteps++;

			if (*it > 0) {
				LUT[*it] = LUTSteps + sequenceSteps;
				LUTSum += static_cast<int64_t>(LUTSteps) + sequenceSteps;
			}
		}

		sequence.clear();

		if (LUT[i] > largest.steps) {
			largest.steps = LUT[i];
			largest.number = i;
			// VT100 escape character for clearing line only works if running through visual studio
			std::cout << "\r                                                                                ";//"\33[2K";
			std::cout << "\r" << i << " : " << largest.steps << "\n";
			std::cout << "Filling lookup table. Currently on: " << i << " of " << LUT.size() << std::flush;
		}
		else if (i % 1000000 == 0) {
			std::cout << "\rFilling lookup table. Currently on: " << i << " of " << LUT.size() << std::flush;
		}
	}

	std::cout << "\33[2K";
	std::cout << "\r" << "Lookup table filled. Avg steps: " << LUTSum / LUT.size() << "\n";
}

class multiThreadCollatz {
public:
	std::vector<int16_t> &LUT;
	std::vector<std::vector<record_t>> &potentialRecords;
	int64_t start;
	int64_t end;
	int64_t blockIndex;
	record_t largest;

	void operator()() {
		for (int64_t j = start; j < end; j++) {
			uint64_t number = j;

			int16_t steps = collatz(&number, LUT.size());
			if (number > LUT.size()) {
				number = overflowCollatz(number, LUT.size(), steps);
			}

			int16_t totalSteps = LUT[number] + steps;
			if (totalSteps > largest.steps) {
				largest = record_t{ j, totalSteps };
				potentialRecords[blockIndex].push_back(largest);
			}
		}
	}
};

enum LUT_SIZE : long long { L1 = 16000, L2 = 130000, L3 = 1000000, RAM = 5000000000 };

int main() {
	// Adding space as a thousands separator for cout
	struct separate_thousands : std::numpunct<char> {
		char_type do_thousands_sep() const override { return ' '; }  // separate with space
		string_type do_grouping() const override { return "\3"; } // groups of 3 digit
	};
	auto thousands = std::make_unique<separate_thousands>();
	std::cout.imbue(std::locale(std::cout.getloc(), thousands.release()));

	const int64_t LUTSize = LUT_SIZE::RAM;
	const int64_t blocks = 100;
	const int64_t iterationsPerBlock = 1e6;

	// Allocate memory for the lookup table
	std::cout << "Allocting " << LUTSize * sizeof(int16_t) << " Bytes of memory" << std::endl;
	std::vector<int16_t> LUT(LUTSize);

	// Setting up the beginning of the lookup table
	LUT[2] = 1;
	std::cout << "1 : " << LUT[1] << std::endl;
	std::cout << "2 : " << LUT[2] << std::endl;
	record_t largest{ 2, LUT[2] };

	// Fill the lookup table and update the largest distance
	fillLUT(LUT, largest);

	ThreadPool<multiThreadCollatz> pool(10);

	// Each block stores all potential records in their own vector
	std::vector<std::vector<record_t>> potentialRecords(blocks);

	std::vector<record_t> records;
	for (int64_t i = LUTSize; i < INT64_MAX; i += (blocks * iterationsPerBlock)) {
		std::cout << "\rCurrently on " << i << std::flush;

		// Start filling buffer 1
		for (int64_t blockIndex = 0; blockIndex < blocks; blockIndex++) {
			pool.addWork(multiThreadCollatz{ LUT, potentialRecords, i + blockIndex * iterationsPerBlock, i + (blockIndex + 1) * iterationsPerBlock, blockIndex, largest });
		}

		pool.waitForThreads();

		// Combine all potential records in a vector and sort it
		for (std::vector<record_t> &blockRecords : potentialRecords) {
			std::copy(blockRecords.begin(), blockRecords.end(), std::back_inserter(records));
			blockRecords.clear();
		}
		std::sort(records.begin(), records.end(), [](record_t const &rhs, record_t const &lhs) { return rhs.number < lhs.number; });

		for (const record_t record : records) {
			if (record.steps > largest.steps) {
				largest = record;
				std::cout << "\r                                                                                ";//"\33[2K";
				std::cout << "\r" << largest.number << " : " << largest.steps << std::endl;
			}
		}
		records.clear();
	}
}


/*
 * Copyright (c) 2015, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    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.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 *          Erik Nelson            ( eanelson@eecs.berkeley.edu )
 */

#include <Eigen/Dense>
#include <gflags/gflags.h>
#include <iostream>

#include <geometry/rotation.h>
#include <math/random_generator.h>
#include <pose/pose.h>

#include <gtest/gtest.h>

namespace bsfm {

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

TEST(Pose, TestPoseAxisAngle) {
  // Create a simple rotation matrix and random translation vector.
  Eigen::Matrix3d R;
  R << cos(0.5), -sin(0.5), 0,
       sin(0.5),  cos(0.5), 0,
       0,         0,        1;

  const Vector3d t = Vector3d::Random();
  const Pose p1 = Pose(R, t);
  Pose p2 = p1;

  // Convert to/from axis angle representation and check nothing has changed.
  Vector3d aa = p2.AxisAngle();
  p2.FromAxisAngle(aa);
  EXPECT_TRUE(p1.IsApprox(p2));
}

TEST(Pose, TestPoseDelta) {

  math::RandomGenerator rng(0);

  for (int ii = 0; ii < 100; ++ii) {
    // Repeatedly make two poses, compose them, and compute the relative
    // transformation between them. Check that this value is correct.
    const double phi1 = rng.DoubleUniform(-M_PI, M_PI);
    const double phi2 = rng.DoubleUniform(-M_PI, M_PI);
    const double theta1 = rng.DoubleUniform(-M_PI, M_PI);
    const double theta2 = rng.DoubleUniform(-M_PI, M_PI);
    const double psi1 = rng.DoubleUniform(-M_PI, M_PI);
    const double psi2 = rng.DoubleUniform(-M_PI, M_PI);

    // Make 2 rotation matrices.
    const Matrix3d R1(EulerAnglesToMatrix(phi1, theta1, psi1));
    const Matrix3d R2(EulerAnglesToMatrix(phi2, theta2, psi2));
    const Vector3d t1(Vector3d::Random());
    const Vector3d t2(Vector3d::Random());

    // Compose the two poses.
    Pose p1(R1, t1);
    Pose p2(R2, t2);
    Pose composed = p1 * p2;

    // Get the deltas between p1 and p2, and between p2 and p1. These should be
    // inverses of one another.
    Pose p12 = p1.Delta(p2);
    Pose p21 = p2.Delta(p1);
    EXPECT_TRUE(p12.IsApprox(p21.Inverse()));

    // Make sure the deltas are what we would expect.
    Matrix4d Rt1(Matrix4d::Identity());
    Rt1.block(0, 0, 3, 3) = R1;
    Rt1.block(0, 3, 3, 1) = t1;

    Matrix4d Rt2(Matrix4d::Identity());
    Rt2.block(0, 0, 3, 3) = R2;
    Rt2.block(0, 3, 3, 1) = t2;

    Matrix4d expected_delta12 = Rt1.inverse() * Rt2;
    Matrix4d expected_delta21 = Rt2.inverse() * Rt1;
    EXPECT_TRUE(expected_delta12.isApprox(p12.Get()));
    EXPECT_TRUE(expected_delta21.isApprox(p21.Get()));
  }

}

} // namespace bsfm


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <algorithm>
#include <glog/logging.h>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/essential_matrix_utils.h"
#include "theia/sfm/pose/test_util.h"
#include "theia/sfm/pose/util.h"
#include "theia/util/random.h"
#include "gtest/gtest.h"

namespace theia {

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

RandomNumberGenerator rng(51);

TEST(DecomposeEssentialMatrix, BasicTest) {
  const double kTranslationTolerance = 1e-6;
  const double kRotationTolerance = 1e-4;

  for (int i = 0; i < 100; i++) {
    const Matrix3d gt_rotation = RandomRotation(10.0, &rng);
    const Vector3d gt_translation = rng.RandVector3d().normalized();
    const Matrix3d essential_matrix =
        CrossProductMatrix(gt_translation) * gt_rotation;

    Matrix3d rotation1, rotation2;
    Vector3d translation;
    DecomposeEssentialMatrix(
        essential_matrix, &rotation1, &rotation2, &translation);

    const double translation_dist =
        std::min((translation - gt_translation).norm(),
                 (translation + gt_translation).norm());

    const Eigen::AngleAxisd rotation1_aa(gt_rotation.transpose() * rotation1);
    const Eigen::AngleAxisd rotation2_aa(gt_rotation.transpose() * rotation2);
    const double rotation1_dist = rotation1_aa.angle();
    const double rotation2_dist = rotation2_aa.angle();

    EXPECT_TRUE(translation_dist < kTranslationTolerance &&
                (rotation1_dist < kRotationTolerance ||
                 rotation2_dist < kRotationTolerance));
  }
}

TEST(EssentialMatrixFromTwoProjectionMatrices, BasicTest) {
  const double kTranslationTolerance = 1e-6;
  const double kRotationTolerance = 1e-4;

  for (int i = 0; i < 100; i++) {
    const Eigen::Matrix3d in_rotation1 = RandomRotation(10.0, &rng);
    const Eigen::Vector3d in_position1 =
        Eigen::Vector3d::Zero();  // rng.RandVector3d().normalized();
    const Eigen::Vector3d in_translation1 = -in_rotation1 * in_position1;
    const Eigen::Matrix3d in_rotation2 = RandomRotation(10.0, &rng);
    const Eigen::Vector3d in_position2 = rng.RandVector3d().normalized();
    const Eigen::Vector3d in_translation2 = -in_rotation2 * in_position2;

    Matrix3x4d proj1, proj2;
    proj1.leftCols<3>() = in_rotation1;
    proj1.rightCols<1>() = in_translation1;
    proj2.leftCols<3>() = in_rotation2;
    proj2.rightCols<1>() = in_translation2;

    // Get the essential matrix.
    Eigen::Matrix3d essential_matrix;
    EssentialMatrixFromTwoProjectionMatrices(proj1, proj2, &essential_matrix);

    Matrix3d rotation1, rotation2;
    Vector3d translation;
    DecomposeEssentialMatrix(
        essential_matrix, &rotation1, &rotation2, &translation);

    const Eigen::Vector3d gt_translation =
        -in_rotation1 * (in_position2 - in_position1);
    const double translation_dist =
        std::min((translation - gt_translation).norm(),
                 (translation + gt_translation).norm());

    const Eigen::Matrix3d gt_rotation = in_rotation1 * in_rotation2.transpose();
    const Eigen::AngleAxisd rotation1_aa(gt_rotation.transpose() * rotation1);
    const Eigen::AngleAxisd rotation2_aa(gt_rotation.transpose() * rotation2);
    const double rotation1_dist = rotation1_aa.angle();
    const double rotation2_dist = rotation2_aa.angle();

    ASSERT_TRUE(translation_dist < kTranslationTolerance);
    ASSERT_TRUE(rotation1_dist < kRotationTolerance ||
                rotation2_dist < kRotationTolerance);
  }
}

void TestGetBestPoseFromEssentialMatrix(const int num_inliers,
                                        const int num_outliers) {
  static const double kTolerance = 1e-12;

  for (int i = 0; i < 100; i++) {
    const Matrix3d gt_rotation = RandomRotation(15.0, &rng);

    const Vector3d gt_translation = rng.RandVector3d().normalized();
    const Vector3d gt_position = -gt_rotation.transpose() * gt_translation;
    const Matrix3d essential_matrix =
        CrossProductMatrix(gt_translation) * gt_rotation;

    // Create Correspondences.
    std::vector<FeatureCorrespondence> correspondences;
    for (int j = 0; j < num_inliers; j++) {
      // Make sure the point is in front of the camera.
      const Vector3d point_3d = rng.RandVector3d() + Vector3d(0, 0, 100);
      const Vector3d proj_3d = gt_rotation * point_3d + gt_translation;

      FeatureCorrespondence correspondence;
      correspondence.feature1.point_ = point_3d.hnormalized();
      correspondence.feature2.point_ = proj_3d.hnormalized();
      correspondences.emplace_back(correspondence);
    }

    // Add outliers
    for (int j = 0; j < num_outliers; j++) {
      // Make sure the point is in front of the camera.
      const Vector3d point_3d = rng.RandVector3d() + Vector3d(0, 0, -100);
      const Vector3d proj_3d = gt_rotation * point_3d + gt_translation;

      FeatureCorrespondence correspondence;
      correspondence.feature1.point_ = point_3d.hnormalized();
      correspondence.feature2.point_ = proj_3d.hnormalized();
      correspondences.emplace_back(correspondence);
    }

    Matrix3d estimated_rotation;
    Vector3d estimated_position;
    const int num_points_in_front =
        GetBestPoseFromEssentialMatrix(essential_matrix,
                                       correspondences,
                                       &estimated_rotation,
                                       &estimated_position);

    // Ensure that the results are correct. Sincer there is no noise we can
    // expect te number of point in front to be exact.
    EXPECT_EQ(num_points_in_front, num_inliers);
    EXPECT_LT((gt_rotation - estimated_rotation).norm(), kTolerance);
    EXPECT_LT((gt_position - estimated_position).norm(), kTolerance);
  }
}

TEST(GetBestPoseFromEssentialMatrix, AllInliers) {
  static const int kNumInliers = 100;
  static const int kNumOutliers = 0;
  TestGetBestPoseFromEssentialMatrix(kNumInliers, kNumOutliers);
}

TEST(GetBestPoseFromEssentialMatrix, MostlyInliers) {
  static const int kNumInliers = 100;
  static const int kNumOutliers = 50;
  TestGetBestPoseFromEssentialMatrix(kNumInliers, kNumOutliers);
}

}  // namespace theia