/************************************************************
 *
 * Copyright (c) 2021, University of California, Los Angeles
 *
 * Authors: Kenny J. Chen, Brett T. Lopez
 * Contact: kennyjchen@ucla.edu, btlopez@ucla.edu
 *
 ***********************************************************/

/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2008-2009  Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
 * Copyright 2008-2009  David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
 * Copyright 2011-2021  Jose Luis Blanco (joseluisblancoc@gmail.com).
 *   All rights reserved.
 *
 * THE BSD LICENSE
 *
 * 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.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
 *************************************************************************/

#ifndef NANO_KDTREE_KDTREE_FLANN_H_
#define NANO_KDTREE_KDTREE_FLANN_H_

#include <boost/shared_ptr.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include "nano_gicp/impl/nanoflann_impl.hpp"

namespace nanoflann
{

template <typename PointT>
class KdTreeFLANN
{
public:

  typedef typename pcl::PointCloud<PointT> PointCloud;
  typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
  typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;

  typedef boost::shared_ptr<KdTreeFLANN<PointT>> Ptr;
  typedef boost::shared_ptr<const KdTreeFLANN<PointT>> ConstPtr;
  typedef boost::shared_ptr<std::vector<int>> IndicesPtr;
  typedef boost::shared_ptr<const std::vector<int>> IndicesConstPtr;

  KdTreeFLANN (bool sorted = false);
  KdTreeFLANN (const KdTreeFLANN<PointT> &k);

  void  setEpsilon (float eps);

  void  setSortedResults (bool sorted);

  inline Ptr makeShared () { return Ptr (new KdTreeFLANN<PointT> (*this)); }

  void setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr &indices = IndicesConstPtr ());

  inline PointCloudConstPtr getInputCloud() const { return _adaptor.pcl; }

  int  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
                       std::vector<float> &k_sqr_distances) const;

  int radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices,
                    std::vector<float> &k_sqr_distances) const;

protected:

  nanoflann::SearchParams _params;

  struct PointCloud_Adaptor
  {
    inline size_t kdtree_get_point_count() const;
    inline float kdtree_get_pt(const size_t idx, int dim) const;
    template <class BBOX> bool kdtree_get_bbox(BBOX&) const { return false; }
    PointCloudConstPtr pcl;
    IndicesConstPtr indices;
  };

  typedef nanoflann::KDTreeSingleIndexAdaptor<
    nanoflann::SO3_Adaptor<float, PointCloud_Adaptor > ,
    PointCloud_Adaptor, 3, int> KDTreeFlann_PCL_SO3;

  PointCloud_Adaptor _adaptor;

  KDTreeFlann_PCL_SO3 _kdtree;

};

//---------- Definitions ---------------------

template<typename PointT> inline
KdTreeFLANN<PointT>::KdTreeFLANN(bool sorted):
  _kdtree(3,_adaptor, KDTreeSingleIndexAdaptorParams(100))
{
  _params.sorted = sorted;
}

template<typename PointT> inline
void KdTreeFLANN<PointT>::setEpsilon(float eps)
{
  _params.eps = eps;
}

template<typename PointT> inline
void KdTreeFLANN<PointT>::setSortedResults(bool sorted)
{
  _params.sorted = sorted;
}

template<typename PointT> inline
void KdTreeFLANN<PointT>::setInputCloud(const KdTreeFLANN::PointCloudConstPtr &cloud,
                                        const IndicesConstPtr &indices)
{
  _adaptor.pcl = cloud;
  _adaptor.indices = indices;
  _kdtree.buildIndex();
}

template<typename PointT> inline
int KdTreeFLANN<PointT>::nearestKSearch(const PointT &point, int num_closest,
                                std::vector<int> &k_indices,
                                std::vector<float> &k_sqr_distances) const
{
  k_indices.resize(num_closest);
  k_sqr_distances.resize(num_closest);

  nanoflann::KNNResultSet<float,int> resultSet(num_closest);
  resultSet.init( k_indices.data(), k_sqr_distances.data());
  _kdtree.findNeighbors(resultSet, point.data, nanoflann::SearchParams() );
  return resultSet.size();
}

template<typename PointT> inline
int KdTreeFLANN<PointT>::radiusSearch(const PointT &point, double radius,
                              std::vector<int> &k_indices,
                              std::vector<float> &k_sqr_distances) const
{
  static std::vector<std::pair<int, float> > indices_dist;
  indices_dist.reserve( 128 );

  RadiusResultSet<float, int> resultSet(radius, indices_dist);
  const size_t nFound = _kdtree.findNeighbors(resultSet, point.data, _params);

  if (_params.sorted)
    std::sort(indices_dist.begin(), indices_dist.end(), IndexDist_Sorter() );

  k_indices.resize(nFound);
  k_sqr_distances.resize(nFound);
  for(int i=0; i<nFound; i++ ){
    k_indices[i]       = indices_dist[i].first;
    k_sqr_distances[i] = indices_dist[i].second;
  }
  return nFound;
}

template<typename PointT> inline
size_t KdTreeFLANN<PointT>::PointCloud_Adaptor::kdtree_get_point_count() const {
  if( indices ) return indices->size();
  if( pcl)  return pcl->points.size();
  return 0;
}

template<typename PointT> inline
float KdTreeFLANN<PointT>::PointCloud_Adaptor::kdtree_get_pt(const size_t idx, int dim) const{
  const PointT& p = ( indices ) ? pcl->points[(*indices)[idx]] : pcl->points[idx];
  if (dim==0) return p.x;
  else if (dim==1) return p.y;
  else if (dim==2) return p.z;
  else return 0.0;
}

}

#endif


/*
Copyright (c) 2020 Naomasa Matsubayashi

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <cstdint>
#include <cmath>
#include <array>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>
#include <boost/program_options.hpp>
#include <nlohmann/json.hpp>
#include "ifm/setter.h"
#include "ifm/store_monoral.h"
#include "ifm/bessel.h"

int main( int argc, char *argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("output,o", boost::program_options::value<std::string>()->default_value( "bessel.mp" ), "出力ファイル")
    ("max,m", boost::program_options::value<int>()->default_value(60), "最大階数");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("output") ) {
    std::cout << options << std::endl;
    return 0;
  }
  const auto pre = ifm::create_bessel_approx_2019_precomp_array( params[ "max" ].as< int >() );
  nlohmann::json out( pre );
  auto mp = nlohmann::json::to_msgpack( out );
  std::ofstream out_file( params["output"].as<std::string>(),std::ofstream::binary );
  std::copy( mp.begin(), mp.end(), std::ostreambuf_iterator< char >( out_file ) );
}


/*
 * SPDX-FileCopyrightText: © 2018 Ambrosys GmbH
 *
 * SPDX-FileContributor: Fabian Sandoval Saldias <fabianvss@gmail.com>
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <AppComponents/Common/Reader/Osm/Conversion.h>
#include <AppComponents/Common/Reader/OsmMapReader.h>

#include <Core/Common/Geometry/Conversion.h>
#include <Core/Common/Geometry/Helper.h>
#include <Core/Common/Postgres/Helper.h>

#include <Generic/String/Split.h>

#include <amblog/global.h>
#include <pqxx/pqxx>

#include <boost/algorithm/string/replace.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <boost/iterator/function_output_iterator.hpp>

#include <iomanip>
#include <iterator>
#include <unordered_map>
#include <vector>

namespace AppComponents::Common::Reader {

namespace {

    struct PointLocation
    {
        size_t osmLineId;
        size_t pointIndex;
    };

    struct UniquePoint
    {
        size_t id;
        std::vector<PointLocation> locations;
    };

    struct OsmPointCandidate
    {
        size_t id;
        Core::Common::Geometry::Point point;
        //std::string layer;
        //std::string level;
        //std::string location;
    };

    struct OsmLineCandidate
    {
        Types::Street::Segment segment;
        Types::Street::TravelDirection travelDirection;
        Types::Street::Highway highway;
        //std::string layer;
        //std::string level;
        //std::string location;
    };

    struct Candidate
    {
        OsmLineCandidate segment;
        std::vector<size_t> osmPointIds;
    };

    using PointGeoindexValue = std::pair<Core::Common::Geometry::Point, std::shared_ptr<UniquePoint>>;
    using PointGeoindexAlgorithm = boost::geometry::index::quadratic<16>;
    using PointGeoindex = boost::geometry::index::rtree<PointGeoindexValue, PointGeoindexAlgorithm>;

    /**
     * Points are considered equal if the distance is 10cm or less.
     */
    std::shared_ptr<UniquePoint> getUniquePoint(PointGeoindex & geoindex, Core::Common::Geometry::Point point)
    {
        unsigned const k_nearest = 1;
        double const maxDistanceInMeters = 0.1;
        auto result = std::optional<PointGeoindexValue>{};
        auto inserter = [&](PointGeoindexValue const & value)
        {
            if (geoDistance(value.first, point) <= maxDistanceInMeters)
                result = value;
        };
        geoindex.query(boost::geometry::index::nearest(point, k_nearest), boost::make_function_output_iterator(inserter));
        if (not result)
        {
            result = PointGeoindexValue{point, std::make_shared<UniquePoint>()};
            geoindex.insert(*result);
        }
        return result->second;
    }

    /**
     * @param records The query should have returned lines and points pairwise sequentially (multiply line data),
     *   like { { line1, point1 }, { line1, point2 }, { line2, point1 }, ... }.
     * @return { candidates, osmPointMap, geoindex }
     */
    std::tuple<std::vector<Candidate>, std::unordered_map<size_t, OsmPointCandidate>, PointGeoindex> getCandidates(pqxx::result & records)
    {
        using namespace Core::Common::Geometry;
        using namespace Core::Common::Postgres;
        using namespace AppComponents::Common::Reader::Osm;

        auto candidates = std::vector<Candidate>{};
        auto osmPointMap = std::unordered_map<size_t, OsmPointCandidate>{};
        auto geoindex = PointGeoindex{};

        auto currentSegment = std::optional<OsmLineCandidate>{};
        auto currentNodes = std::vector<size_t>{};

        size_t nextPointId = 0;
        auto addCurrentStreet = [&]()
        {
            assert(currentSegment);
            for (size_t i = 0; i < currentSegment->segment.geometry.size(); ++i)
            {
                auto & uniquePoint = *getUniquePoint(geoindex, currentSegment->segment.geometry[i]);
                uniquePoint.id = nextPointId++;  // TODO: only increment if a new UniquePoint was created?
                uniquePoint.locations.push_back(PointLocation{currentSegment->segment.originId, i});
            }
            candidates.push_back(Candidate{std::move(*currentSegment), std::move(currentNodes)});
            currentSegment.reset();
            currentNodes.clear();
        };

        for (auto const & row : records)
        {
            auto segmentIdRaw = row.at("line_id").as<int64_t>();
            size_t segmentId = segmentIdRaw >= 0 ? static_cast<size_t>(segmentIdRaw) : static_cast<size_t>(-segmentIdRaw);
            if (not currentSegment || currentSegment->segment.originId != segmentId)
            {
                if (currentSegment)
                    addCurrentStreet();
                auto way = row.at("line_way").as<std::string>();
                auto oneway = getOptional(row.at("line_oneway"), std::string{});
                currentSegment = OsmLineCandidate{
                    {segmentId, 0, toLineString(way)}, toTravelDirection(oneway), toHighway(row.at("line_highway").as<std::string>())
                    //getOptional( row.at( "line_layer" ), std::string{} ),
                    //getOptional( row.at( "line_level" ), std::string{} ),
                    //getOptional( row.at( "line_location" ), std::string{} )
                };
            }
            /*
            auto osmPointId = getOptional<size_t>( row.at( "point_id" ) );
            if( osmPointId )
            {
                if( osmPointMap.find( *osmPointId ) == osmPointMap.end() )
                {
                    auto pointway = getOptional<std::string>( row.at( "point_way" ) );
                    if( !pointway )
                    {
                        APP_LOG( error ) << "planet_osm_point.way is null where planet_osm_point.osm_id is not";
                        APP_THROW_LOGGED_EXCEPTION();
                    }

                    osmPointMap.emplace( *osmPointId, OsmPointCandidate{
                        *osmPointId,
                        toPoint( *pointway )
                        //getOptional( row.at( "point_layer" ), std::string{} ),
                        //getOptional( row.at( "point_level" ), std::string{} ),
                        //getOptional( row.at( "point_location" ), std::string{} )
                    } );
                }
                currentNodes.emplace_back( *osmPointId );
            }
            */
        }
        if (currentSegment)
            addCurrentStreet();

        return {std::move(candidates), std::move(osmPointMap), std::move(geoindex)};
    }

    /**
     *
     * @param candidates
     * @param osmPointMap
     * @param geoindex
     * @param splitOnOverlappingPoints If true, candidates are split on shared point intersections.
     */
    std::tuple<Types::Street::SegmentList, Types::Street::NodePairList, Types::Street::TravelDirectionList, Types::Street::HighwayList> processCandidates(
        std::vector<Candidate> const & candidates,
        std::unordered_map<size_t, OsmPointCandidate> const & osmPointMap [[gnu::unused]],
        PointGeoindex & geoindex,
        bool const splitOnOverlappingPoints)
    {
        Types::Street::SegmentList segmentList;
        Types::Street::NodePairList nodePairList;
        Types::Street::TravelDirectionList travelDirectionList;
        Types::Street::HighwayList highwayList;

        auto addStreet = [&](OsmLineCandidate segmentCandidate, size_t firstPointIndex, size_t lastPointIndex, size_t firstPointId, size_t lastPointId)
        {
            Types::Street::Segment segment;
            segment.originId = segmentCandidate.segment.originId;
            segment.originOffset = firstPointIndex;
            for (size_t i = firstPointIndex; i <= lastPointIndex; ++i)
                segment.geometry.push_back(segmentCandidate.segment.geometry[i]);
            segmentList.push_back(segment);
            nodePairList.emplace_back(firstPointId, lastPointId);
            travelDirectionList.push_back(segmentCandidate.travelDirection);
            highwayList.push_back(segmentCandidate.highway);
        };

        for (auto const & candidate : candidates)
        {
            size_t firstPointIndex = 0;
            size_t firstPointId = getUniquePoint(geoindex, candidate.segment.segment.geometry[firstPointIndex])->id;
            if (splitOnOverlappingPoints)
                for (size_t i = 1; i < candidate.segment.segment.geometry.size() - 1; ++i)
                {
                    auto & uniquePoint = *getUniquePoint(geoindex, candidate.segment.segment.geometry[i]);
                    if (uniquePoint.locations.size() > 1)  // split street on junction
                    {
                        addStreet(candidate.segment, firstPointIndex, i, firstPointId, uniquePoint.id);
                        firstPointIndex = i;
                        firstPointId = uniquePoint.id;
                    }
                }
            size_t lastPointIndex = candidate.segment.segment.geometry.size() - 1;
            size_t lastPointId = getUniquePoint(geoindex, candidate.segment.segment.geometry[lastPointIndex])->id;
            addStreet(candidate.segment, firstPointIndex, lastPointIndex, firstPointId, lastPointId);
        }

        return {std::move(segmentList), std::move(nodePairList), std::move(travelDirectionList), std::move(highwayList)};
    }

    template <typename Iterator, typename Distance>
    static inline void inner_douglas_peucker_(Iterator begin,
                                Iterator end,
                                Distance const& max_dist,
                                int& n)
    {

        std::size_t size = end - begin;

        // size must be at least 3
        // because we want to consider a candidate point in between
        if (size <= 2)
        {
            return;
        }

        Iterator last = end - 1;

        // Find most far point, compare to the current segment
        //geometry::segment<Point const> s(begin->p, last->p);
        auto md(-1.0); // any value < 0
        Iterator candidate = end;
        for (Iterator it = begin + 1; it != last; ++it)
        {

            auto dist = Core::Common::Geometry::geoDistance(it->p, Core::Common::Geometry::Segment(begin->p, last->p));

            if (md <  std::get<0>(dist))
            {
                md = std::get<0>(dist);
                candidate = it;
            }
        }

        // If a point is found, set the include flag
        // and handle segments in between recursively
        if (max_dist < md && candidate != end)
        {
            candidate->included = true;
            n++;

            inner_douglas_peucker_(begin, candidate + 1, max_dist, n);
            inner_douglas_peucker_(candidate, end, max_dist, n);
        }
    }

    /**
     * Extended apply_ from https://github.com/boostorg/geometry/blob/develop/include/boost/geometry/algorithms/simplify.hpp
     *
     * @tparam Range
     * @tparam OutputIterator
     * @tparam SkippedItereator
     * @tparam Distance
     * @tparam PSDistanceStrategy
     * @param range
     * @param out
     * @param Distance
     * @return
     */
    template <typename Distance>
    static inline void douglas_peucker_(AppComponents::Common::Types::Track::PointList const & range,
                                        AppComponents::Common::Types::Track::PointList & out,
                                        Distance const & max_distance)
    {

        typedef typename boost::range_value<AppComponents::Common::Types::Track::PointList>::type point_type;
        typedef boost::geometry::strategy::simplify::detail::douglas_peucker_point<point_type> dp_point_type;

        // Copy coordinates, a vector of references to all points
        std::vector<dp_point_type> ref_candidates(boost::begin(range),
                                                  boost::end(range));

        // Include first and last point of line,
        // they are always part of the line
        int n = 2;
        ref_candidates.front().included = true;
        ref_candidates.back().included = true;

        // Get points, recursively, including them if they are further away
        // than the specified distance
        inner_douglas_peucker_(boost::begin(ref_candidates), boost::end(ref_candidates), max_distance, n);

        // Copy included elements to the output,
        int index = 0;
        for (auto it = boost::begin(ref_candidates); it != boost::end(ref_candidates); ++it, index++)
        {
            if (it->included)
            {
                // copy-coordinates does not work because OutputIterator
                // does not model Point (??)
                //geometry::convert(*(it->p), *out);
                out.push_back(it->p);
            }

        }
    }

}  // namespace

OsmMapReader::OsmMapReader(
    Core::Common::Postgres::Connection & connection,
    std::unordered_set<Types::Street::HighwayType> const & highwaySelection,
    double const fetchCorridor,
    bool const useSingleSearchCircle,
    bool const splitOnOverlappingPoints)
  : connection_(connection), highwaySelection_(highwaySelection), fetchCorridor_(fetchCorridor), useSingleSearchCircle_(useSingleSearchCircle),
    splitOnOverlappingPoints_(splitOnOverlappingPoints)
{
}

bool OsmMapReader::init(
    Types::Track::PointList const & pointList,
    double const fetchCorridor,
    bool const useSingleSearchCircle,
    std::optional<std::unordered_set<Types::Street::HighwayType>> const & highwaySelection)
{
    using namespace Core::Common;

    std::string pointsString;
    if (!useSingleSearchCircle_)
    {
        // Simplify Track
        // ToDo: With correct type handling and inheritance the boost::geometry::simplify can be used directly
        typedef boost::geometry::model::d2::point_xy<double> xy;
        float maxDistance = 1.0; // GPS granularity is 2m, allow half of it since it can be left and right
        AppComponents::Common::Types::Track::PointList simplified{};
        douglas_peucker_(pointList, simplified, maxDistance);

        searchRadius_ = fetchCorridor_ + maxDistance;
        pointsString_ = Geometry::toWkt(simplified);
    }
    else
    {
        if (pointList.empty())
        {
            searchRadius_ = 0;
            pointsString_ = Geometry::toWkt(std::vector<Geometry::Point>{});
        }
        else if (pointList.size() == 1)
        {
            searchRadius_ = fetchCorridor_;
            pointsString_ = toWkt(pointList[0]);
        }
        else
        {
            double distance = Geometry::geoDistance(pointList[0], pointList[pointList.size() - 1]);
            double heading = Geometry::heading(pointList[0], pointList[pointList.size() - 1]);
            auto center = Geometry::reverseHaversine(pointList[0], heading, distance / 2.0);

            searchRadius_ = distance / 2.0 + fetchCorridor_;
            pointsString_ = toWkt(center);
        }
    }

    if (highwaySelection)
        highwaySelection_ = highwaySelection.value();

    return false;
}

bool OsmMapReader::operator()(
    Types::Street::SegmentList & segmentList,
    Types::Street::NodePairList & nodePairList,
    Types::Street::TravelDirectionList & travelDirectionList,
    Types::Street::HighwayList & highwayList)
{
    using namespace Core::Common;
    using namespace AppComponents::Common::Reader::Osm;

    auto dbConnection = connection_.getConnection();
    auto dbTransaction = pqxx::work{*dbConnection};

    // Occurences germany 2018-07-18:
    //   count highway
    // 2624300 track
    // 2059463 service
    // 1694021 residential
    // 1287753 footway
    //  996774 path
    //  363015 secondary
    //  339168 unclassified
    //  329683 tertiary
    //  184903 steps
    //  181721 primary
    //  123117 living_street
    //  106666 cycleway
    //   62585 motorway
    //   38529 motorway_link
    //   28494 trunk
    //   26718 pedestrian
    //   18351 trunk_link
    //   17649 primary_link
    //    9541 secondary_link
    //    8937 construction
    //    8931 platform
    //    6581 bridleway
    //    6264 proposed
    //    4243 tertiary_link
    //    3104 road
    //    1929 corridor
    //    1443 raceway
    //     920 elevator
    //     739 bus_stop
    //     424 rest_area
    //    1460 [53 different types]

    /* // TODO: This is the query with layer data from lines and points.
    auto query = std::string{ R"sql(
        select
            line.osm_id     line_id,
            line.oneway     line_oneway,
            line.layer      line_layer,
            line.level      line_level,
            line.location   line_location,
            point.osm_id    point_id,
            point.layer     point_layer,
            point.level     point_level,
            point.location  point_location,
            ST_AsText( ST_Transform( line.way, 4326 ) )   line_way,
            ST_AsText( ST_Transform( point.way, 4326 ) )  point_way
        from
            planet_osm_line   line
        left outer join
            planet_osm_point  point
            on
                ST_Touches( line.way, point.way )
                and (point.layer is not null or point.level is not null or point.location is not null)
        where
            (
                line.highway = 'motorway'
                or line.highway = 'trunk'
                or line.highway = 'primary'
                or line.highway = 'secondary'
                or line.highway = 'motorway_link'
                or line.highway = 'trunk_link'
                or line.highway = 'primary_link'
                or line.highway = 'secondary_link'
            )
            and ST_DWithin( ST_Transform( ST_GeomFromText( $1, 4326 ), 32632 ), line.way, $2 )
        )sql" };
    */
    auto query = std::string{R"sql(
        select
            line.osm_id  line_id,
            line.oneway  line_oneway,
            line.highway line_highway,
            ST_AsText( ST_Transform( line.way, 4326 ) )  line_way
        from
            planet_osm_line  line
        where
            ( $HIGHWAY_CONDITION )
            and ST_DWithin( ST_Transform( ST_GeomFromText( $1, 4326 ), 32632 ), line.way, $2 )
        )sql"};
    boost::replace_all(query, "$HIGHWAY_CONDITION", toHighwaySelectionSql(highwaySelection_, "line"));
    dbConnection->prepare("", query);
    pqxx::result records = dbTransaction.exec_prepared("", pointsString_, searchRadius_);

    APP_LOG_TAG_MS(noise, "DB") << records.size() << " records were read";

    auto [candidates, osmPointMap, geoindex] = getCandidates(records);

    APP_LOG_TAG_MS(noise, "DB") << candidates.size() << " lines, " << osmPointMap.size() << " points and " << geoindex.size() << " coordinates fetched";

    std::tie(segmentList, nodePairList, travelDirectionList, highwayList) = processCandidates(candidates, osmPointMap, geoindex, splitOnOverlappingPoints_);

    APP_LOG_MS(noise) << segmentList.size() << " street segments created";

    return true;
}

bool OsmMapReader::operator()(
    Types::Track::PointList const & pointList,
    Types::Street::SegmentList & segmentList,
    Types::Street::NodePairList & nodePairList,
    Types::Street::TravelDirectionList & travelDirectionList,
    Types::Street::HighwayList & highwayList)
{
    init(pointList, searchRadius_, useSingleSearchCircle_, highwaySelection_);

    return this->operator()(segmentList, nodePairList, travelDirectionList, highwayList);
}

}  // namespace AppComponents::Common::Reader


#include <boost/python.hpp>
#include <cstdint>
#include <atomic>
#include <vector>
#include <random>

namespace p  = boost::python;

template <uint64_t seed=42>
struct fmix64_t {

    typedef uint64_t data_t;
    typedef uint64_t rtrn_t;
    static constexpr uint64_t width = sizeof(data_t) << 3;

    rtrn_t operator()(const data_t x, const uint64_t k) const {

        #define BIG_CONSTANT(x) (x##LLU)

        rtrn_t y = x+k+seed;

        y ^= y >> 33;
        y *= BIG_CONSTANT(0xff51afd7ed558ccd);
        y ^= y >> 33;
        y *= BIG_CONSTANT(0xc4ceb9fe1a85ec53);
        y ^= y >> 33;

        return y;
    }

    rtrn_t mod_width(const rtrn_t x) const {
        return x & 63;
    }
};
#include<iostream>
template <uint64_t seed=42, uint64_t max_k=128>
struct tmix64_t {

    typedef uint64_t data_t;
    typedef uint64_t rtrn_t;
    static constexpr uint64_t width = sizeof(data_t) << 3;

    std::vector<uint64_t> table_range;

    tmix64_t () {

        std::mt19937 engine(seed);
        std::uniform_int_distribution<uint64_t> range;

        for (uint64_t k = 0; k < max_k; k++)
            for (uint64_t slot = 0; slot < 8; slot++)
                for (uint64_t entry = 0; entry < 256; entry++)
                    table_range.push_back(range(engine));
    }

    rtrn_t operator()(const data_t x, const uint64_t k) const {

        rtrn_t result = 0;
        for (uint64_t slot = 0; slot < 8; slot++) {
            const uint64_t entry = (x >> (8*slot)) & 255;
            result ^= table_range[k*256*8+slot*256+entry];
        }

        return result;
    }

    rtrn_t mod_width(const rtrn_t x) const {
        return x & 63;
    }
};

template <class data_t>
struct Bitarray {

    static constexpr uint64_t width = sizeof(data_t) << 3;

    std::atomic<data_t> * data;

    const uint64_t capacity;
    const uint64_t num_slots;
    const bool in_parallel;

    Bitarray(const uint64_t capacity_,
             const bool in_parallel_=false) :
        capacity    (capacity_),
        num_slots   ((capacity_+width-1)/width),
        in_parallel (in_parallel_) {

        data = new std::atomic<data_t>[num_slots];

        # pragma omp parallel for if (in_parallel)
        for (uint64_t slot = 0; slot < num_slots; slot++)
            data[slot].store(0);
    }

    ~Bitarray () {
        delete [] data;
    }

    template <bool throw_exception=false>
    void set(const uint64_t pos) {

        if (pos >= capacity) {
            if (throw_exception)
                throw std::out_of_range("Index out of range");
            else
                return;
        }

        const uint64_t slot = pos/width;
        const uint64_t mask = 1UL << (pos-slot*width);

        const data_t expected = data[slot].load();

        if (!(expected & mask))
            data[slot].fetch_or(mask);

    }

    template <bool throw_exception=false>
    bool get(const uint64_t pos) {

        if (pos >= capacity) {
            if (throw_exception)
                throw std::out_of_range("Index out of range");
            else
                return false;
        }

        const uint64_t slot = pos/width;
        const uint64_t mask = 1UL << (pos-slot*width);

        return data[slot].load() & mask;
    }

    uint64_t len () {
        return capacity;
    }
};

template <class hash_t>
struct BloomFilter{

    typedef typename hash_t::data_t data_t;
    typedef Bitarray<data_t> array_t;
    static constexpr uint64_t width = hash_t::width;

    const uint64_t capacity;
    const uint64_t num_slots;
    const uint64_t num_hashes;
    const hash_t hash;
    const bool in_parallel;

    std::atomic<data_t> * data;

    BloomFilter(
        const uint64_t capacity_,
        const uint64_t num_hashes_,
        const bool in_parallel_=false) :
            capacity    (capacity_),
            num_slots   ((capacity_+width-1)/width),
            num_hashes  (num_hashes_),
            hash        (hash_t()),
            in_parallel (in_parallel_) {

        data = new std::atomic<data_t>[num_slots];

        # pragma omp parallel for if (in_parallel)
        for (uint64_t slot = 0; slot < num_slots; slot++)
            data[slot].store(0);
    }

    ~BloomFilter () {
        delete [] data;
    }

    void insert(p::list& list) {

        const uint64_t num_pos = p::len(list);

        data_t * hashes = new data_t[num_pos];
        for (uint64_t pos = 0; pos < num_pos; pos++)
            hashes[pos] = PyObject_Hash(p::api::object(list[pos]).ptr());

        # pragma omp parallel for if (in_parallel)
        for (uint64_t pos = 0; pos < num_pos; pos++) {

            const data_t c_hash = hashes[pos];

            for (uint64_t hash_id = 0; hash_id < num_hashes; hash_id++) {
                const uint64_t loc  = hash(c_hash, hash_id) % capacity;
                const uint64_t slot = loc/width;
                const uint64_t bit  = loc-slot*width;
                const data_t   mask = 1UL << bit;
                const data_t expected = data[slot].load();

                if ((expected & mask) !=  mask)
                    data[slot].fetch_or(mask);
            }
        }

        delete [] hashes;
    }

    boost::shared_ptr<array_t> query(p::list& list) {

        const uint64_t num_pos = p::len(list);
        boost::shared_ptr<array_t> result(new array_t(num_pos, in_parallel));

        data_t * hashes = new data_t[num_pos];
        for (uint64_t pos = 0; pos < num_pos; pos++)
            hashes[pos] = PyObject_Hash(p::api::object(list[pos]).ptr());

        # pragma omp parallel for if (in_parallel)
        for (uint64_t pos = 0; pos < num_pos; pos++) {

            const data_t c_hash = hashes[pos];

            bool go = true;
            for (uint64_t hash_id = 0; hash_id < num_hashes && go; hash_id++) {
                const uint64_t loc  = hash(c_hash, hash_id) % capacity;
                const uint64_t slot = loc/width;
                const uint64_t bit  = loc-slot*width;
                const data_t   mask = 1UL << bit;
                const data_t expected = data[slot].load();

                if ((expected & mask) !=  mask)
                    go = false;
            }

            if (go)
                result-> template set<false>(pos);
        }

        delete [] hashes;

        return result;
    }
};

template <class hash_t>
struct PartitionedBloomFilter{

    typedef typename hash_t::data_t data_t;
    typedef Bitarray<data_t> array_t;
    static constexpr uint64_t width = hash_t::width;

    const uint64_t capacity;
    const uint64_t num_slots;
    const uint64_t num_hashes;
    const hash_t hash;
    const bool in_parallel;

    std::atomic<data_t> * data;

    PartitionedBloomFilter(
        const uint64_t capacity_,
        const uint64_t num_hashes_,
        const bool in_parallel_=false) :
            capacity    (capacity_),
            num_slots   ((capacity_+width-1)/width),
            num_hashes  (num_hashes_),
            hash        (hash_t()),
            in_parallel (in_parallel_) {

        data = new std::atomic<data_t>[num_slots];

        # pragma omp parallel for if (in_parallel)
        for (uint64_t slot = 0; slot < num_slots; slot++)
            data[slot].store(0);
    }

    ~PartitionedBloomFilter () {
        delete [] data;
    }

    void insert(p::list& list) {

        const uint64_t num_pos = p::len(list);

        data_t * hashes = new data_t[num_pos];
        for (uint64_t pos = 0; pos < num_pos; pos++)
            hashes[pos] = PyObject_Hash(p::api::object(list[pos]).ptr());

        # pragma omp parallel for if (in_parallel)
        for (uint64_t pos = 0; pos < num_pos; pos++) {

            const data_t c_hash = hashes[pos];

            data_t mask = 0;
            for (uint64_t hash_id = 0; hash_id < num_hashes; hash_id++)
                mask |= 1UL << hash.mod_width(hash(c_hash, hash_id));

            const uint64_t slot = hash(c_hash, num_hashes) % num_slots;
            const data_t expected = data[slot].load();

            if ((expected & mask) !=  mask)
                data[slot].fetch_or(mask);
        }

        delete [] hashes;
    }

    boost::shared_ptr<array_t> query(p::list& list) {

        const uint64_t num_pos = p::len(list);
        boost::shared_ptr<array_t> result(new array_t(num_pos, in_parallel));

        data_t * hashes = new data_t[num_pos];
        for (uint64_t pos = 0; pos < num_pos; pos++)
            hashes[pos] = PyObject_Hash(p::api::object(list[pos]).ptr());

        # pragma omp parallel for if (in_parallel)
        for (uint64_t pos = 0; pos < num_pos; pos++) {

            const data_t c_hash = hashes[pos];

            data_t mask = 0;
            for (uint64_t hash_id = 0; hash_id < num_hashes; hash_id++)
                mask |= 1UL << hash.mod_width(hash(c_hash, hash_id));

            const uint64_t slot = hash(c_hash, num_hashes) % num_slots;
            const data_t expected = data[slot].load();

            if ((expected & mask) ==  mask)
                result-> template set<false>(pos);

        }

        delete [] hashes;

        return result;
    }
};

BOOST_PYTHON_MODULE(pyfere) {

    typedef BloomFilter<fmix64_t<>> bfilter64_t;
    typedef boost::shared_ptr<bfilter64_t> bfilter64_ptr_t;

    p::class_<bfilter64_t, bfilter64_ptr_t>("BloomFilter",
                          p::init<uint64_t, uint64_t, p::optional<bool> >())
        .def("insert", &bfilter64_t::insert)
        .def("query",  &bfilter64_t::query );

    typedef PartitionedBloomFilter<fmix64_t<>> pfilter64_t;
    typedef boost::shared_ptr<pfilter64_t> pfilter64_ptr_t;

    p::class_<pfilter64_t, pfilter64_ptr_t>("PartitionedBloomFilter",
                          p::init<uint64_t, uint64_t, p::optional<bool> >())
        .def("insert", &pfilter64_t::insert)
        .def("query",  &pfilter64_t::query );

    typedef Bitarray<typename pfilter64_t::data_t> bitarray64_t;
    typedef boost::shared_ptr<bitarray64_t> bitarray64_ptr_t;

    p::class_<bitarray64_t, bitarray64_ptr_t>("Bitarray",
                            p::init<uint64_t, p::optional<bool> >())
        .def("__getitem__", &bitarray64_t::get<true>)
        .def("__len__",     &bitarray64_t::len);
}


/**
 * sudoku_gd.cpp
 *
 * By Sebastian Raaphorst, 2018.
 */

#include <iostream>
#include <memory>
#include <stdexcept>

#include <boost/program_options.hpp>

#include <GenSudokuBoard.h>
#include <GenSudokuBoardAscenderPopulator.h>
#include <GreatDelugeAlgorithm.h>

#include "progopts.h"

namespace po = boost::program_options;
using namespace vorpal::gensudoku;
using namespace vorpal::stochastic;

int main(int argc, const char * const argv[]) {
    using solver = GreatDelugeAlgorithm<SudokuBoard>;
    solver::options_type options;

    try {
        auto desc = init_options(options);
        desc->add_options()
                ("initial-water-level,w", po::value<double>()->default_value(100.0), "initial water level")
                ("rain-speed,r", po::value<double>()->default_value(0.1), "rain speed");
        auto done = end_options<SudokuBoardAscenderPopulator, solver::options_type>(argc, argv, desc, options,
        [](po::variables_map &vm, solver::options_type &options) {
            options.initial_water_level = vm["initial-water-level"].as<double>();
            options.rain_speed          = vm["rain-speed"].as<double>();
        });
        if (done) return 0;
    } catch (const std::exception &e) {
        std::cerr << e.what() << '\n';
        return 1;
    }

    run<solver>(options);
}


/*
 *  testPMLParameters.cpp
 *  Trogdor6
 *
 *  Created by Paul Hansen on 4/1/10.
 *  Copyright 2010 Stanford University. All rights reserved.
 *
 *  This file is covered by the MIT license.  See LICENSE.txt.
 */

#define BOOST_TEST_MAIN
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Test3D

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

#include "PMLParameters.h"
#include "PhysicalConstants.h"

using namespace std;

BOOST_AUTO_TEST_CASE( Test1 )
{
    float dx = 5e-9;
    float gridDiagonalSize = 1000e-9;
    float greaterThanOne = 1.001;

    float sigmaBounds[] = { 0.0, greaterThanOne*0.8f*4/float(sqrt(Constants::eta0))/dx };
    float alphaBounds[] = { 0.0, greaterThanOne*float(Constants::c)*float(Constants::eps0)/50/dx };
    float kappaBounds[] = { 1.0, greaterThanOne*5.0f };

    BOOST_TEST_MESSAGE("Cell size " << dx << " meters.");
    BOOST_TEST_MESSAGE("Sigma bounds [" << sigmaBounds[0] << ", " <<
        sigmaBounds[1] << "].");
    BOOST_TEST_MESSAGE("Kappa bounds [" << kappaBounds[0] << ", " <<
        kappaBounds[1] << "].");
    BOOST_TEST_MESSAGE("Alpha bounds [" << alphaBounds[0] << ", " <<
        alphaBounds[1] << "].");

    CalculatePMLParameters callback(dx, gridDiagonalSize,
        Constants::eps0, Constants::mu0);

    for (double depth = -1.0; depth <= 2.0; depth += 0.1)
    {
        PMLParameters p = callback(depth);

        // depths out of range should yield default PML parameters
        if (depth <= 0.0 || depth > 1.0)
        {
            BOOST_CHECK(p == PMLParameters());
        }
        else
        {
            BOOST_CHECK(p.sigma() > 0.0);
            BOOST_CHECK(p.kappa() > 0.0);
            BOOST_CHECK(p.alpha() >= 0.0);

            BOOST_CHECK(p.sigma() >= sigmaBounds[0]);
            BOOST_CHECK(p.sigma() <= sigmaBounds[1]);
            BOOST_CHECK(p.kappa() >= kappaBounds[0]);
            BOOST_CHECK(p.kappa() <= kappaBounds[1]);
            BOOST_CHECK(p.alpha() >= alphaBounds[0]);
            BOOST_CHECK(p.alpha() <= alphaBounds[1]);

            // these are sanity tests really, aren't they?  well they helped
            // me find a bug.
        }
    }
}


/*
 * This file is a part of
 *
 * ============================================
 * ###   Pteros molecular modeling library  ###
 * ============================================
 *
 * https://github.com/yesint/pteros
 *
 * (C) 2009-2020, Semen Yesylevskyy
 *
 * All works, which use Pteros, should cite the following papers:
 *
 *  1.  Semen O. Yesylevskyy, "Pteros 2.0: Evolution of the fast parallel
 *      molecular analysis library for C++ and python",
 *      Journal of Computational Chemistry, 2015, 36(19), 1480–1488.
 *      doi: 10.1002/jcc.23943.
 *
 *  2.  Semen O. Yesylevskyy, "Pteros: Fast and easy to use open-source C++
 *      library for molecular analysis",
 *      Journal of Computational Chemistry, 2012, 33(19), 1632–1636.
 *      doi: 10.1002/jcc.22989.
 *
 * This is free software distributed under Artistic License:
 * http://www.opensource.org/licenses/artistic-license-2.0.php
 *
*/

#include "pteros/extras/gnm.h"
#include "pteros/core/pteros_error.h"
#include "pteros/core/distance_search.h"
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <ctime>

using namespace std;
using namespace pteros;
using namespace Eigen;

void GNM::compute(Selection& sel, float cutoff){
    int i,j,k;

    cout << "Running GNM..."<<endl;

    N = sel.size();
    eigenvalues.resize(N-1);
    eigenvectors.resize(N,N-1);

    cout << "Constructing GNM Kirkgoff matrix. Cut-off: " << cutoff << " Size: " << N << endl;
    MatrixXf kirk(N,N); //Kirgoff matrix
    float d, time1, time2;

    kirk.fill(0.0);
    // Compute off-diagonal elements
    /*
    for(i=0;i<N-1;++i)
        for(j=i+1;j<N;++j){
            d = (sel.xyz(i)-sel.xyz(j)).norm();
            if(d<=cutoff){
                kirk(i,j)= -1.0;
                kirk(j,i)= -1.0;
            }
        }
    */

    vector<Eigen::Vector2i> bon;
    search_contacts(cutoff,sel,bon);
    for(int i=0; i<bon.size(); ++i){
        kirk(bon[i](0),bon[i](1)) = kirk(bon[i](1),bon[i](0)) = -1.0;
    }

    // Compute diagonal elements
    for(i=0;i<N;++i) kirk(i,i) = -kirk.col(i).sum();

    cout << "Computing eigenvectors...";
    time1 = clock();

    Eigen::SelfAdjointEigenSolver<MatrixXf> solver(kirk);
    eigenvalues = solver.eigenvalues();
    eigenvectors = solver.eigenvectors();
    // Vectors are already sorted, cool :)

    time2 = clock();
    cout << " Done in " << (time2-time1)/CLOCKS_PER_SEC << " s." << endl;
}

GNM::GNM(Selection& sel, float cutoff){
    compute(sel,cutoff);
}

void GNM::write_eigenvectors(string fname, int v1, int v2){
    if(v1<0 || v2>N-2 || v2<v1) throw Pteros_error("Can't write these eigenvectors!");
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");
    int i,j;
    for(i=0;i<N;++i){
        f << i << " ";
        for(j=v1;j<=v2;++j) f << eigenvectors(i,j) << " ";
        f << endl;
    }
    f.close();
}

void GNM::compute_c_matrix(bool normalize){
    int i,j,k;
    cout << "Calculating correlations..." << endl;
    c.resize(N,N);
    c.fill(0.0);
    for(k=1;k<N;++k) // For all eigenvectors except first, which is zero
        for(i=0;i<N;++i)
            for(j=i;j<N;++j)
                c(i,j) = c(i,j) + eigenvectors(i,k)*eigenvectors(j,k)/eigenvalues(k);

    // Set lower triangle of the matrix
    for(i=0;i<N-1;++i)
        for(j=i+1;j<N;++j)
            c(j,i) = c(i,j);

    if(normalize)
        for(i=0;i<N;++i)
            for(j=0;j<N;++j)
                c(i,j) = c(i,j)/sqrt(c(i,i)*c(j,j));
}

void GNM::compute_p_matrix(){
    if(!c.size()) compute_c_matrix();
    cout << "Calculating correlations of correlation patterns...";
    int i,j;
    float time1,time2;

    time1 = clock();

    p.resize(N,N);
    p.fill(0.0);

    // See if c-matrix is ready
    if(!c.cols()) compute_c_matrix();

    // Pre-compute m and s
    VectorXf m(N), s(N);

    for(i=0;i<N;++i){
        m(i) = c.col(i).sum()/float(N);
        s(i) = c.col(i).array().pow(2).sum()/float(N) - pow(m(i),2);
    }

    for(i=0;i<N;++i)
        for(j=i;j<N;++j){
            p(i,j) = (c.col(i).array() * c.col(j).array()).sum() /float(N) - m(i)*m(j);
            p(i,j) = p(i,j) / sqrt(s(i)*s(j));
            p(j,i) = p(i,j);
        }

    time2 = clock();
    cout << " Done in " << (time2-time1)/CLOCKS_PER_SEC << " s." << endl;
}

void GNM::write_c_matrix(string fname){
    // See if c-matrix is ready
    if(!c.cols()) compute_c_matrix();

    int i,j;
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");
    for(i=0;i<N;++i){
        for(j=0;j<N;++j)
            f << c(i,j) << " ";
        f << endl;
    }
    f.close();
}

void GNM::write_p_matrix(string fname){
    // See if p-matrix is ready
    if(!p.cols()) compute_p_matrix();

    int i,j;
    ofstream f(fname.c_str());
    if(!f) throw Pteros_error("Can't open file "+fname+" for writing!");

    for(i=0;i<N;++i){
        for(j=0;j<N;++j)
            f << p(i,j) << " ";
        f << endl;
    }
    f.close();
}


#pragma once

#include <Eigen/Dense>

#include "Data.h"

namespace ranger
{

template<class MatrixType = Eigen::MatrixXd>
class DataDense : public Data
{
  public:
    DataDense() = default;

    DataDense(MatrixType &data, Eigen::MatrixXd& data_extended, std::vector<std::string> variable_names, size_t num_rows,
              size_t num_cols);

    DataDense(const DataDense &) = delete;
    DataDense &operator=(const DataDense &) = delete;

    virtual ~DataDense() override = default;

    double get(size_t row, size_t col) const override
    {
        // Use permuted data for corrected impurity importance
        size_t col_permuted = col;
        if (col >= num_cols)
        {
            col = getUnpermutedVarID(col);
            row = getPermutedSampleID(row);
        }
        if (col >= data.cols())
            return data_extended.coeff(row,col - data.cols());
        else
            return data.coeff(row, col);
    }

    void reserveMemory() override
    {
    }

    void set(size_t col, size_t row, double value, bool &error) override
    {
        if (col >= data.cols())
            data_extended.coeffRef(row, col-data.cols()) = value;
        else
            data.coeffRef(row, col) = value;
    }

    // void filterRows(const std::vector<size_t>& f) {
    //     data = std::move(data(f,Eigen::all)).eval();
    //     data_extended = std::move(data(f,Eigen::all)).eval();
    //     num_rows = f.size();
    // }

  public:
    MatrixType data;
    Eigen::MatrixXd& data_extended;
};

} // namespace ranger


#ifndef STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_CHOLESKY_LPDF_HPP
#define STAN_MATH_PRIM_MAT_PROB_MULTI_NORMAL_CHOLESKY_LPDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/mat/meta/operands_and_partials.hpp>
#include <stan/math/prim/mat/fun/dot_self.hpp>
#include <stan/math/prim/mat/fun/log.hpp>
#include <stan/math/prim/mat/fun/mdivide_left_tri.hpp>
#include <stan/math/prim/mat/fun/transpose.hpp>
#include <stan/math/prim/mat/meta/vector_seq_view.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/max_size_mvt.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>
#include <stan/math/prim/scal/meta/return_type.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {
/**
 * The log of the multivariate normal density for the given y, mu, and
 * a Cholesky factor L of the variance matrix.
 * Sigma = LL', a square, semi-positive definite matrix.
 *
 * Analytic expressions taken from
 * http://qwone.com/~jason/writing/multivariateNormal.pdf
 * written by Jason D. M. Rennie.
 *
 * All expressions are adapted to avoid (most) inversions and maximal
 * reuse of intermediates.
 *
 * @param y A scalar vector
 * @param mu The mean vector of the multivariate normal distribution.
 * @param L The Cholesky decomposition of a variance matrix
 * of the multivariate normal distribution
 * @return The log of the multivariate normal density.
 * @throw std::domain_error if LL' is not square, not symmetric,
 * or not semi-positive definite.
 * @tparam T_y Type of scalar.
 * @tparam T_loc Type of location.
 * @tparam T_covar Type of scale.
 */
template <bool propto, typename T_y, typename T_loc, typename T_covar>
typename return_type<T_y, T_loc, T_covar>::type multi_normal_cholesky_lpdf(
    const T_y& y, const T_loc& mu, const T_covar& L) {
  static const char* function = "multi_normal_cholesky_lpdf";
  typedef typename scalar_type<T_covar>::type T_covar_elem;
  typedef typename return_type<T_y, T_loc, T_covar>::type T_return;
  typedef typename stan::partials_return_type<T_y, T_loc, T_covar>::type
      T_partials_return;
  typedef Eigen::Matrix<T_partials_return, Eigen::Dynamic, Eigen::Dynamic>
      matrix_partials_t;
  typedef Eigen::Matrix<T_partials_return, Eigen::Dynamic, 1> vector_partials_t;
  typedef Eigen::Matrix<T_partials_return, 1, Eigen::Dynamic>
      row_vector_partials_t;

  vector_seq_view<T_y> y_vec(y);
  vector_seq_view<T_loc> mu_vec(mu);
  const size_t size_vec = max_size_mvt(y, mu);

  const int size_y = y_vec[0].size();
  const int size_mu = mu_vec[0].size();
  if (likely(size_vec > 1)) {
    // check size consistency of all random variables y
    int size_y_old = size_y;
    for (size_t i = 1, size_ = length_mvt(y); i < size_; i++) {
      int size_y_new = y_vec[i].size();
      check_size_match(function,
                       "Size of one of the vectors of "
                       "the random variable",
                       size_y_new,
                       "Size of another vector of the "
                       "random variable",
                       size_y_old);
      size_y_old = size_y_new;
    }
    // check size consistency of all means mu
    int size_mu_old = size_mu;
    for (size_t i = 1, size_ = length_mvt(mu); i < size_; i++) {
      int size_mu_new = mu_vec[i].size();
      check_size_match(function,
                       "Size of one of the vectors of "
                       "the location variable",
                       size_mu_new,
                       "Size of another vector of the "
                       "location variable",
                       size_mu_old);
      size_mu_old = size_mu_new;
    }
  }

  check_size_match(function, "Size of random variable", size_y,
                   "size of location parameter", size_mu);
  check_size_match(function, "Size of random variable", size_y,
                   "rows of covariance parameter", L.rows());
  check_size_match(function, "Size of random variable", size_y,
                   "columns of covariance parameter", L.cols());

  for (size_t i = 0; i < size_vec; i++) {
    check_finite(function, "Location parameter", mu_vec[i]);
    check_not_nan(function, "Random variable", y_vec[i]);
  }

  if (unlikely(size_y == 0))
    return T_return(0.0);

  T_partials_return logp(0.0);
  operands_and_partials<T_y, T_loc, T_covar> ops_partials(y, mu, L);

  if (include_summand<propto>::value)
    logp += NEG_LOG_SQRT_TWO_PI * size_y * size_vec;

  const matrix_partials_t inv_L_dbl
      = mdivide_left_tri<Eigen::Lower>(value_of(L));

  if (include_summand<propto, T_y, T_loc, T_covar_elem>::value) {
    for (size_t i = 0; i < size_vec; i++) {
      vector_partials_t y_minus_mu_dbl(size_y);
      for (int j = 0; j < size_y; j++)
        y_minus_mu_dbl(j) = value_of(y_vec[i](j)) - value_of(mu_vec[i](j));

      const row_vector_partials_t half
          = (inv_L_dbl.template triangularView<Eigen::Lower>() * y_minus_mu_dbl)
                .transpose();
      const vector_partials_t scaled_diff
          = (half * inv_L_dbl.template triangularView<Eigen::Lower>())
                .transpose();

      logp -= 0.5 * dot_self(half);

      if (!is_constant_struct<T_y>::value) {
        for (int j = 0; j < size_y; j++)
          ops_partials.edge1_.partials_vec_[i](j) -= scaled_diff(j);
      }
      if (!is_constant_struct<T_loc>::value) {
        for (int j = 0; j < size_y; j++)
          ops_partials.edge2_.partials_vec_[i](j) += scaled_diff(j);
      }
      if (!is_constant_struct<T_covar>::value) {
        ops_partials.edge3_.partials_ += scaled_diff * half;
      }
    }
  }

  if (include_summand<propto, T_covar_elem>::value) {
    logp += inv_L_dbl.diagonal().array().log().sum() * size_vec;
    if (!is_constant_struct<T_covar>::value) {
      ops_partials.edge3_.partials_ -= size_vec * inv_L_dbl.transpose();
    }
  }

  return ops_partials.build(logp);
}

template <typename T_y, typename T_loc, typename T_covar>
inline typename return_type<T_y, T_loc, T_covar>::type
multi_normal_cholesky_lpdf(const T_y& y, const T_loc& mu, const T_covar& L) {
  return multi_normal_cholesky_lpdf<false>(y, mu, L);
}

}  // namespace math
}  // namespace stan
#endif


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */

#include <cstdio>
#include <cstring>
#include <cmath>
#include <cstdlib>
#include <Eigen/Eigen>
#include "fix_smd_move_triangulated_surface.h"
#include "atom.h"
#include "comm.h"
#include "force.h"
#include "neighbor.h"
#include "neigh_list.h"
#include "neigh_request.h"
#include "update.h"
#include "integrate.h"
#include "respa.h"
#include "memory.h"
#include "error.h"
#include "pair.h"
#include "domain.h"
#include "math_const.h"

using namespace Eigen;
using namespace LAMMPS_NS;
using namespace FixConst;
using namespace MathConst;
using namespace std;

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

FixSMDMoveTriSurf::FixSMDMoveTriSurf(LAMMPS *lmp, int narg, char **arg) :
                Fix(lmp, narg, arg) {

        if (atom->smd_flag != 1) {
                error->all(FLERR, "fix fix smd/move_tri_surf command requires atom_style smd");
        }

        if (narg < 3)
                error->all(FLERR, "Illegal number of arguments for fix fix smd/move_tri_surf command");

        rotateFlag = linearFlag = wiggleFlag = false;
        wiggle_direction  = 1.0;
        wiggle_max_travel = 0.0;

        int iarg = 3;

        if (comm->me == 0) {
                printf("\n>>========>>========>>========>>========>>========>>========>>========>>========\n");
                printf("fix fix smd/move_tri_surf is active for group: %s \n", arg[1]);
        }
        while (true) {

                if (iarg >= narg) {
                        break;
                }

                if (strcmp(arg[iarg], "*LINEAR") == 0) {
                        linearFlag = true;
                        if (comm->me == 0) {
                                printf("... will move surface in a linear fashion\n");
                        }

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected three floats for velocity following *LINEAR");
                        }
                        vx = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected three floats for velocity following *LINEAR");
                        }
                        vy = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected three floats for velocity following *LINEAR");
                        }
                        vz = force->numeric(FLERR, arg[iarg]);

                } else if (strcmp(arg[iarg], "*WIGGLE") == 0) {
                        wiggleFlag = true;
                        if (comm->me == 0) {
                                printf("... will move surface in wiggle fashion\n");
                        }

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
                        }
                        vx = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
                        }
                        vy = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
                        }
                        vz = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 4 floats following *WIGGLE : vx vy vz max_travel");
                        }
                        wiggle_max_travel = force->numeric(FLERR, arg[iarg]);

                } else if (strcmp(arg[iarg], "*ROTATE") == 0) {
                        rotateFlag = true;

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        origin(0) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        origin(1) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        origin(2) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        rotation_axis(0) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        rotation_axis(1) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        rotation_axis(2) = force->numeric(FLERR, arg[iarg]);

                        iarg++;
                        if (iarg == narg) {
                                error->all(FLERR, "expected 7 floats following *ROTATE: origin, rotation axis, and rotation period");
                        }
                        rotation_period = force->numeric(FLERR, arg[iarg]);

                        /*
                         * construct rotation matrix
                         */

                        u_cross(0, 0) = 0.0;
                        u_cross(0, 1) = -rotation_axis(2);
                        u_cross(0, 2) = rotation_axis(1);

                        u_cross(1, 0) = rotation_axis(2);
                        u_cross(1, 1) = 0.0;
                        u_cross(1, 2) = -rotation_axis(0);

                        u_cross(2, 0) = -rotation_axis(1);
                        u_cross(2, 1) = rotation_axis(0);
                        u_cross(2, 2) = 0.0;

                        uxu = rotation_axis * rotation_axis.transpose();

                        if (comm->me == 0) {
                                printf("will rotate with period %f\n", rotation_period);
                        }

                } else {
                        char msg[128];
                        snprintf(msg,128, "Illegal keyword for fix smd/move_tri_surf: %s\n", arg[iarg]);
                        error->all(FLERR, msg);
                }

                iarg++;

        }

        if (comm->me == 0) {
                printf(">>========>>========>>========>>========>>========>>========>>========>>========\n\n");
        }

        // set comm sizes needed by this fix
        comm_forward = 12;

        //atom->add_callback(0);
        //atom->add_callback(1);

        time_integrate = 1;
}

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

FixSMDMoveTriSurf::~FixSMDMoveTriSurf()
{
  // unregister callbacks to this fix from Atom class
  //atom->delete_callback(id,0);
  //atom->delete_callback(id,1);
}

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

int FixSMDMoveTriSurf::setmask() {
        int mask = 0;
        mask |= INITIAL_INTEGRATE;
        //mask |= PRE_EXCHANGE;
        return mask;
}

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

void FixSMDMoveTriSurf::init() {
        dtv = update->dt;
}

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

void FixSMDMoveTriSurf::initial_integrate(int /*vflag*/) {
        double **x = atom->x;
        double **x0 = atom->x0;
        double **v = atom->v;
        double **vest = atom->vest;
        double **smd_data_9 = atom->smd_data_9;
        tagint *mol = atom->molecule;

        int *mask = atom->mask;
        int nlocal = atom->nlocal;
        double phi;
        int i;
        Matrix3d eye, Rot;
        eye.setIdentity();

        Vector3d v1, v2, v3, n, point, rotated_point, R, vel;

        if (igroup == atom->firstgroup)
                nlocal = atom->nfirst;

        if (linearFlag) { // translate particles
                for (i = 0; i < nlocal; i++) {
                        if (mask[i] & groupbit) {

                                v[i][0] = vx;
                                v[i][1] = vy;
                                v[i][2] = vz;

                                vest[i][0] = vx;
                                vest[i][1] = vy;
                                vest[i][2] = vz;

                                x[i][0] += dtv * vx;
                                x[i][1] += dtv * vy;
                                x[i][2] += dtv * vz;

                                /*
                                 * if this is a triangle, move the vertices as well
                                 */

                                if (mol[i] >= 65535) {
                                        smd_data_9[i][0] += dtv * vx;
                                        smd_data_9[i][1] += dtv * vy;
                                        smd_data_9[i][2] += dtv * vz;

                                        smd_data_9[i][3] += dtv * vx;
                                        smd_data_9[i][4] += dtv * vy;
                                        smd_data_9[i][5] += dtv * vz;

                                        smd_data_9[i][6] += dtv * vx;
                                        smd_data_9[i][7] += dtv * vy;
                                        smd_data_9[i][8] += dtv * vz;
                                }

                        }
                }
        }

        if (wiggleFlag) { // wiggle particles forward and backward

                wiggle_travel += sqrt(vx * vx + vy * vy + vz * vz ) * dtv;
                double wiggle_vx = wiggle_direction * vx;
                double wiggle_vy = wiggle_direction * vy;
                double wiggle_vz = wiggle_direction * vz;

                //printf("wiggle vz is %f, wiggle_max_travel is %f, dir=%f\n", wiggle_vz, wiggle_max_travel, wiggle_direction);

                for (i = 0; i < nlocal; i++) {
                        if (mask[i] & groupbit) {

                                v[i][0] = wiggle_vx;
                                v[i][1] = wiggle_vy;
                                v[i][2] = wiggle_vz;

                                vest[i][0] = wiggle_vx;
                                vest[i][1] = wiggle_vy;
                                vest[i][2] = wiggle_vz;

                                x[i][0] += dtv * wiggle_vx;
                                x[i][1] += dtv * wiggle_vy;
                                x[i][2] += dtv * wiggle_vz;

                                /*
                                 * if this is a triangle, move the vertices as well
                                 */

                                if (mol[i] >= 65535) {
                                        smd_data_9[i][0] += dtv * wiggle_vx;
                                        smd_data_9[i][1] += dtv * wiggle_vy;
                                        smd_data_9[i][2] += dtv * wiggle_vz;

                                        smd_data_9[i][3] += dtv * wiggle_vx;
                                        smd_data_9[i][4] += dtv * wiggle_vy;
                                        smd_data_9[i][5] += dtv * wiggle_vz;

                                        smd_data_9[i][6] += dtv * wiggle_vx;
                                        smd_data_9[i][7] += dtv * wiggle_vy;
                                        smd_data_9[i][8] += dtv * wiggle_vz;
                                }

                        }
                }

                if (wiggle_travel >= wiggle_max_travel) {
                        wiggle_direction *= -1.0;
                        wiggle_travel = 0.0;
                }
        }

        if (rotateFlag) { // rotate particles
                Vector3d xnew, R_new, x_correct;

                /*
                 * rotation angle and matrix form of Rodrigues' rotation formula
                 */

                phi = MY_2PI * dtv / rotation_period;
                //printf("dt=%f, phi =%f, T=%f\n", dtv, phi, rotation_period);
                Rot = cos(phi) * eye + sin(phi) * u_cross + (1.0 - cos(phi)) * uxu;

                for (i = 0; i < nlocal; i++) {
                        if (mask[i] & groupbit) {

                                /*
                                 * generate vector R from origin to point which is to be rotated
                                 */
                                point << x[i][0], x[i][1], x[i][2];
                                R = point - origin;

                                /*
                                 * rotate vector and shift away from origin
                                 */
                                rotated_point = Rot * R + origin;

                                /*
                                 * determine velocity
                                 */
                                vel = (rotated_point - point) / update->dt;

                                /*
                                 * assign new velocities and coordinates
                                 */
                                v[i][0] = vel(0);
                                v[i][1] = vel(1);
                                v[i][2] = vel(2);

                                vest[i][0] = vel(0);
                                vest[i][1] = vel(1);
                                vest[i][2] = vel(2);

                                x[i][0] = rotated_point(0);
                                x[i][1] = rotated_point(1);
                                x[i][2] = rotated_point(2);

                                /*
                                 * if this is a triangle, rotate the vertices as well
                                 */

                                if (mol[i] >= 65535) {

                                        v1 << smd_data_9[i][0], smd_data_9[i][1], smd_data_9[i][2];
                                        R = v1 - origin;
                                        rotated_point = Rot * R + origin;
                                        vel = (rotated_point - v1) / update->dt;
                                        smd_data_9[i][0] = rotated_point(0);
                                        smd_data_9[i][1] = rotated_point(1);
                                        smd_data_9[i][2] = rotated_point(2);
                                        v1 = rotated_point;

                                        v2 << smd_data_9[i][3], smd_data_9[i][4], smd_data_9[i][5];
                                        R = v2 - origin;
                                        rotated_point = Rot * R + origin;
                                        vel = (rotated_point - v2) / update->dt;
                                        smd_data_9[i][3] = rotated_point(0);
                                        smd_data_9[i][4] = rotated_point(1);
                                        smd_data_9[i][5] = rotated_point(2);
                                        v2 = rotated_point;

                                        v3 << smd_data_9[i][6], smd_data_9[i][7], smd_data_9[i][8];
                                        R = v3 - origin;
                                        rotated_point = Rot * R + origin;
                                        vel = (rotated_point - v3) / update->dt;
                                        smd_data_9[i][6] = rotated_point(0);
                                        smd_data_9[i][7] = rotated_point(1);
                                        smd_data_9[i][8] = rotated_point(2);
                                        v3 = rotated_point;

                                        // recalculate triangle normal
                                        n = (v2 - v1).cross(v2 - v3);
                                        n /= n.norm();
                                        x0[i][0] = n(0);
                                        x0[i][1] = n(1);
                                        x0[i][2] = n(2);

                                }

                        }
                }
        }

        // we changed smd_data_9, x0. perform communication to ghosts
        comm->forward_comm_fix(this);

}

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

void FixSMDMoveTriSurf::reset_dt() {
        dtv = update->dt;
}

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

int FixSMDMoveTriSurf::pack_forward_comm(int n, int *list, double *buf, int /*pbc_flag*/, int * /*pbc*/) {
        int i, j, m;
        double **x0 = atom->x0;
        double **smd_data_9 = atom->smd_data_9;

        //printf("in FixSMDIntegrateTlsph::pack_forward_comm\n");
        m = 0;
        for (i = 0; i < n; i++) {
                j = list[i];
                buf[m++] = x0[j][0];
                buf[m++] = x0[j][1];
                buf[m++] = x0[j][2];

                buf[m++] = smd_data_9[j][0];
                buf[m++] = smd_data_9[j][1];
                buf[m++] = smd_data_9[j][2];
                buf[m++] = smd_data_9[j][3];
                buf[m++] = smd_data_9[j][4];
                buf[m++] = smd_data_9[j][5];
                buf[m++] = smd_data_9[j][6];
                buf[m++] = smd_data_9[j][7];
                buf[m++] = smd_data_9[j][8];

        }
        return m;
}

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

void FixSMDMoveTriSurf::unpack_forward_comm(int n, int first, double *buf) {
        int i, m, last;
        double **x0 = atom->x0;
        double **smd_data_9 = atom->smd_data_9;

        //printf("in FixSMDMoveTriSurf::unpack_forward_comm\n");
        m = 0;
        last = first + n;
        for (i = first; i < last; i++) {
                x0[i][0] = buf[m++];
                x0[i][1] = buf[m++];
                x0[i][2] = buf[m++];

                smd_data_9[i][0] = buf[m++];
                smd_data_9[i][1] = buf[m++];
                smd_data_9[i][2] = buf[m++];
                smd_data_9[i][3] = buf[m++];
                smd_data_9[i][4] = buf[m++];
                smd_data_9[i][5] = buf[m++];
                smd_data_9[i][6] = buf[m++];
                smd_data_9[i][7] = buf[m++];
                smd_data_9[i][8] = buf[m++];
        }
}


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver 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 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver 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 PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <iostream>

#include <Eigen/Core>

#include "bi_operators/CollocationIntegrator.hpp"
#include "green/DerivativeTypes.hpp"
#include "cavity/GePolCavity.hpp"
#include "utils/Molecule.hpp"
#include "green/Vacuum.hpp"
#include "green/UniformDielectric.hpp"
#include "solver/IEFSolver.hpp"
#include "TestingMolecules.hpp"

SCENARIO("Test solver for the IEFPCM for a point charge in different Abelian point groups", "[solver][iefpcm][iefpcm_symmetry]")
{
    GIVEN("An isotropic environment and a point charge")
    {
        double permittivity = 78.39;
        Vacuum<> gf_i;
        UniformDielectric<> gf_o(permittivity);
        bool symm = true;

        double charge = 8.0;
        double totalASC = - charge * (permittivity - 1) / permittivity;

        /*! \class IEFSolver
         *  \test \b pointChargeGePolC1 tests IEFSolver using a point charge with a GePol cavity in C1 symmetry
         */
        WHEN("the point group is C1")
        {
            Molecule point = dummy<0>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "C1");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolC2 tests IEFSolver using a point charge with a GePol cavity in C2 symmetry
         */
        WHEN("the point group is C2")
        {
            Molecule point = dummy<1>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "C2");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolCs tests IEFSolver using a point charge with a GePol cavity in Cs symmetry
         */
        WHEN("the point group is Cs")
        {
            Molecule point = dummy<2>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "Cs");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolCi tests IEFSolver using a point charge with a GePol cavity in Ci symmetry
         */
        WHEN("the point group is Ci")
        {
            Molecule point = dummy<3>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "Ci");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolD2 tests IEFSolver using a point charge with a GePol cavity in D2 symmetry
         */
        WHEN("the point group is D2")
        {
            Molecule point = dummy<4>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "D2");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolC2v tests IEFSolver using a point charge with a GePol cavity in C2v symmetry
         */
        WHEN("the point group is C2v")
        {
            Molecule point = dummy<5>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "C2v");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolC2h tests IEFSolver using a point charge with a GePol cavity in C2h symmetry
         */
        WHEN("the point group is C2h")
        {
            Molecule point = dummy<6>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "C2h");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }

        /*! \class IEFSolver
         *  \test \b pointChargeGePolD2h tests IEFSolver using a point charge with a GePol cavity in D2h symmetry
         */
        WHEN("the point group is D2h")
        {
            Molecule point = dummy<7>(2.929075493);
            double area = 0.4;
            double probeRadius = 0.0;
            double minRadius = 100.0;
            GePolCavity cavity(point, area, probeRadius, minRadius, "D2h");

            int size = cavity.size();
            Eigen::VectorXd fake_mep = computeMEP(cavity.elements(), charge);
            for (int i = 0; i < size; ++i) {
                Eigen::Vector3d center = cavity.elementCenter(i);
                double distance = center.norm();
                fake_mep(i) = charge / distance;
            }

            IEFSolver solver(symm);
            solver.buildSystemMatrix(cavity, gf_i, gf_o);
            THEN("the total apparent surface charge is")
            {
                int irr_size = cavity.irreducible_size();
                Eigen::VectorXd fake_asc = Eigen::VectorXd::Zero(size);
                fake_asc = solver.computeCharge(fake_mep);
                int nr_irrep = cavity.pointGroup().nrIrrep();
                double totalFakeASC = fake_asc.sum() * nr_irrep;
                CAPTURE(totalASC - totalFakeASC);
                REQUIRE(totalASC == Approx(totalFakeASC).epsilon(1.0e-03));
            }
        }
    }
}


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

#include <boost/simd/ieee/functions/saturate.hpp>
#include <boost/simd/include/functions/simd/splat.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/sdk/meta/scalar_of.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT_IF          ( saturate_, tag::cpu_
                                       , (A0)(T)
                                       , (is_same<typename meta::scalar_of<typename T::type>::type, typename meta::scalar_of<A0>::type>)
                                       , (generic_< arithmetic_<A0> >)
                                         (target_< generic_<arithmetic_<T> > >)
                                       )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      return a0;
    }
  };

  // any->any, only works if target is strictly smaller
  // FIXME: is Valmax<target_t>/Valmin<target_t> representable with A0 if floating-point?
  BOOST_DISPATCH_IMPLEMENT          ( saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (generic_< unspecified_<A0> >)
                                      (target_< generic_< arithmetic_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<typename T::type>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      const A0 vma = splat<A0>(Inf<target_t>());
      const A0 vmi = splat<A0>(Minf<target_t>());
      return min(vma, max(vmi, a0));
    }
  };

  // unsigned->any, only works if target is smaller or equal
  // FIXME: is Valmax<target_t> representable with A0 if floating-point?
  BOOST_DISPATCH_IMPLEMENT          ( saturate_, tag::cpu_
                                    , (A0)(T)
                                    , (generic_<unsigned_<A0> >)
                                      (target_< generic_< arithmetic_<T> > >)
                                    )
  {
    typedef A0 result_type;
    typedef typename meta::scalar_of<typename T::type>::type target_t;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      const A0 vma = splat<A0>(Valmax<target_t>());
      return min(vma, a0);
    }
  };

  // if target is greater or equal and signed->unsigned, just need to check if negative
  BOOST_DISPATCH_IMPLEMENT_IF          ( saturate_, tag::cpu_
                                       , (A0)(T)
                                       , (mpl::greater_equal< mpl::sizeof_<typename meta::scalar_of<typename T::type>::type>, mpl::sizeof_<typename meta::scalar_of<A0>::type> >)
                                       , (generic_< int_<A0> >)
                                         (target_< generic_< uint_<T> > >)
                                       )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      return max(Zero<A0>(), a0);
    }
  };

  // if target is strictly greater, nothing to saturate
  BOOST_DISPATCH_IMPLEMENT_IF          ( saturate_, tag::cpu_
                                       , (A0)(T)
                                       , (mpl::greater< mpl::sizeof_<typename meta::scalar_of<typename T::type>::type>, mpl::sizeof_<typename meta::scalar_of<A0>::type> >)
                                       , (generic_< integer_<A0> >)
                                         (target_< generic_< arithmetic_<T> > >)
                                       )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& a0, T const& ) const
    {
      return a0;
    }
  };
} } }

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "../Lstm.hpp"

#include <boost/test/data/test_case.hpp>

BOOST_AUTO_TEST_SUITE(LstmTests)

using namespace armnn_driver;

BOOST_DATA_TEST_CASE(LstmNoCifgNoPeepholeNoProjectionTest, COMPUTE_DEVICES)
{
    LstmNoCifgNoPeepholeNoProjection<hal_1_2::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmCifgPeepholeNoProjectionTest, COMPUTE_DEVICES)
{
    LstmCifgPeepholeNoProjection<hal_1_2::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmNoCifgPeepholeProjectionTest, COMPUTE_DEVICES)
{
    LstmNoCifgPeepholeProjection<hal_1_2::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmCifgPeepholeNoProjectionBatch2Test, COMPUTE_DEVICES)
{
    LstmCifgPeepholeNoProjectionBatch2<hal_1_2::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmNoCifgPeepholeProjectionNoClippingLayerNormTest, COMPUTE_DEVICES)
{
    LstmNoCifgPeepholeProjectionNoClippingLayerNorm<hal_1_2::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmCifgPeepholeProjectionNoClippingLayerNormTest, COMPUTE_DEVICES)
{
    LstmCifgPeepholeProjectionNoClippingLayerNorm<hal_1_2::HalPolicy>(sample);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once
#include <string>
#include <atomic>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <petuum_ps_common/include/petuum_ps.hpp>

#include "matrix_loader.hpp"

namespace sparsecoding {
class SCEngine {
    public:
        SCEngine();
        ~SCEngine();
        void Start();
    private:
        std::atomic<int> thread_counter_;

        // petuum parameters
        int client_id_, num_clients_, num_worker_threads_;
        float maximum_running_time_;

        // objective function parameters
        int dictionary_size_;
        float C_, lambda_;

        // minibatch and evaluate parameters
        int num_epochs_, minibatch_size_, num_eval_minibatch_,
            num_iter_S_per_minibatch_,
            num_eval_samples_, num_eval_per_client_;

        // optimization parameters
        float init_step_size_B_, step_size_offset_B_, step_size_pow_B_,
              init_step_size_S_, step_size_offset_S_, step_size_pow_S_,
              init_S_low_, init_S_high_, init_B_low_, init_B_high_;

        // input and output
        std::string data_file_, input_data_format_, output_path_,
            output_data_format_, cache_path_;
        int is_partitioned_;
        int load_cache_;

        // matrix loader for data X and dictionary S
        MatrixLoader<float> X_matrix_loader_, S_matrix_loader_;

        // timer
        boost::posix_time::ptime initT_;

        // Save results to disk
        void SaveResults(int thread_id, petuum::Table<float> & B_table,
                petuum::Table<float> & loss_table);

        // Init B with random values and normalize elements to have unit norm
        void InitRand(int thread_id, petuum::Table<float> & B_table);

        // Load B and S from disk
        void LoadCache(int thread_id, petuum::Table<float> & B_table);
};
}; // namespace sparsecoding


#include "source/newtonian/two_dimensional/hdsim2d.hpp"
#include "source/newtonian/common/hllc.hpp"
#include "source/newtonian/common/ideal_gas.hpp"
#include "source/newtonian/two_dimensional/amr.hpp"
#include "source/newtonian/two_dimensional/modular_flux_calculator.hpp"
#include "source/newtonian/two_dimensional/simple_cell_updater.hpp"
#include "source/newtonian/two_dimensional/simple_cfl.hpp"
#include "source/newtonian/two_dimensional/simple_extensive_updater.hpp"
#include "source/newtonian/two_dimensional/point_motions/lagrangian.hpp"
#include "source/newtonian/two_dimensional/point_motions/round_cells.hpp"
#include "source/newtonian/two_dimensional/interpolations/LinearGaussImproved.hpp"
#include "source/tessellation/VoronoiMesh.hpp"
#include "source/misc/mesh_generator.hpp"
#include "source/newtonian/two_dimensional/periodic_edge_velocities.hpp"
#include "source/newtonian/two_dimensional/geometric_outer_boundaries/PeriodicBox.hpp"
#include "source/newtonian/two_dimensional/ghost_point_generators/PeriodicGhostGenerator.hpp"
#include "source/newtonian/two_dimensional/source_terms/zero_force.hpp"
#include "source/newtonian/two_dimensional/hdf5_diagnostics.hpp"
#include <boost/random/uniform_int_distribution.hpp>
#include "source/misc/simple_io.hpp"

namespace {
class ConserveRefine : public CellsToRefine
{
public:
	vector<size_t> ToRefine(Tessellation const& tess, vector<ComputationalCell> const& /*cells*/, double time,
	TracerStickerNames const& /*ts*/)const
	{
		boost::random::mt19937_64 gen(static_cast<uint64_t>(time*10000));
		boost::random::uniform_int_distribution<> dist(0, tess.GetPointNo()-1);
		vector<size_t> res(10);
		for (size_t i = 0; i < res.size(); ++i)
		  res[i] = static_cast<size_t>(dist(gen));
		return res;
	}
};

class ConserveRemove : public CellsToRemove
{
public:
	std::pair<vector<size_t>, vector<double> > ToRemove(Tessellation const& tess, vector<ComputationalCell> const& /*cells*/, double time,
	TracerStickerNames const& /*ts*/)const
	{
		boost::random::mt19937_64 gen(static_cast<uint64_t>(time * 11000));
		boost::random::uniform_int_distribution<> dist(0,tess.GetPointNo()-1);
		std::pair<vector<size_t>, vector<double> > res(vector<size_t>(10), vector<double>(10));
		for (size_t i = 0; i < res.first.size(); ++i)
		{
		  res.first[i] = static_cast<size_t>(dist(gen));
			res.second[i] = static_cast<double>(dist(gen));
		}
		return res;
	}
};

double TotalMass(hdsim const& sim)
{
	double res = 0;
	for (int i = 0; i < sim.getTessellation().GetPointNo(); ++i)
		res += sim.getTessellation().GetVolume(i)*sim.getAllCells()[static_cast<size_t>(i)].density;
	return res;
}

vector<ComputationalCell> calc_cells(size_t n)
{
	vector<ComputationalCell> res(n);
	for (size_t i = 0; i < n; ++i)
	{
		res[i].density = 1;
		res[i].pressure = 1;
		res[i].velocity = Vector2D(1, 0);
	}
	return res;
}
}

int main(void)
{
	int np = 30;
	vector<Vector2D> points = cartesian_mesh(np, np, Vector2D(-1, -1), Vector2D(1, 1));
	PeriodicBox outer(Vector2D(-1, -1), Vector2D(1, 1));
	VoronoiMesh tess(points, outer);
	vector<ComputationalCell> cells = calc_cells(static_cast<size_t>(tess.GetPointNo()));
	Hllc rs;
	IdealGas eos(5. / 3.);
	PeriodicGhostGenerator ghost;
	PeriodicEdgeVelocities vedge;
	LinearGaussImproved interp(eos, ghost);
	SimpleCellUpdater cu;
	SimpleExtensiveUpdater eu;
	SimpleCFL tsf(0.3);
	ModularFluxCalculator fc(interp, rs);
	Lagrangian pm;
	SlabSymmetry pg;
	ZeroForce force;

	hdsim sim(tess, outer, pg, cells, eos, pm, vedge, force, tsf, fc, eu, cu);

	ConserveRefine refine;
	ConserveRemove remove;
	ConservativeAMR amr(refine, remove,true, &interp);

	for (size_t i = 0; i < 50; ++i)
	{
		sim.TimeAdvance2Heun();
		amr(sim);
	}

	write_number(TotalMass(sim), "mass.txt",9);
	write_snapshot_to_hdf5(sim, "final.h5");

	return 0;
}