/**********************************************************************
*  Copyright (c) 2008-2016, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#include "Geometry.hpp"
#include "Intersection.hpp"
#include "../data/Matrix.hpp"
#include "../core/Assert.hpp"
#include "../core/Logger.hpp"

#undef BOOST_UBLAS_TYPE_CHECK
#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/ring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_franklin.hpp>
#include <boost/geometry/strategies/cartesian/point_in_poly_crossings_multiply.hpp>
#include <boost/geometry/algorithms/within.hpp>

typedef boost::geometry::model::d2::point_xy<double> BoostPoint;
typedef boost::geometry::model::polygon<BoostPoint> BoostPolygon;
typedef boost::geometry::model::ring<BoostPoint> BoostRing;
typedef boost::geometry::model::multi_polygon<BoostPolygon> BoostMultiPolygon;

#include <polypartition/polypartition.h>

#include <list>

// remove_spikes
// adapted from https://github.com/boostorg/geometry/commits/develop/include/boost/geometry/algorithms/remove_spikes.hpp eb3260708eb241d8da337f4be73b41d69d33cd09

/*
Remove spikes from a ring/polygon.
Ring (having 8 vertices, including closing vertex)
+------+
| |
| +--+
| | ^this "spike" is removed, can be located outside/inside the ring
+------+
(the actual determination if it is removed is done by a strategy)

*/

namespace openstudio {

  template <typename Point1, typename Point2, typename Point3>
  static inline bool point_is_spike_or_equal(Point1 const& last_point, Point2 const& segment_a, Point3 const& segment_b)
  {
    // adapted from boost\geometry\algorithms\detail\point_is_spike_or_equal.hpp to include tolerance checking

    // segment_a is at the beginning
    // segment_b is in the middle
    // last_point is at the end

    // segment_b is being considered for deletion

    double normTol = 0.001; // 1 mm
    double tol = 0.001; // relative to 1

    double diff1_x = last_point.x()-segment_b.x();
    double diff1_y = last_point.y()-segment_b.y();
    double norm1 = sqrt(pow(diff1_x, 2) + pow(diff1_y, 2));
    if (norm1 > normTol){
      diff1_x = diff1_x/norm1;
      diff1_y = diff1_y/norm1;
    }else{
      // last point is too close to segment b
      return true;
    }

    double diff2_x = segment_b.x()-segment_a.x();
    double diff2_y = segment_b.y()-segment_a.y();
    double norm2 = sqrt(pow(diff2_x, 2) + pow(diff2_y, 2));
    if (norm2 > normTol){
      diff2_x = diff2_x/norm2;
      diff2_y = diff2_y/norm2;
    }else{
      // segment b is too close to segment a
      return true;
    }

    double crossProduct = diff1_x*diff2_y-diff1_y*diff2_x;
    if (abs(crossProduct) < tol){
      double dotProduct = diff1_x*diff2_x+diff1_y*diff2_y;
      if (dotProduct <= -1.0 + tol){
        // reversal
        return true;
      }
    }

    return false;
  }

}

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace remove_spikes
{

template <typename Range>
struct range_remove_spikes
{
    typedef typename strategy::side::services::default_strategy
    <
        typename cs_tag<Range>::type
    >::type side_strategy;

    typedef typename coordinate_type<Range>::type coordinate_type;
    typedef typename point_type<Range>::type point_type;

    static inline void apply(Range& range)
    {
        std::size_t n = boost::size(range);
        std::size_t const min_num_points = core_detail::closure::minimum_ring_size
            <
                geometry::closure<Range>::value
            >::value;
        if (n < min_num_points)
        {
            return;
        }

        std::deque<point_type> cleaned;
        for (typename boost::range_iterator<Range const>::type it = boost::begin(range);
            it != boost::end(range); ++it)
        {
            // Add point
            cleaned.push_back(*it);

            while(cleaned.size() >= 3
                    && openstudio::point_is_spike_or_equal(cleaned.back(), *(cleaned.end() - 3), *(cleaned.end() - 2)))
            {
                // Remove pen-ultimate point causing the spike (or which was equal)
                cleaned.erase(cleaned.end() - 2);
            }
        }

        // For a closed-polygon, remove closing point, this makes checking first point(s) easier and consistent
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.pop_back();
        }

        bool found = false;
        do
        {
            found = false;
            // Check for spike in first point
            int const penultimate = 2;
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(cleaned.front(), *(cleaned.end() - penultimate), cleaned.back()))
            {
                cleaned.pop_back();
                found = true;
            }
            // Check for spike in second point
            while(cleaned.size() > 3 && openstudio::point_is_spike_or_equal(*(cleaned.begin() + 1), cleaned.back(), cleaned.front()))
            {
                cleaned.pop_front();
                found = true;
            }
        }
        while (found);

        // Close if necessary
        if (geometry::closure<Range>::value == geometry::closed)
        {
            cleaned.push_back(cleaned.front());
        }

        // Copy output
        geometry::clear(range);
        std::copy(cleaned.begin(), cleaned.end(), std::back_inserter(range));
    }
};

template <typename Polygon>
struct polygon_remove_spikes
{
    static inline void apply(Polygon& polygon)
    {
        typedef typename geometry::ring_type<Polygon>::type ring_type;

        typedef range_remove_spikes<ring_type> per_range;
        per_range::apply(exterior_ring(polygon));

        typename interior_return_type<Polygon>::type rings
                    = interior_rings(polygon);
        for (BOOST_AUTO_TPL(it, boost::begin(rings)); it != boost::end(rings); ++it)
        {
            per_range::apply(*it);
        }
    }
};

}} // namespace detail::remove_spikes
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry,
    typename Tag = typename tag<Geometry>::type
>
struct remove_spikes
{
    static inline void apply(Geometry&)
    {}
};

template <typename Ring>
struct remove_spikes<Ring, ring_tag>
    : detail::remove_spikes::range_remove_spikes<Ring>
{};

template <typename Polygon>
struct remove_spikes<Polygon, polygon_tag>
    : detail::remove_spikes::polygon_remove_spikes<Polygon>
{};

} // namespace dispatch
#endif

namespace resolve_variant {

template <typename Geometry>
struct remove_spikes
{
    static void apply(Geometry& geometry)
    {
        concept::check<Geometry>();
        dispatch::remove_spikes<Geometry>::apply(geometry);
    }
};

template <BOOST_VARIANT_ENUM_PARAMS(typename T)>
struct remove_spikes<boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)> >
{
    struct visitor: boost::static_visitor<void>
    {
        template <typename Geometry>
        void operator()(Geometry& geometry) const
        {
            remove_spikes<Geometry>::apply(geometry);
        }
    };

    static inline void apply(boost::variant<BOOST_VARIANT_ENUM_PARAMS(T)>& geometry)
    {
        boost::apply_visitor(visitor(), geometry);
    }
};

} // namespace resolve_variant

/*!
\ingroup remove_spikes
\tparam Geometry geometry type
\param geometry the geometry to make remove_spikes
*/
template <typename Geometry>
inline void remove_spikes(Geometry& geometry)
{
    resolve_variant::remove_spikes<Geometry>::apply(geometry);
}

}} // namespace boost::geometry
// remove_spikes

namespace openstudio{

  // Private implementation functions

  BoostPolygon removeSpikes(const BoostPolygon& polygon)
  {
    BoostPolygon temp(polygon);
    boost::geometry::remove_spikes(temp);
    return temp;
  }

  std::vector<BoostPolygon> removeSpikes(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon& polygon : polygons){
      result.push_back(removeSpikes(polygon));
    }
    return result;
  }

  std::vector<BoostPolygon> removeHoles(const BoostPolygon& boostPolygon)
  {
    std::vector<BoostPolygon> result;

    // convert to vector of TPPLPoly
    std::list<TPPLPoly> polys;

    BoostRing outer = boostPolygon.outer();
    TPPLPoly outerPoly; // must be counter-clockwise
    outerPoly.Init(outer.size() - 1);
    outerPoly.SetHole(false);
    //std::cout << "outer :";
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      outerPoly[i].x = outer[i].x();
      outerPoly[i].y = outer[i].y();
      //std::cout << "(" << outer[i].x() << ", " << outer[i].y() << ") ";
    }
    //std::cout << std::endl;
    outerPoly.SetOrientation(TPPL_CCW);
    polys.push_back(outerPoly);

    std::vector<BoostRing> inners = boostPolygon.inners();
    for (const BoostRing& inner : inners){
      TPPLPoly innerPoly; // must be clockwise
      innerPoly.Init(inner.size() - 1);
      innerPoly.SetHole(true);
      //std::cout << "inner :";
      for(unsigned i = 0; i < inner.size() - 1; ++i){
        innerPoly[i].x = inner[i].x();
        innerPoly[i].y = inner[i].y();
        //std::cout << "(" << inner[i].x() << ", " << inner[i].y() << ") ";
      }
      //std::cout << std::endl;
      innerPoly.SetOrientation(TPPL_CW);
      polys.push_back(innerPoly);
    }

    // do partitioning
    TPPLPartition pp;
    std::list<TPPLPoly> resultPolys;
    int test = pp.ConvexPartition_HM(&polys,&resultPolys);
    if (test == 0){
      LOG_FREE(Error, "utilities.geometry.removeHoles", "Failed to partition polygon");
      return result;
    }

    // convert back to BoostPolygon
    std::list<TPPLPoly>::iterator it, itend;
    for(it = resultPolys.begin(), itend = resultPolys.end(); it != itend; ++it){
      BoostPolygon newBoostPolygon;
      //std::cout << "result :";
      for (long i = 0; i < it->GetNumPoints(); ++i){
        TPPLPoint point = it->GetPoint(i);
        boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
        //std::cout << "(" << point.x << ", " << point.y << ") ";
      }
      TPPLPoint point = it->GetPoint(0);
      boost::geometry::append(newBoostPolygon, boost::make_tuple(point.x, point.y));
      //std::cout << "(" << point.x << ", " << point.y << ") ";
      //std::cout << std::endl;

      boost::geometry::correct(newBoostPolygon);
      result.push_back(newBoostPolygon);
    }

    return result;
  }

  std::vector<BoostPolygon> removeHoles(const std::vector<BoostPolygon>& polygons)
  {
    std::vector<BoostPolygon> result;
    for (const BoostPolygon polygon : polygons){
      if (polygon.inners().empty()){
        // DLM: might also want to partition if this polygon is self intersecting?
        result.push_back(polygon);
      }else{
        std::vector<BoostPolygon> temp = removeHoles(polygon);
        result.insert(result.end(), temp.begin(), temp.end());
      }
    }
    return result;
  }

  // convert a Point3d to a BoostPoint
  boost::tuple<double, double> boostPointFromPoint3d(const Point3d& point3d, std::vector<Point3d>& allPoints, double tol)
  {
    OS_ASSERT(abs(point3d.z()) <= tol);

    // simple method
    //return boost::make_tuple(point3d.x(), point3d.y());

    // detailed method, try to combine points within tolerance
    Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

    return boost::make_tuple(resultPoint.x(), resultPoint.y());
  }

  // convert vertices to a boost polygon, all vertices must lie on z = 0 plane
  boost::optional<BoostPolygon> boostPolygonFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostPolygon polygon;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostPolygonFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(polygon, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(polygon, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(polygon);

    boost::optional<double> testArea = boost::geometry::area(polygon);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return polygon;
  }

  boost::optional<BoostPolygon> nonIntersectingBoostPolygonFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostPolygon> result = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostPolygonFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert vertices to a boost ring, all vertices must lie on z = 0 plane
  boost::optional<BoostRing> boostRingFromVertices(const std::vector<Point3d>& vertices, std::vector<Point3d>& allPoints, double tol)
  {
    if (vertices.size () < 3){
      return boost::none;
    }

    BoostRing ring;
    for (const Point3d& vertex : vertices){

      // should all have zero z coordinate now
      double z = vertex.z();
      if (abs(z) > tol){
        LOG_FREE(Error, "utilities.geometry.boostRingFromVertices", "All points must be on z = 0 plane");
        return boost::none;
      }

      // use helper method which combines close points
      boost::geometry::append(ring, boostPointFromPoint3d(vertex, allPoints, tol));
    }

    // close polygon, use helper method which combines close points
    boost::geometry::append(ring, boostPointFromPoint3d(vertices[0], allPoints, tol));

    //boost::geometry::correct(ring);

    boost::optional<double> testArea = boost::geometry::area(ring);
    if (!testArea || (*testArea < 0)){
      // DLM: we could offer to reverse these vertices here but that might not be the best idea
      return boost::none;
    }

    return ring;
  }

  boost::optional<BoostRing> nonIntersectingBoostRingFromVertices(const std::vector<Point3d>& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    boost::optional<BoostRing> result = boostRingFromVertices(polygon, allPoints, tol);
    if (!result){
      return boost::none;
    }
    // check if polygon overlaps itself
    try{
      boost::geometry::detail::overlay::has_self_intersections(*result);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      //LOG_FREE(Error, "utilities.geometry.nonIntersectingBoostRingFromVertices", "Self intersecting polygon");
      return boost::none;
    }
    return result;
  }

  // convert a boost polygon to vertices
  std::vector<Point3d> verticesFromBoostPolygon(const BoostPolygon& polygon, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    BoostRing outer = polygon.outer();
    if (outer.empty()){
      return result;
    }

    // add point for each vertex except final vertex
    for(unsigned i = 0; i < outer.size() - 1; ++i){
      Point3d point3d(outer[i].x(), outer[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    OS_ASSERT(polygon.inners().empty());

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // convert a boost ring to vertices
  std::vector<Point3d> verticesFromBoostRing(const BoostRing& ring, std::vector<Point3d>& allPoints, double tol)
  {
    std::vector<Point3d> result;

    // add point for each vertex except final vertex
    for (unsigned i = 0; i < ring.size() - 1; ++i){
      Point3d point3d(ring[i].x(), ring[i].y(), 0.0);

      // try to combine points within tolerance
      Point3d resultPoint = getCombinedPoint(point3d, allPoints, tol);

      // don't keep repeated vertices
      if ((i > 0) && (result.back() == resultPoint)){
        continue;
      }
      result.push_back(resultPoint);
    }

    result = removeCollinear(result);

    // don't keep repeated vertices
    if (result.front() == result.back()){
      result.pop_back();
    }

    if (result.size() < 3){
      return std::vector<Point3d>();
    }

    return result;
  }

  // struct used to sort polygons in descending order by area
  struct BoostPolygonAreaGreater{
    bool operator()(const BoostPolygon& left, const BoostPolygon& right){
      boost::optional<double> leftA = boost::geometry::area(left);
      if (!leftA){
        leftA = 0;
      }
      boost::optional<double> rightA = boost::geometry::area(right);
      if (!rightA){
        rightA = 0;
      }
      return (*leftA > *rightA);
    }
  };

  // Public functions

  IntersectionResult::IntersectionResult(const std::vector<Point3d>& polygon1,
                                         const std::vector<Point3d>& polygon2,
                                         const std::vector< std::vector<Point3d> >& newPolygons1,
                                         const std::vector< std::vector<Point3d> >& newPolygons2)
    : m_polygon1(polygon1), m_polygon2(polygon2), m_newPolygons1(newPolygons1), m_newPolygons2(newPolygons2)
  {
  }

  std::vector<Point3d> IntersectionResult::polygon1() const
  {
    return m_polygon1;
  }

  std::vector<Point3d> IntersectionResult::polygon2() const
  {
    return m_polygon2;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons1() const
  {
    return m_newPolygons1;
  }

  std::vector< std::vector<Point3d> > IntersectionResult::newPolygons2() const
  {
    return m_newPolygons2;
  }

  std::vector<Point3d> removeSpikes(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> boostPolygon = boostPolygonFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return std::vector<Point3d>();
    }

    BoostPolygon boostResult = removeSpikes(*boostPolygon);

    std::vector<Point3d> result = verticesFromBoostPolygon(boostResult, allPoints, tol);

    return result;
  }

  bool pointInPolygon(const Point3d& point, const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon = nonIntersectingBoostRingFromVertices(polygon, allPoints, tol);
    if (!boostPolygon){
      return false;
    }

    if (abs(point.z()) > tol){
      return false;
    }

    boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
    BoostPoint boostPoint(p.get<0>(), p.get<1>());

    //boost::geometry::strategy::within::winding<BoostPoint> strategy;
    //boost::geometry::strategy::within::franklin<BoostPoint> strategy;
    //boost::geometry::strategy::within::crossings_multiply<BoostPoint> strategy;
    //bool result = boost::geometry::within(boostPoint, *boostPolygon, strategy);

    //bool result = boost::geometry::intersects(boostPoint, *boostPolygon);

    //bool result = boost::geometry::overlaps(boostPoint, *boostPolygon);

    double distance = boost::geometry::distance(boostPoint, *boostPolygon);
    bool result = (distance <= 0.0001);

    return result;

  }

  boost::optional<std::vector<Point3d> > join(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // union the points in face coordinates,
    std::vector<BoostPolygon> unionResult;
    try{
      boost::geometry::union_(*boostPolygon1, *boostPolygon2, unionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "overlay_invalid_input_exception");
      return boost::none;
    }

    unionResult = removeSpikes(unionResult);

    // should not be any holes, check for that below

    // check that union is ok
    if (unionResult.empty()){
      return boost::none;
    }else if (unionResult.size() > 1){
      return boost::none;
    }

    std::vector<Point3d> unionVertices = verticesFromBoostPolygon(unionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(unionResult[0]);
    if (!testArea || unionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.join", "Cannot compute area of union");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.join", "Union has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(unionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.join", "Union is self intersecting");
      return boost::none;
    }

    // check for holes
    if (!unionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.join", "Union has inner loops");
      return boost::none;
    };

    unionVertices = reorderULC(unionVertices);
    unionVertices = removeCollinear(unionVertices);

    return unionVertices;
  }

  std::vector<std::vector<Point3d> > joinAll(const std::vector<std::vector<Point3d> >& polygons, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    unsigned N = polygons.size();
    if (N <= 1){
      return polygons;
    }

    // compute adjacency matrix
    Matrix A(N,N,0.0);
    for (unsigned i = 0; i < polygons.size(); ++i){
      A(i,i) = 1.0;
      for (unsigned j = i+1; j < polygons.size(); ++j){
        if (join(polygons[i], polygons[j], tol)){
          A(i,j) = 1.0;
          A(j,i) = 1.0;
        }
      }
    }

    std::vector<std::vector<unsigned> > connectedComponents = findConnectedComponents(A);
    for (const std::vector<unsigned>& component : connectedComponents){
      std::vector<Point3d> points;
      for (unsigned i : component){
        if (points.empty()){
          points = polygons[i];
        }else{
          boost::optional<std::vector<Point3d> > joined = join(points, polygons[i], tol);
          if (!joined){
            LOG_FREE(Error, "utilities.geometry.joinAll", "Expected polygons to join together");
          }else{
            points = *joined;
          }
        }
      }
      result.push_back(points);
    }

    return result;
  }

  boost::optional<IntersectionResult> intersect(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> resultPolygon1;
    std::vector<Point3d> resultPolygon2;
    std::vector< std::vector<Point3d> > newPolygons1;
    std::vector< std::vector<Point3d> > newPolygons2;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostRing> boostPolygon1 = nonIntersectingBoostRingFromVertices(polygon1, allPoints, tol);
    if (!boostPolygon1){
      return boost::none;
    }

    boost::optional<BoostRing> boostPolygon2 = nonIntersectingBoostRingFromVertices(polygon2, allPoints, tol);
    if (!boostPolygon2){
      return boost::none;
    }

    // intersect the points in face coordinates,
    std::vector<BoostPolygon> intersectionResult;
    try{
      boost::geometry::intersection(*boostPolygon1, *boostPolygon2, intersectionResult);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "overlay_invalid_input_exception");
      return boost::none;
    }

    // check if intersection is empty
    if (intersectionResult.empty()){
      //LOG_FREE(Info, "utilities.geometry.intersect", "Intersection is empty");
      return boost::none;
    }

    intersectionResult = removeSpikes(intersectionResult);
    intersectionResult = removeHoles(intersectionResult);

    // check for multiple intersections
    if (intersectionResult.size() > 1){
      LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has " << intersectionResult.size() << " elements");
      std::sort(intersectionResult.begin(), intersectionResult.end(), BoostPolygonAreaGreater());
    }

    // check that largest intersection is ok
    std::vector<Point3d> intersectionVertices = verticesFromBoostPolygon(intersectionResult[0], allPoints, tol);
    boost::optional<double> testArea = boost::geometry::area(intersectionResult[0]);
    if (!testArea || intersectionVertices.empty()){
      LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of largest intersection");
      return boost::none;
    }else if (*testArea < tol*tol){
      LOG_FREE(Info, "utilities.geometry.intersect", "Largest intersection has very small area of " << *testArea << " m^2");
      return boost::none;
    }
    try{
      boost::geometry::detail::overlay::has_self_intersections(intersectionResult[0]);
    }catch(const boost::geometry::overlay_invalid_input_exception&){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection is self intersecting");
      return boost::none;
    }
    if (!intersectionResult[0].inners().empty()){
      LOG_FREE(Error, "utilities.geometry.intersect", "Largest intersection has inner loops");
      return boost::none;
    };

    // intersections are the same
    resultPolygon1 = intersectionVertices;
    resultPolygon2 = intersectionVertices;

    // create new polygon for each remaining intersection
    for (unsigned i = 1; i < intersectionResult.size(); ++i){

      std::vector<Point3d> newPolygon = verticesFromBoostPolygon(intersectionResult[i], allPoints, tol);

      testArea = boost::geometry::area(intersectionResult[i]);
      if (!testArea || newPolygon.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of intersection, result will not include this polygon, " << newPolygon);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Intersection has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(intersectionResult[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection is self intersecting, result will not include this polygon, " << newPolygon);
        continue;
      }
      if (!intersectionResult[i].inners().empty()){
        LOG_FREE(Error, "utilities.geometry.intersect", "Intersection has inner loops, result will not include this polygon, " << newPolygon);
        continue;
      };

      newPolygons1.push_back(newPolygon);
      newPolygons2.push_back(newPolygon);
    }

    // polygon1 minus polygon2
    std::vector<BoostPolygon> differenceResult1;
    boost::geometry::difference(*boostPolygon1, *boostPolygon2, differenceResult1);
    differenceResult1 = removeSpikes(differenceResult1);
    differenceResult1 = removeHoles(differenceResult1);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult1.size(); ++i){

      std::vector<Point3d> newPolygon1 = verticesFromBoostPolygon(differenceResult1[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult1[i]);
      if (!testArea || newPolygon1.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon1);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon1);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult1[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon1);
        continue;
      }

      newPolygons1.push_back(newPolygon1);
    }

    // polygon2 minus polygon1
    std::vector<BoostPolygon> differenceResult2;
    boost::geometry::difference(*boostPolygon2, *boostPolygon1, differenceResult2);
    differenceResult2 = removeSpikes(differenceResult2);
    differenceResult2 = removeHoles(differenceResult2);

    // create new polygon for each difference
    for (unsigned i = 0; i < differenceResult2.size(); ++i){

      std::vector<Point3d> newPolygon2 = verticesFromBoostPolygon(differenceResult2[i], allPoints, tol);

      testArea = boost::geometry::area(differenceResult2[i]);
      if (!testArea || newPolygon2.empty()){
        LOG_FREE(Info, "utilities.geometry.intersect", "Cannot compute area of face difference, result will not include this polygon, " << newPolygon2);
        continue;
      }else if (*testArea < tol*tol){
        LOG_FREE(Info, "utilities.geometry.intersect", "Face difference has very small area of " << *testArea << " m^2, result will not include this polygon, " << newPolygon2);
        continue;
      }
      try{
        boost::geometry::detail::overlay::has_self_intersections(differenceResult2[i]);
      }catch(const boost::geometry::overlay_invalid_input_exception&){
        LOG_FREE(Error, "utilities.geometry.intersect", "Face difference is self intersecting, result will not include this polygon, " << newPolygon2);
        continue;
      }

      newPolygons2.push_back(newPolygon2);
    }

    return IntersectionResult(resultPolygon1, resultPolygon2, newPolygons1, newPolygons2);
  }

  std::vector<std::vector<Point3d> > subtract(const std::vector<Point3d>& polygon, const std::vector<std::vector<Point3d> >& holes, double tol)
  {
    std::vector<std::vector<Point3d> > result;

    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> initialBoostPolygon = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!initialBoostPolygon){
      return result;
    }

    std::vector<BoostPolygon> boostPolygons;
    boostPolygons.push_back(*initialBoostPolygon);

    std::vector<BoostPolygon> newBoostPolygons;
    for (const std::vector<Point3d>& hole : holes){
      boost::optional<BoostPolygon> boostHole = nonIntersectingBoostPolygonFromVertices(hole, allPoints, tol);
      if (!boostHole){
        return result;
      }

      for (const BoostPolygon& boostPolygon : boostPolygons){
        std::vector<BoostPolygon> diffResult;
        boost::geometry::difference(boostPolygon, *boostHole, diffResult);
        diffResult = removeSpikes(diffResult);
        diffResult = removeHoles(diffResult);
        newBoostPolygons.insert(newBoostPolygons.end(), diffResult.begin(), diffResult.end());
      }
      boostPolygons.swap(newBoostPolygons);
    }

    for (const BoostPolygon& boostPolygon : boostPolygons){
      result.push_back(verticesFromBoostPolygon(boostPolygon, allPoints, tol));
    }

    return result;
  }

  bool selfIntersects(const std::vector<Point3d>& polygon, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp = nonIntersectingBoostPolygonFromVertices(polygon, allPoints, tol);
    if (!bp){
      return false;
    }
    return true;
  }

  bool intersects(const std::vector<Point3d>& polygon1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(polygon1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

    if (bp1 && bp2){
      return boost::geometry::intersects(*bp1, *bp2);
    }

    return false;
  }

  bool within(const Point3d& point1, const std::vector<Point3d>& polygon2, double tol)
  {
    std::vector<Point3d> geometry1;
    geometry1.push_back(point1);
    return within(geometry1, polygon2, tol);
  }

  bool within(const std::vector<Point3d>& geometry1, const std::vector<Point3d>& polygon2, double tol)
  {
    // convert vertices to boost rings
    std::vector<Point3d> allPoints;

    if (geometry1.size() == 1){
      if (geometry1[0].z() > tol){
        return false;
      }

      boost::tuple<double, double> p = boostPointFromPoint3d(geometry1[0], allPoints, tol);
      BoostPoint boostPoint(p.get<0>(), p.get<1>());

      boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);

      if (bp2){
        return boost::geometry::within(boostPoint, *bp2);
      }

      return false;
    }

    /*
    // DLM: this is the better implementation, requires boost 1.57
    boost::optional<BoostPolygon> bp1 = boostPolygonFromVertices(geometry1, allPoints, tol);
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp1 && bp2){
      return boost::geometry::within(*bp1, *bp2);
    }
    */

    // DLM: temp code
    if (geometry1.size() < 3){
      return false;
    }
    boost::optional<BoostPolygon> bp2 = boostPolygonFromVertices(polygon2, allPoints, tol);
    if (bp2){
      for (const Point3d& point : geometry1)
      {
        boost::tuple<double, double> p = boostPointFromPoint3d(point, allPoints, tol);
        BoostPoint boostPoint(p.get<0>(), p.get<1>());

        if (!boost::geometry::within(boostPoint, *bp2)){
          return false;
        }
      }
    } else{
      return false;
    }

    return true;
  }

} // openstudio


#ifndef TVMTL_MANIFOLD_HPP
#define TVMTL_MANIFOLD_HPP

//Eigen includes
#include <Eigen/StdVector>

//own includes

#include "enumerators.hpp"

namespace tvmtl {

// Primary Template
template < enum MANIFOLD_TYPE MF, int N, int P=0 >
struct Manifold { };

} // end namespace tvmtl

#include "manifold_euc.hpp"
#include "manifold_sphere.hpp"
#include "manifold_son.hpp"
#include "manifold_spd.hpp"
#include "manifold_grassmann.hpp"

#endif


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

#include <boost/make_shared.hpp>

#if USE_CSPICE
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#endif

#include "Tudat/Astrodynamics/Gravitation/timeDependentSphericalHarmonicsGravityField.h"
#include "Tudat/Astrodynamics/Gravitation/triAxialEllipsoidGravity.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGravityField.h"
#include "Tudat/InputOutput/basicInputOutput.h"

namespace tudat
{

namespace simulation_setup
{

//! Get the path of the SH file for a SH model.
std::string getPathForSphericalHarmonicsModel( const SphericalHarmonicsModel sphericalHarmonicsModel )
{
    switch ( sphericalHarmonicsModel )
    {
    case egm96:
        return input_output::getGravityModelsPath( ) + "Earth/egm96.txt";
    case ggm02c:
        return input_output::getGravityModelsPath( ) + "Earth/ggm02c.txt";
    case ggm02s:
        return input_output::getGravityModelsPath( ) + "Earth/ggm02s.txt";
    case glgm3150:
        return input_output::getGravityModelsPath( ) + "Moon/glgm3150.txt";
    case lpe200:
        return input_output::getGravityModelsPath( ) + "Moon/lpe200.txt";
    case jgmro120d:
        return input_output::getGravityModelsPath( ) + "Mars/jgmro120d.txt";
    default:
        std::cerr << "No path known for Spherical Harmonics Model " << sphericalHarmonicsModel << std::endl;
        throw;
    }
}

//! Get the associated reference frame for a SH model.
std::string getReferenceFrameForSphericalHarmonicsModel( const SphericalHarmonicsModel sphericalHarmonicsModel )
{
    switch ( sphericalHarmonicsModel )
    {
    case egm96:
    case ggm02c:
    case ggm02s:
        return "IAU_Earth";
    case glgm3150:
    case lpe200:
        return "IAU_Moon";
    case jgmro120d:
        return "IAU_Mars";
    default:
        std::cerr << "No reference frame known for Spherical Harmonics Model " << sphericalHarmonicsModel << std::endl;
        throw;
    }
}

//! Constructor with custom model.
FromFileSphericalHarmonicsGravityFieldSettings::FromFileSphericalHarmonicsGravityFieldSettings(
        const std::string& filePath, const std::string& associatedReferenceFrame,
        const int maximumDegree, const int maximumOrder,
        const int gravitationalParameterIndex, const int referenceRadiusIndex,
        const double gravitationalParameter, const double referenceRadius ) :
    SphericalHarmonicsGravityFieldSettings( gravitationalParameter, referenceRadius, Eigen::MatrixXd( ),
                                            Eigen::MatrixXd( ), associatedReferenceFrame ),
    filePath_( filePath ),
    maximumDegree_( maximumDegree ),
    maximumOrder_( maximumOrder ),
    gravitationalParameterIndex_( gravitationalParameterIndex ),
    referenceRadiusIndex_( referenceRadiusIndex )
{
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients;
    std::pair< double, double > referenceData =
            readGravityFieldFile( filePath, maximumDegree, maximumOrder, coefficients,
                                  gravitationalParameterIndex, referenceRadiusIndex );
    gravitationalParameter_ = gravitationalParameterIndex >= 0 ? referenceData.first : gravitationalParameter;
    referenceRadius_ = referenceRadiusIndex >= 0 ? referenceData.second : referenceRadius;
    cosineCoefficients_ = coefficients.first;
    sineCoefficients_ = coefficients.second;
}

//! Constructor with model included in Tudat.
FromFileSphericalHarmonicsGravityFieldSettings::FromFileSphericalHarmonicsGravityFieldSettings(
        const SphericalHarmonicsModel sphericalHarmonicsModel ) :
    FromFileSphericalHarmonicsGravityFieldSettings( getPathForSphericalHarmonicsModel( sphericalHarmonicsModel ),
                                                    getReferenceFrameForSphericalHarmonicsModel( sphericalHarmonicsModel ),
                                                    50, 50, 0, 1 )
{
    sphericalHarmonicsModel_ = sphericalHarmonicsModel;
}

//! Function to read a gravity field file
std::pair< double, double  > readGravityFieldFile(
        const std::string& fileName, const int maximumDegree, const int maximumOrder,
        std::pair< Eigen::MatrixXd, Eigen::MatrixXd >& coefficients,
        const int gravitationalParameterIndex, const int referenceRadiusIndex )
{
    // Attempt to open gravity file.
    std::fstream stream( fileName.c_str( ), std::ios::in );
    if( stream.fail( ) )
    {
        throw std::runtime_error( "Pds gravity field data file could not be opened: " + fileName );
    }

    // Declare variables for reading file.
    std::vector< std::string > vectorOfIndividualStrings;
    vectorOfIndividualStrings.resize( 4 );
    std::string line;

    double gravitationalParameter = TUDAT_NAN;
    double referenceRadius = TUDAT_NAN;

    if( ( gravitationalParameterIndex >= 0 ) &&
            ( referenceRadiusIndex >= 0 ) )
    {
        // Get first line of file.
        std::getline( stream, line );

        // Get reference radius and gravitational parameter from first line of file.
        boost::algorithm::trim( line );
        boost::algorithm::split( vectorOfIndividualStrings,
                                 line,
                                 boost::algorithm::is_any_of( "\t, " ),
                                 boost::algorithm::token_compress_on );
        if( gravitationalParameterIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) ||
                referenceRadiusIndex >= static_cast< int >( vectorOfIndividualStrings.size( ) ) )
        {
            throw std::runtime_error( "Error when reading gravity field file, requested header index exceeds file contents" );
        }

        gravitationalParameter = std::stod( vectorOfIndividualStrings[ gravitationalParameterIndex ] );
        referenceRadius = std::stod( vectorOfIndividualStrings[ referenceRadiusIndex ] );
    }
    else if( ( !( gravitationalParameterIndex >= 0 ) &&
               ( referenceRadiusIndex >= 0 ) ) ||
             ( ( gravitationalParameterIndex >= 0 ) &&
               !( referenceRadiusIndex >= 0 ) ) )
    {
        throw std::runtime_error( "Error when reading gravity field file, must retrieve either both or neither of Re and mu" );
    }

    // Declare variables for reading in cosine and sine coefficients.
    int currentDegree = 0, currentOrder = 0;
    Eigen::MatrixXd cosineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
    cosineCoefficients.setZero( );
    Eigen::MatrixXd sineCoefficients = Eigen::MatrixXd( maximumDegree + 1, maximumOrder + 1 );
    sineCoefficients.setZero( );

    // Read coefficients up to required maximum degree and order.
    while ( !stream.fail( ) && !stream.eof( ) &&
            ( currentDegree <= maximumDegree && currentOrder < maximumOrder )  )
    {
        // Read current line
        std::getline( stream, line );

        // Trim input string (removes all leading and trailing whitespaces).
        boost::algorithm::trim( line );

        // Split string into multiple strings, each containing one element from a line from the
        // data file.
        boost::algorithm::split( vectorOfIndividualStrings,
                                 line,
                                 boost::algorithm::is_any_of( ", " ),
                                 boost::algorithm::token_compress_on );

        // Check current line for consistency
        if( vectorOfIndividualStrings.size( ) != 0 )
        {
            if( vectorOfIndividualStrings.size( ) < 4 )
            {
                std::string errorMessage = "Error when reading pds gravity field file, number of fields is " +
                        std::to_string( vectorOfIndividualStrings.size( ) );
                throw std::runtime_error( errorMessage );
            }
            else
            {
                // Read current degree and orde from line.
                currentDegree = std::stoi( vectorOfIndividualStrings[ 0 ] );
                currentOrder = std::stoi( vectorOfIndividualStrings[ 1 ] );

                // Set cosine and sine coefficients for current degree and order.
                if( currentDegree <= maximumDegree && currentOrder <= maximumOrder )
                {
                    cosineCoefficients( currentDegree, currentOrder ) =
                            std::stod( vectorOfIndividualStrings[ 2 ] );
                    sineCoefficients( currentDegree, currentOrder ) =
                            std::stod( vectorOfIndividualStrings[ 3 ] );
                }
            }
        }
    }

    // Set cosine coefficient at (0,0) to 1.
    cosineCoefficients( 0, 0 ) = 1.0;
    coefficients = std::make_pair( cosineCoefficients, sineCoefficients );

    return std::make_pair( gravitationalParameter, referenceRadius );
}

//! Function to create a gravity field model.
std::shared_ptr< gravitation::GravityFieldModel > createGravityFieldModel(
        const std::shared_ptr< GravityFieldSettings > gravityFieldSettings,
        const std::string& body,
        const NamedBodyMap& bodyMap,
        const std::vector< std::shared_ptr< GravityFieldVariationSettings > >& gravityFieldVariationSettings )
{
    using namespace tudat::gravitation;

    // Declare return object.
    std::shared_ptr< GravityFieldModel > gravityFieldModel;

    // Check which type of gravity field model is to be created.
    switch( gravityFieldSettings->getGravityFieldType( ) )
    {
    case central:
    {
        // Check whether settings for point mass gravity field model are consistent with its type.
        std::shared_ptr< CentralGravityFieldSettings > centralFieldSettings =
                std::dynamic_pointer_cast< CentralGravityFieldSettings >( gravityFieldSettings );
        if( centralFieldSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected central field settings when making gravity field model for body " +
                        body);
        }
        else if( gravityFieldVariationSettings.size( ) != 0 )
        {
            throw std::runtime_error( "Error, requested central gravity field, but field variations settings are not empty." );
        }
        else
        {
            // Create and initialize point mass gravity field model.
            gravityFieldModel = std::make_shared< GravityFieldModel >(
                        centralFieldSettings->getGravitationalParameter( ) );
        }
        break;
    }
#if USE_CSPICE
    case central_spice:
    {
        if( gravityFieldVariationSettings.size( ) != 0 )
        {
            throw std::runtime_error( "Error, requested central gravity field, but field variations settings are not empty." );
        }
        else
        {
            // Create and initialize point mass gravity field model from Spice.
            gravityFieldModel = std::make_shared< GravityFieldModel >(
                        spice_interface::getBodyGravitationalParameter( body ) );
        }

        break;
    }
#endif
    case spherical_harmonic:
    {
        // Check whether settings for spherical harmonic gravity field model are consistent with
        // its type.
        std::shared_ptr< SphericalHarmonicsGravityFieldSettings > sphericalHarmonicFieldSettings =
                std::dynamic_pointer_cast< SphericalHarmonicsGravityFieldSettings >(
                    gravityFieldSettings );

        if( sphericalHarmonicFieldSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected spherical harmonic field settings when making gravity field model of "
                        + body );
        }
        else
        {
            std::function< void( ) > inertiaTensorUpdateFunction;
            if( bodyMap.count( body ) == 0 )
            {
                inertiaTensorUpdateFunction = std::function< void( ) >( );
            }
            else
            {
                inertiaTensorUpdateFunction =
                    std::bind( &Body::setBodyInertiaTensorFromGravityFieldAndExistingMeanMoment, bodyMap.at( body ), true );
            }

            // Check consistency of cosine and sine coefficients.
            if( ( sphericalHarmonicFieldSettings->getCosineCoefficients( ).rows( ) !=
                  sphericalHarmonicFieldSettings->getSineCoefficients( ).rows( ) ) ||
                    ( sphericalHarmonicFieldSettings->getCosineCoefficients( ).cols( ) !=
                      sphericalHarmonicFieldSettings->getSineCoefficients( ).cols( ) ) )
            {
                throw std::runtime_error(
                            std::string( "Error when making spherical harmonic field, sine and " ) +
                            std::string( "cosine matrix  sizes are not equal for body " ) + body );
            }
            else
            {

                if( gravityFieldVariationSettings.size( ) == 0 &&
                        sphericalHarmonicFieldSettings->getCreateTimeDependentField( ) == 0 )
                {
                    // Create and initialize spherical harmonic gravity field model.
                    gravityFieldModel = std::make_shared< SphericalHarmonicsGravityField >(
                                sphericalHarmonicFieldSettings->getGravitationalParameter( ),
                                sphericalHarmonicFieldSettings->getReferenceRadius( ),
                                sphericalHarmonicFieldSettings->getCosineCoefficients( ),
                                sphericalHarmonicFieldSettings->getSineCoefficients( ),
                                sphericalHarmonicFieldSettings->getAssociatedReferenceFrame( ),
                                inertiaTensorUpdateFunction );
                }
                else
                {
                    if( bodyMap.at( body )->getGravityFieldModel( ) != nullptr )
                    {
                        std::string errorMessage = "Warning when making time-dependent gravity field model for body " + body +
                                " existing gravity field is not empty but overwritten in Body! ";
                        throw std::runtime_error( errorMessage );
                    }

                    // Create preliminary TimeDependentSphericalHarmonicsGravityField, without actual variation settings.
                    gravityFieldModel = std::make_shared< TimeDependentSphericalHarmonicsGravityField >(
                                sphericalHarmonicFieldSettings->getGravitationalParameter( ),
                                sphericalHarmonicFieldSettings->getReferenceRadius( ),
                                sphericalHarmonicFieldSettings->getCosineCoefficients( ),
                                sphericalHarmonicFieldSettings->getSineCoefficients( ),
                                sphericalHarmonicFieldSettings->getAssociatedReferenceFrame( ),
                                inertiaTensorUpdateFunction );
                }

            }
        }
        break;
    }
    default:
        throw std::runtime_error(
                    "Error, did not recognize gravity field model settings type " +
                    std::to_string(
                        gravityFieldSettings->getGravityFieldType( ) ) );
    }

    return gravityFieldModel;
}

//! Function to create gravity field settings for a homogeneous triaxial ellipsoid
std::shared_ptr< SphericalHarmonicsGravityFieldSettings > createHomogeneousTriAxialEllipsoidGravitySettings(
        const double axisA, const double axisB, const double axisC, const double ellipsoidDensity,
        const int maximumDegree, const int maximumOrder,
        const std::string& associatedReferenceFrame  )
{
    // Compute reference quantities
    double ellipsoidGravitationalParameter = gravitation::calculateTriAxialEllipsoidVolume(
                axisA, axisB, axisC ) * ellipsoidDensity * physical_constants::GRAVITATIONAL_CONSTANT;
    double ellipsoidReferenceRadius = gravitation::calculateTriAxialEllipsoidVolume(
                axisA, axisB, axisC );

    // Compute coefficients
    std::pair< Eigen::MatrixXd, Eigen::MatrixXd > coefficients =
            gravitation::createTriAxialEllipsoidNormalizedSphericalHarmonicCoefficients(
                axisA, axisB, axisC, maximumDegree, maximumOrder );

    return std::make_shared< SphericalHarmonicsGravityFieldSettings >(
                ellipsoidGravitationalParameter, ellipsoidReferenceRadius, coefficients.first,
                coefficients.second, associatedReferenceFrame );
}

} // namespace simulation_setup

} // namespace tudat


//==============================================================================
//         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
//==============================================================================
#include <boost/simd/bitwise/include/functions/bitwise_notand.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/constant/constant.hpp>

NT2_TEST_CASE_TPL ( bitwise_notand_real,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::bitwise_notand;
  using boost::simd::tag::bitwise_notand_;
  typedef typename boost::dispatch::meta::call<bitwise_notand_(T,T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Inf<T>(), boost::simd::Inf<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Minf<T>(), boost::simd::Minf<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Nan<T>(), boost::simd::Nan<T>()), boost::simd::Zero<r_t>());
#endif
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<T>(), boost::simd::Zero<T>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<T>(),boost::simd::One<T>()), boost::simd::One<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL(bitwise_notand_mix, BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::bitwise_notand;
  using boost::simd::tag::bitwise_notand_;
  using boost::simd::native;
  typedef typename  boost::dispatch::meta::as_integer<T>::type siT;
  typedef typename  boost::dispatch::meta::as_integer<T, unsigned>::type uiT;

  // return type conformity test
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notand_(T,uiT)>::type, T);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notand_(T,siT)>::type, T);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notand_(uiT, T)>::type, uiT);
  NT2_TEST_TYPE_IS(typename boost::dispatch::meta::call<bitwise_notand_(siT, T)>::type, siT);

  // specific values tests
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<T>(),boost::simd::Valmax<uiT>()), boost::simd::Nan<T>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<T>(), boost::simd::Mone<siT>()), boost::simd::Nan<T>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<siT>(),boost::simd::Nan<T>()), boost::simd::Mone<siT>());
  NT2_TEST_EQUAL(bitwise_notand(boost::simd::Zero<uiT>(), boost::simd::Nan<T>()), boost::simd::Valmax<uiT>());
}


#include <cmath>
#include <vector>
#include <algorithm>
#include <boost/scoped_ptr.hpp>
#include "gtest/gtest.h"
#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/filler.hpp"
#include "caffe/layers/ordinal_regression_loss_layer.hpp"
#include "caffe/test/test_caffe_main.hpp"
#include "caffe/test/test_gradient_check_util.hpp"

using boost::scoped_ptr;

namespace caffe {

    template<typename TypeParam>
    class OrdinalRegressionLossLayerTest : public MultiDeviceTest<TypeParam> {
        typedef typename TypeParam::Dtype Dtype;

    protected:
        OrdinalRegressionLossLayerTest()
            : blob_bottom_data_(new Blob<Dtype>(100, 200, 1, 1)),
            blob_bottom_label_(new Blob<Dtype>(100, 1, 1, 1)),
            blob_top_loss_(new Blob<Dtype>()) {
            FillerParameter filler_param;
            filler_param.set_std(10);
            GaussianFiller<Dtype> filler(filler_param);
            filler.Fill(this->blob_bottom_data_);
            for (int i = 0; i < blob_bottom_label_->count(); i++) {
                blob_bottom_label_->mutable_cpu_data()[i] = caffe_rng_rand() % 100;
            }
            blob_bottom_vec_.push_back(blob_bottom_data_);
            blob_bottom_vec_.push_back(blob_bottom_label_);
            blob_top_vec_.push_back(blob_top_loss_);
        }

        virtual ~OrdinalRegressionLossLayerTest() {
            delete blob_bottom_data_;
            delete blob_bottom_label_;
            delete blob_top_loss_;
        }

        Blob<Dtype>* const blob_bottom_data_;
        Blob<Dtype>* const blob_bottom_label_;
        Blob<Dtype>* const blob_top_loss_;
        vector<Blob<Dtype>*> blob_bottom_vec_;
        vector<Blob<Dtype>*> blob_top_vec_;
    };

    TYPED_TEST_CASE(OrdinalRegressionLossLayerTest, TestDtypesAndDevices);

    // TYPED_TEST(OrdinalRegressionLossLayerTest, TestGradient) {
    //   typedef typename TypeParam::Dtype Dtype;
    //   LayerParameter layer_param;
    //   OrdinalRegressionLossLayer<Dtype> layer(layer_param);
    //   GradientChecker<Dtype> checker(1e-2, 1e-2, 1701);
    //   checker.CheckGradientExhaustive(&layer, this->blob_bottom_vec_,
    //         this->blob_top_vec_, 0);
    // }

    TYPED_TEST(OrdinalRegressionLossLayerTest, TestForward) {
        typedef typename TypeParam::Dtype Dtype;
        LayerParameter layer_param;
        scoped_ptr<OrdinalRegressionLossLayer<Dtype> > layer(
            new OrdinalRegressionLossLayer<Dtype>(layer_param));
        layer->SetUp(this->blob_bottom_vec_, this->blob_top_vec_);
        layer->Forward(this->blob_bottom_vec_, this->blob_top_vec_);
        Dtype loss = this->blob_top_loss_->cpu_data()[0];
    }

    TYPED_TEST(OrdinalRegressionLossLayerTest, TestBackward) {
        typedef typename TypeParam::Dtype Dtype;
        LayerParameter layer_param;
        scoped_ptr<OrdinalRegressionLossLayer<Dtype> > layer(
            new OrdinalRegressionLossLayer<Dtype>(layer_param));
        layer->SetUp(this->blob_bottom_vec_, this->blob_top_vec_);
        layer->Forward(this->blob_bottom_vec_, this->blob_top_vec_);
        vector<bool> propagate_down;
        propagate_down.push_back(true);
        propagate_down.push_back(false);
        layer->Backward(this->blob_top_vec_, propagate_down, this->blob_bottom_vec_);
    }

}  // namespace caffe


//==================================================================================================
/*!
  @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_SIGNBIT_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIGNBIT_HPP_INCLUDED

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

 /*!
  @ingroup group-arithmetic
    Function object implementing signbit capabilities

    True or false according to the bit of sign set or not set.

    This is a convenience alias of @ref is_negative used by stdlibc++.
  **/
  as_logical_t<Value> signbit(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/is_negative.hpp>
#include <boost/simd/function/simd/signbit.hpp>

#endif


/* test_real_distribution.ipp
 *
 * Copyright Steven Watanabe 2011
 * 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)
 *
 * $Id$
 *
 */

#ifndef BOOST_MATH_DISTRIBUTION_INIT
#ifdef BOOST_RANDOM_ARG2_TYPE
#define BOOST_MATH_DISTRIBUTION_INIT (BOOST_RANDOM_ARG1_NAME, BOOST_RANDOM_ARG2_NAME)
#else
#define BOOST_MATH_DISTRIBUTION_INIT (BOOST_RANDOM_ARG1_NAME)
#endif
#endif

#ifndef BOOST_RANDOM_DISTRIBUTION_INIT
#ifdef BOOST_RANDOM_ARG2_TYPE
#define BOOST_RANDOM_DISTRIBUTION_INIT (BOOST_RANDOM_ARG1_NAME, BOOST_RANDOM_ARG2_NAME)
#else
#define BOOST_RANDOM_DISTRIBUTION_INIT (BOOST_RANDOM_ARG1_NAME)
#endif
#endif

#ifndef BOOST_RANDOM_P_CUTOFF
#define BOOST_RANDOM_P_CUTOFF 0.99
#endif

#include <boost/random/mersenne_twister.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/exception/diagnostic_information.hpp>
#include <boost/preprocessor/stringize.hpp>
#include <boost/range/numeric.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <iostream>
#include <vector>

#include "statistic_tests.hpp"
#include "chi_squared_test.hpp"

bool do_test(BOOST_RANDOM_ARG1_TYPE BOOST_RANDOM_ARG1_NAME,
#ifdef BOOST_RANDOM_ARG2_TYPE
             BOOST_RANDOM_ARG2_TYPE BOOST_RANDOM_ARG2_NAME,
#endif
             long long max, boost::mt19937& gen) {
    std::cout << "running " BOOST_PP_STRINGIZE(BOOST_RANDOM_DISTRIBUTION_NAME) "("
        << BOOST_RANDOM_ARG1_NAME;
#ifdef BOOST_RANDOM_ARG2_NAME
    std::cout << ", " << BOOST_RANDOM_ARG2_NAME;
#endif
    std::cout << ")" << " " << max << " times: " << std::flush;

    BOOST_MATH_DISTRIBUTION expected BOOST_MATH_DISTRIBUTION_INIT;

    BOOST_RANDOM_DISTRIBUTION dist BOOST_RANDOM_DISTRIBUTION_INIT;

#ifdef BOOST_RANDOM_DISTRIBUTION_MAX

    BOOST_RANDOM_DISTRIBUTION::result_type max_value = BOOST_RANDOM_DISTRIBUTION_MAX;

    std::vector<double> expected_pdf(max_value+1);
    {
        for(int i = 0; i <= max_value; ++i) {
            expected_pdf[i] = pdf(expected, i);
        }
        expected_pdf.back() += 1 - boost::accumulate(expected_pdf, 0.0);
    }

    std::vector<long long> results(max_value + 1);
    for(long long i = 0; i < max; ++i) {
        ++results[(std::min)(dist(gen), max_value)];
    }

    long long sum = boost::accumulate(results, 0ll);
    if(sum != max) {
        std::cout << "*** Failed: incorrect total: " << sum << " ***" << std::endl;
        return false;
    }
    double prob = chi_squared_test(results, expected_pdf, max);

#else

    kolmogorov_experiment test(boost::numeric_cast<int>(max));
    boost::variate_generator<boost::mt19937&, BOOST_RANDOM_DISTRIBUTION > vgen(gen, dist);

    double prob = test.probability(test.run(vgen, expected));

#endif

    bool result = prob < BOOST_RANDOM_P_CUTOFF;
    const char* err = result? "" : "*";
    std::cout << std::setprecision(17) << prob << err << std::endl;

    std::cout << std::setprecision(6);

    return result;
}

template<class Dist1
#ifdef BOOST_RANDOM_ARG2_NAME
       , class Dist2
#endif
>
bool do_tests(int repeat, Dist1 d1,
#ifdef BOOST_RANDOM_ARG2_NAME
              Dist2 d2,
#endif
              long long trials) {
    boost::mt19937 gen;
    int errors = 0;
    for(int i = 0; i < repeat; ++i) {
        if(!do_test(d1(gen),
#ifdef BOOST_RANDOM_ARG2_NAME
            d2(gen),
#endif
            trials, gen)) {
            ++errors;
        }
    }
    if(errors != 0) {
        std::cout << "*** " << errors << " errors detected ***" << std::endl;
    }
    return errors == 0;
}

int usage() {
    std::cerr << "Usage: test_" BOOST_PP_STRINGIZE(BOOST_RANDOM_DISTRIBUTION_NAME)
        " -r <repeat>"
        " -" BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG1_NAME)
        " <max " BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG1_NAME) ">"
#ifdef BOOST_RANDOM_ARG2_NAME
        " -" BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG2_NAME)
        " <max " BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG2_NAME) ">"
#endif
        " -t <trials>" << std::endl;
    return 2;
}

template<class T>
bool handle_option(int& argc, char**& argv, const char* opt, T& value) {
    if(std::strcmp(argv[0], opt) == 0 && argc > 1) {
        --argc;
        ++argv;
        value = boost::lexical_cast<T>(argv[0]);
        return true;
    } else {
        return false;
    }
}

int main(int argc, char** argv) {
    int repeat = 1;
    BOOST_RANDOM_ARG1_TYPE max_arg1 = BOOST_RANDOM_ARG1_DEFAULT;
#ifdef BOOST_RANDOM_ARG2_TYPE
    BOOST_RANDOM_ARG2_TYPE max_arg2 = BOOST_RANDOM_ARG2_DEFAULT;
#endif
    long long trials = 100000;

    if(argc > 0) {
        --argc;
        ++argv;
    }
    while(argc > 0) {
        if(argv[0][0] != '-') return usage();
        else if(!handle_option(argc, argv, "-r", repeat)
             && !handle_option(argc, argv, "-" BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG1_NAME), max_arg1)
#ifdef BOOST_RANDOM_ARG2_TYPE
             && !handle_option(argc, argv, "-" BOOST_PP_STRINGIZE(BOOST_RANDOM_ARG2_NAME), max_arg2)
#endif
             && !handle_option(argc, argv, "-t", trials)) {
            return usage();
        }
        --argc;
        ++argv;
    }

    try {
        if(do_tests(repeat,
                    BOOST_RANDOM_ARG1_DISTRIBUTION(max_arg1),
#ifdef BOOST_RANDOM_ARG2_TYPE
                    BOOST_RANDOM_ARG2_DISTRIBUTION(max_arg2),
#endif
                    trials)) {
            return 0;
        } else {
            return EXIT_FAILURE;
        }
    } catch(...) {
        std::cerr << boost::current_exception_diagnostic_information() << std::endl;
        return EXIT_FAILURE;
    }
}


#include <cmath>
#include <cstdlib>
#include <ctime>
#include <iostream>

#include <boost/test/unit_test.hpp>

#include <am/succinct/succinct_vector.hpp>

using namespace std;
BOOST_AUTO_TEST_SUITE( t_succinct_vector_suite )

BOOST_AUTO_TEST_CASE(Insert)
{
    izenelib::am::succinct::vector<double> foo;
    const unsigned limit = 100;
    for(unsigned i = 0; i < limit; ++i) {
        foo.push_back(i);
        BOOST_CHECK (foo.size() == i+1);
        for(unsigned j = 0; j <= sqrt(i); ++j) {
            const double k = rand() % foo.size();
            BOOST_CHECK (k == foo[k]);
            foo[k] = static_cast<double>(k);
        }
        const unsigned little = min(static_cast<unsigned>(sqrt(limit)),i);
        for(unsigned j = 0; j <= little; ++j) {
            foo.pop_back();
        }
        for(unsigned j = i - little; j <= i; ++j) {
            foo.push_back(j);
        }
    }
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include "Boost_Float.hxx"
#include "sdp_convert/Dual_Constraint_Group.hxx"
#include "sdp_convert/write_vector.hxx"

#include <boost/filesystem.hpp>

#include <vector>

void write_sdpb_input_files(
  const boost::filesystem::path &output_path, const int &rank,
  const size_t &num_blocks, const std::vector<std::string> &command_arguments,
  const El::BigFloat &objective_const,
  const std::vector<El::BigFloat> &dual_objective_b,
  const std::vector<Dual_Constraint_Group> &dual_constraint_groups);


#include <boost/multiprecision/cpp_bin_float/transcendental.hpp>


#include "src/utils/eigen_utils.h"

#include <gtest/gtest.h>

#include <Eigen/Dense>
#include <vector>

TEST(vstack, 1) {
  std::vector<Eigen::MatrixXd> mats(3);
  mats[0] = Eigen::MatrixXd::Ones(3, 2);
  mats[1] = Eigen::MatrixXd::Zero(1, 2);
  mats[2] = Eigen::MatrixXd(1, 2);
  mats[2] << 1, 2;

  EXPECT_NO_THROW(bayesmix::vstack(mats));
  Eigen::MatrixXd out = bayesmix::vstack(mats);

  ASSERT_EQ(out(0, 0), mats[0](0, 0));
  ASSERT_EQ(out(3, 0), mats[1](0, 0));
  ASSERT_EQ(out(4, 0), mats[2](0, 0));
  ASSERT_EQ(out(4, 1), mats[2](0, 1));

  mats[2] = Eigen::MatrixXd::Zero(1, 1);
  EXPECT_THROW(bayesmix::vstack(mats), std::invalid_argument);
}

TEST(append_by_row, inplace) {
  Eigen::MatrixXd a = Eigen::MatrixXd::Ones(3, 2);
  Eigen::MatrixXd b = Eigen::MatrixXd::Zero(1, 2);
  int init_a_rows = a.rows();

  EXPECT_NO_THROW(bayesmix::append_by_row(&a, b));
  ASSERT_EQ(a.rows(), init_a_rows + b.rows());
  ASSERT_EQ(a.cols(), 2);
  ASSERT_EQ(a(init_a_rows, 0), b(0, 0));
  ASSERT_EQ(a(init_a_rows, 1), b(0, 1));

  Eigen::MatrixXd a2 = Eigen::MatrixXd::Ones(0, 0);
  EXPECT_NO_THROW(bayesmix::append_by_row(&a2, b));
  ASSERT_EQ(a2.rows(), b.rows());
  ASSERT_EQ(a2(0, 0), b(0, 0));

  init_a_rows = a.rows();
  Eigen::MatrixXd b2 = Eigen::MatrixXd::Ones(0, 0);
  EXPECT_NO_THROW(bayesmix::append_by_row(&a, b2));
  ASSERT_EQ(a.rows(), init_a_rows);

  Eigen::MatrixXd b3 = Eigen::MatrixXd::Zero(1, 1);
  EXPECT_THROW(bayesmix::append_by_row(&a, b3), std::invalid_argument);
}

TEST(append_by_row, copy) {
  Eigen::MatrixXd a = Eigen::MatrixXd::Ones(3, 2);
  Eigen::MatrixXd b = Eigen::MatrixXd::Zero(1, 2);
  int init_a_rows = a.rows();

  EXPECT_NO_THROW(bayesmix::append_by_row(a, b));
  Eigen::MatrixXd anew = bayesmix::append_by_row(a, b);
  ASSERT_GT(anew.rows(), init_a_rows);
  ASSERT_EQ(anew.cols(), 2);
  ASSERT_EQ(anew(init_a_rows, 0), b(0, 0));
  ASSERT_EQ(anew(init_a_rows, 1), b(0, 1));

  Eigen::MatrixXd a2 = Eigen::MatrixXd::Ones(0, 0);
  EXPECT_NO_THROW(bayesmix::append_by_row(a2, b));
  Eigen::MatrixXd a2new = bayesmix::append_by_row(a2, b);
  ASSERT_EQ(a2new.rows(), b.rows());
  ASSERT_EQ(a2new(0, 0), b(0, 0));

  init_a_rows = a.rows();
  Eigen::MatrixXd b2 = Eigen::MatrixXd::Ones(0, 0);
  EXPECT_NO_THROW(bayesmix::append_by_row(a, b2));
  ASSERT_EQ(a.rows(), init_a_rows);

  Eigen::MatrixXd b3 = Eigen::MatrixXd::Zero(1, 1);
  EXPECT_THROW(bayesmix::append_by_row(a, b3), std::invalid_argument);
}


// Distributed under the MIT License.
// See LICENSE.txt for details.

#pragma once

#include <boost/optional.hpp>
#include <tuple>

#include "DataStructures/DataBox/DataBox.hpp"
#include "DataStructures/DataBox/Prefixes.hpp"
#include "Domain/TagsTimeDependent.hpp"
#include "NumericalAlgorithms/LinearOperators/PartialDerivatives.hpp"
#include "Utilities/Gsl.hpp"
#include "Utilities/Requires.hpp"
#include "Utilities/TMPL.hpp"
#include "Utilities/TaggedTuple.hpp"

/// \cond
namespace Frame {
struct Inertial;
}  // namespace Frame
namespace Parallel {
template <typename Metavariables>
class GlobalCache;
}  // namespace Parallel
// IWYU pragma: no_forward_declare db::DataBox
/// \endcond

namespace evolution {
namespace Actions {
/*!
 * \ingroup ActionsGroup
 * \brief Compute and add the advective term for nonconservative systems on
 * moving meshes
 *
 * Adds the following term to the time derivative:
 *
 * \f{align}{
 *  v^i_g \partial_i u_\alpha,
 * \f}
 *
 * where \f$u_\alpha\f$ are the evolved variables and \f$v^i_g\f$ is the
 * velocity of the mesh.
 *
 * \note The term is always added in the `Frame::Inertial` frame, and the plus
 * sign arises because we add it to the time derivative.
 *
 * Uses:
 * - DataBox:
 *   - `Tags::deriv<system::variables_tags, Dim, Frame::Inertial>`
 *   - `domain::Tags::MeshVelocity<Dim>`
 *
 * DataBox changes:
 * - Adds: nothing
 * - Removes: nothing
 * - Modifies: `Tags::dt<system::variable_tags>`
 */
struct AddMeshVelocityNonconservative {
  template <typename DbTagsList, typename... InboxTags, typename Metavariables,
            typename ArrayIndex, typename ActionList,
            typename ParallelComponent>
  static std::tuple<db::DataBox<DbTagsList>&&> apply(
      db::DataBox<DbTagsList>& box,
      const tuples::TaggedTuple<InboxTags...>& /*inboxes*/,
      const Parallel::GlobalCache<Metavariables>& /*cache*/,
      const ArrayIndex& /*array_index*/, ActionList /*meta*/,
      const ParallelComponent* const /*meta*/) noexcept {  // NOLINT const
    using variables_tags =
        typename Metavariables::system::variables_tag::tags_list;
    const auto& mesh_velocity =
        db::get<::domain::Tags::MeshVelocity<Metavariables::volume_dim>>(box);

    if (static_cast<bool>(mesh_velocity)) {
      tmpl::for_each<variables_tags>([&box, &mesh_velocity](
                                         auto variables_tag_v) noexcept {
        using variable_tag = typename decltype(variables_tag_v)::type;
        using dt_variable_tag = ::Tags::dt<variable_tag>;
        using deriv_tag =
            ::Tags::deriv<variable_tag, tmpl::size_t<Metavariables::volume_dim>,
                          Frame::Inertial>;

        db::mutate<dt_variable_tag>(
            make_not_null(&box),
            [](const auto dt_var_ptr, const auto& deriv_tensor,
               const boost::optional<tnsr::I<
                   DataVector, Metavariables::volume_dim, Frame::Inertial>>&
                   grid_velocity) noexcept {
              for (size_t storage_index = 0;
                   storage_index < deriv_tensor.size(); ++storage_index) {
                // We grab the `deriv_tensor_index`, which would be e.g.
                // `(i, a, b)`, so `(0, 2, 3)`
                const auto deriv_tensor_index =
                    deriv_tensor.get_tensor_index(storage_index);
                // Then we drop the derivative index (the first entry) to get
                // `(a, b)` (or `(2, 3)`)
                const auto tensor_index =
                    all_but_specified_element_of(deriv_tensor_index, 0);
                // Set `deriv_index` to `i` (or `0` in the example)
                const size_t deriv_index = gsl::at(deriv_tensor_index, 0);
                dt_var_ptr->get(tensor_index) +=
                    grid_velocity->get(deriv_index) *
                    deriv_tensor[storage_index];
              }
            },
            db::get<deriv_tag>(box), mesh_velocity);
      });
    }
    return std::forward_as_tuple(std::move(box));
  }
};
}  // namespace Actions
}  // namespace evolution


/*
    Copyright (C) 2008 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

/**
 *  \file fitdata.cpp
 *  \brief Gather statistics from dataset for MPLSH tuning.
 *
 *  This program gahters statistical data from a small sample dataset
 *  for automatic MPLSH parameter tuning.  It carries out the following
 *  steps:
 *  -# Sample N points from the dataset. Only those N points will be used for future computation.
 *  -# Sample P pairs of points from the sample, calculate the distance for each pair.
 *  -# Sample Q points from the sample as queries points.
 *  -# Divide the sample into F folds.
 *  -# For i = 1 to F, take i folds and run K-NN search, so the query points
 *     will be searched against sample datasets of N/F, 2N/F, ..., N/F points.
 *
 *  The statistical data is printed to standard output after the progress display.
 *
\verbatim
Allowed options:
  -h [ --help ]          produce help message.
  -N [ -- ] arg (=0)     number of points to use
  -P [ -- ] arg (=50000) number of pairs to sample
  -Q [ -- ] arg (=1000)  number of queries to sample
  -K [ -- ] arg (=100)   search for K nearest neighbors
  -F [ -- ] arg (=10)    divide the sample to F folds
  -D [ --data ] arg      data file
\endverbatim
 */

#include <cstdlib>
#include <gsl/gsl_multifit.h>
#include <boost/program_options.hpp>
#include <boost/progress.hpp>
#include <lshkit.h>

using namespace std;
using namespace lshkit;
using namespace tr1;
namespace po = boost::program_options;

bool is_good_value (double v) {
    return ((v > -std::numeric_limits<double>::max())
                    && (v < std::numeric_limits<double>::max()));
}

int main (int argc, char *argv[])
{
    string data_file;
    unsigned N, P, Q, K, F;
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message.")
        (",N", po::value<unsigned>(&N)->default_value(0), "number of points to use")
        (",P", po::value<unsigned>(&P)->default_value(50000), "number of pairs to sample")
        (",Q", po::value<unsigned>(&Q)->default_value(1000), "number of queries to sample")
        (",K", po::value<unsigned>(&K)->default_value(100), "search for K nearest neighbors")
        (",F", po::value<unsigned>(&F)->default_value(10), "divide the sample to F folds")
        ("data,D", po::value<string>(&data_file), "data file")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") || (vm.count("data") < 1))
    {
        cout << desc;
        return 0;
    }

    /* load matrix */
    Matrix<float> data(data_file);

    vector<unsigned> idx(data.getSize());
    for (unsigned i = 0; i < idx.size(); ++i) idx[i] = i;
    random_shuffle(idx.begin(), idx.end());

    if (N > 0 && N < data.getSize()) idx.resize(N);

    metric::l2sqr<float> l2sqr(data.getDim());

    DefaultRng rng;
    boost::variate_generator<DefaultRng &, UniformUnsigned> gen(rng,
             UniformUnsigned(0, idx.size()-1));

    double gM = 0.0;
    double gG = 0.0;
    {
        // sample P pairs of points
        for (unsigned k = 0; k < P; ++k)
        {
            double dist, logdist;
            for (;;)
            {
                unsigned i = gen();
                unsigned j = gen();
                if (i == j) continue;
                dist = l2sqr(data[idx[i]], data[idx[j]]);
                logdist = log(dist);
                if (is_good_value(logdist)) break;
            }
            gM += dist;
            gG += logdist;
        }
        gM /= P;
        gG /= P;
        gG = exp(gG);
    }

    if (Q > idx.size()) Q = idx.size();
    if (K > idx.size() - Q) K = idx.size() - Q;
    /* sample query */
    vector<unsigned> qry(Q);

    SampleQueries(&qry, idx.size(), rng);

    /* do the queries */
    vector<Topk<unsigned> > topks(Q);
    for (unsigned i = 0; i < Q; ++i) topks[i].reset(K);

    /* ... */
    gsl_matrix *X = gsl_matrix_alloc(F * K, 3);
    gsl_vector *yM = gsl_vector_alloc(F * K);
    gsl_vector *yG = gsl_vector_alloc(F * K);
    gsl_vector *pM = gsl_vector_alloc(3);
    gsl_vector *pG = gsl_vector_alloc(3);
    gsl_matrix *cov = gsl_matrix_alloc(3,3);

    vector<double> M(K);
    vector<double> G(K);

    boost::progress_display progress(F, cerr);
    unsigned m = 0;
    for (unsigned l = 0; l < F; l++)
    {
        // Scan
        for (unsigned i = l; i< idx.size(); i += F)
        {
            for (unsigned j = 0; j < Q; j++)
            {
                int id = qry[j];
                if (i != id)
                {
                    float d = l2sqr(data[idx[id]], data[idx[i]]);
                    if (is_good_value(log(double(d)))) topks[j] << Topk<unsigned>::Element(i, d);
                }
            }
        }

        fill(M.begin(), M.end(), 0.0);
        fill(G.begin(), G.end(), 0.0);

        for (unsigned i = 0; i < Q; i++)
        {
            for (unsigned k = 0; k < K; k++)
            {
                M[k] += topks[i][k].dist;
                G[k] += log(topks[i][k].dist);
            }
        }

        for (unsigned k = 0; k < K; k++)
        {
            M[k] = log(M[k]/Q);
            G[k] /= Q;
            gsl_matrix_set(X, m, 0, 1.0);
            gsl_matrix_set(X, m, 1, log(double(data.getSize() * (l + 1)) / double(F)));
            gsl_matrix_set(X, m, 2, log(double(k + 1)));
            gsl_vector_set(yM, m, M[k]);
            gsl_vector_set(yG, m, G[k]);
            ++m;
        }

        ++progress;
    }

    gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc(F * K, 3);

    double chisq;

    gsl_multifit_linear(X, yM, pM, cov, &chisq, work);
    gsl_multifit_linear(X, yG, pG, cov, &chisq, work);

    cout << gM << '\t' << gG << endl;
    cout << gsl_vector_get(pM, 0) << '\t'
         << gsl_vector_get(pM, 1) << '\t'
         << gsl_vector_get(pM, 2) << endl;
    cout << gsl_vector_get(pG, 0) << '\t'
         << gsl_vector_get(pG, 1) << '\t'
         << gsl_vector_get(pG, 2) << endl;

    gsl_matrix_free(X);
    gsl_matrix_free(cov);
    gsl_vector_free(pM);
    gsl_vector_free(pG);
    gsl_vector_free(yM);
    gsl_vector_free(yG);

    return 0;
}