// Copyright (C) 2011-2012 by the Bem++ Authors
//
// 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.

#ifndef fiber_default_local_assembler_for_potential_operators_on_surfaces_hpp
#define fiber_default_local_assembler_for_potential_operators_on_surfaces_hpp

#include "../common/common.hpp"

#include "local_assembler_for_potential_operators.hpp"

#include "_2d_array.hpp"
#include "accuracy_options.hpp"
#include "default_local_assembler_for_operators_on_surfaces_utilities.hpp"
#include "element_pair_topology.hpp"
#include "numerical_quadrature.hpp"
#include "parallelization_options.hpp"
#include "shared_ptr.hpp"
#include "kernel_trial_integrator.hpp"
#include "verbosity_level.hpp"

#include <boost/static_assert.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <tbb/concurrent_unordered_map.h>
#include <cstring>
#include <climits>
#include <set>
#include <utility>
#include <vector>

namespace Fiber {

/** \cond FORWARD_DECL */
class OpenClHandler;
template <typename CoordinateType> class CollectionOfShapesetTransformations;
template <typename ValueType> class CollectionOfKernels;
template <typename BasisFunctionType, typename KernelType, typename ResultType>
class KernelTrialIntegral;
template <typename CoordinateType> class RawGridGeometry;
template <typename BasisFunctionType>
class QuadratureDescriptorSelectorForPotentialOperators;
template <typename CoordinateType> class SingleQuadratureRuleFamily;
/** \endcond */

template <typename BasisFunctionType, typename KernelType, typename ResultType,
          typename GeometryFactory>
class DefaultLocalAssemblerForPotentialOperatorsOnSurfaces
    : public LocalAssemblerForPotentialOperators<ResultType> {
public:
  typedef typename ScalarTraits<ResultType>::RealType CoordinateType;

  DefaultLocalAssemblerForPotentialOperatorsOnSurfaces(
      const arma::Mat<CoordinateType> &points,
      const shared_ptr<const GeometryFactory> &geometryFactory,
      const shared_ptr<const RawGridGeometry<CoordinateType>> &rawGeometry,
      const shared_ptr<const std::vector<const Shapeset<BasisFunctionType> *>> &
          trialShapesets,
      const shared_ptr<const CollectionOfKernels<KernelType>> &kernels,
      const shared_ptr<const CollectionOfShapesetTransformations<
          CoordinateType>> &trialTransformations,
      const shared_ptr<const KernelTrialIntegral<BasisFunctionType, KernelType,
                                                 ResultType>> &integral,
      const ParallelizationOptions &parallelizationOptions,
      VerbosityLevel::Level verbosityLevel,
      const shared_ptr<const QuadratureDescriptorSelectorForPotentialOperators<
          BasisFunctionType>> &quadDescSelector,
      const shared_ptr<const SingleQuadratureRuleFamily<CoordinateType>> &
          quadRuleFamily);
  virtual ~DefaultLocalAssemblerForPotentialOperatorsOnSurfaces();

  virtual void
  evaluateLocalContributions(const std::vector<int> &pointIndices,
                             int trialElementIndex,
                             LocalDofIndex localTrialDofIndex,
                             std::vector<arma::Mat<ResultType>> &result,
                             CoordinateType nominalDistance = -1.);

  virtual void
  evaluateLocalContributions(int pointIndex, int componentIndex,
                             const std::vector<int> &trialElementIndices,
                             std::vector<arma::Mat<ResultType>> &result,
                             CoordinateType nominalDistance = -1.);

  virtual void
  evaluateLocalContributions(const std::vector<int> &pointIndices,
                             const std::vector<int> &trialElementIndices,
                             Fiber::_2dArray<arma::Mat<ResultType>> &result,
                             CoordinateType nominalDistance = -1.);

  virtual int resultDimension() const;

  virtual CoordinateType estimateRelativeScale(CoordinateType minDist) const;

private:
  /** \cond PRIVATE */
  typedef KernelTrialIntegrator<BasisFunctionType, KernelType, ResultType>
  Integrator;
  typedef DefaultLocalAssemblerForOperatorsOnSurfacesUtilities<
      BasisFunctionType> Utilities;

  const Integrator &selectIntegrator(int testElementIndex,
                                     int trialElementIndex,
                                     CoordinateType nominalDistance = -1.);

  int order(int pointIndex, int trialElementIndex,
            CoordinateType nominalDistance) const;

  const Integrator &getIntegrator(const SingleQuadratureDescriptor &index);

  CoordinateType pointElementDistanceSquared(int pointIndex,
                                             int trialElementIndex) const;

  void precalculateElementSizesAndCenters();

private:
  arma::Mat<CoordinateType> m_points;
  shared_ptr<const GeometryFactory> m_geometryFactory;
  shared_ptr<const RawGridGeometry<CoordinateType>> m_rawGeometry;
  shared_ptr<const std::vector<const Shapeset<BasisFunctionType> *>>
  m_trialShapesets;
  shared_ptr<const CollectionOfKernels<KernelType>> m_kernels;
  shared_ptr<const CollectionOfShapesetTransformations<CoordinateType>>
  m_trialTransformations;
  shared_ptr<const KernelTrialIntegral<BasisFunctionType, KernelType,
                                       ResultType>> m_integral;
  ParallelizationOptions m_parallelizationOptions;
  VerbosityLevel::Level m_verbosityLevel;
  shared_ptr<const QuadratureDescriptorSelectorForPotentialOperators<
      BasisFunctionType>> m_quadDescSelector;
  shared_ptr<const SingleQuadratureRuleFamily<CoordinateType>> m_quadRuleFamily;

  typedef tbb::concurrent_unordered_map<SingleQuadratureDescriptor,
                                        Integrator *> IntegratorMap;
  IntegratorMap m_kernelTrialIntegrators;

  enum {
    INVALID_INDEX = INT_MAX
  };
  /** \endcond */
};

} // namespace Fiber

#include "default_local_assembler_for_potential_operators_on_surfaces_imp.hpp"

#endif


#ifndef __TVC_HH__
#define __TVC_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the TVC Module On Board)
LIBRARY DEPENDENCY:
      ((../src/Tvc.cpp))
*******************************************************************************/
#include <armadillo>
#include <functional>
#include <tuple>
#include <vector>
#include "Component.hh"
#include "Module.hh"
#include "aux.hh"
#include "global_constants.hh"
#include "icf_trx_ctrl.h"

class TVC : public Dynamics {
  TRICK_INTERFACE(TVC);

 public:
  TVC(Data_exchang &input);
  TVC(const TVC &other);

  TVC &operator=(const TVC &other);

  virtual void init();

  virtual void algorithm(double int_step);
  void Allocate_ENG(int NumEng, std::vector<ENG *> &Eng_list);

  enum TVC_TYPE {
    NO_TVC = 0,
    NO_DYNAMIC_TVC,
    SECON_ORDER_TVC,          // TVC Second order dynamics with rate limiting
    ONLINE_SECOND_ORDER_TVC,  // same as 2nd order but with on-line TVC gain
    S2_TVC,
    S3_TVC
  };

  enum TVC_TYPE get_mtvc();
  void set_mtvc(enum TVC_TYPE);
  void set_S2_TVC();
  void set_S3_TVC();

  std::function<double()> grab_theta_a_cmd;
  std::function<double()> grab_theta_b_cmd;
  std::function<double()> grab_theta_c_cmd;
  std::function<double()> grab_theta_d_cmd;
  std::vector<ENG *> S2_Eng_list;
  std::vector<ENG *> S3_Eng_list;

 private:
  /* Internal Initializers */
  void default_data();

  /* State */
  enum TVC_TYPE mtvc; /* *o  (--)      see TVC_TYPE */

  /* Constants */
  double gtvc; /* *o  (--)    TVC nozzle deflection gain n*/

  double theta_a_cmd;  /* *o (r)  Store first actuator command */
  double theta_b_cmd;  /* *o (r)  Store second actuator command */
  double theta_c_cmd;  /* *o (r)  Store third actuator command */
  double theta_d_cmd;  /* *o (r)  Store fourth actuator command */
  double ActOutput1;   /* *o (r)  Store first actuator output value for trick data record */
  double ActOutput2;   /* *o (r)  Store second actuator output value for trick data record */
  double ActOutput3;   /* *o (r)  Store third actuator output value for trick data record */
  double ActOutput4;   /* *o (r)  Store fourth actuator output value for trick data record */

  VECTOR(Q_TVC, 6);    /* *o (--) External force generated by the rocket engines */
};

#endif  // __TVC_HH__


#include "AssemblyOptions.hpp"
using namespace ChanZuckerberg;
using namespace shasta;

// Boost libraries.
#include <boost/tokenizer.hpp>

// Standard library.
#include"stdexcept.hpp"

// Add the AssemblyOptions to a Boost option description object.
void AssemblyOptions::add(boost::program_options::options_description& options)
{
    using boost::program_options::value;

    options.add_options()

        ("Reads.minReadLength",
        value<int>(&Reads.minReadLength)->
        default_value(10000),
        "Read length cutoff.")

        ("Reads.palindromicReads.maxSkip",
        value<int>(&Reads.palindromicReads.maxSkip)->
        default_value(100),
        "Used for palindromic read detection.")

        ("Reads.palindromicReads.maxMarkerFrequency",
        value<int>(&Reads.palindromicReads.maxMarkerFrequency)->
        default_value(10),
        "Used for palindromic read detection.")

        ("Reads.palindromicReads.alignedFractionThreshold",
        value<double>(&Reads.palindromicReads.alignedFractionThreshold)->
        default_value(0.1, "0.1"),
        "Used for palindromic read detection.")

        ("Reads.palindromicReads.nearDiagonalFractionThreshold",
        value<double>(&Reads.palindromicReads.nearDiagonalFractionThreshold)->
        default_value(0.1, "0.1"),
        "Used for palindromic read detection.")

        ("Reads.palindromicReads.deltaThreshold",
         value<int>(&Reads.palindromicReads.deltaThreshold)->
         default_value(100),
         "Used for palindromic read detection.")

        ("Kmers.k",
        value<int>(&Kmers.k)->
        default_value(10),
        "Length of marker k-mers (in run-length space).")

        ("Kmers.probability",
        value<double>(&Kmers.probability)->
        default_value(0.1, "0.1"),
        "Probability that a k-mer is used as a marker.")

        ("MinHash.m",
        value<int>(&MinHash.m)->
        default_value(4),
        "The number of consecutive markers that define a MinHash/LowHash feature.")

        ("MinHash.hashFraction",
        value<double>(&MinHash.hashFraction)->
        default_value(0.01, "0.01"),
        "Defines how low a hash has to be to be used with the LowHash algorithm.")

        ("MinHash.minHashIterationCount",
        value<int>(&MinHash.minHashIterationCount)->
        default_value(10),
        "The number of MinHash/LowHash iterations.")

        ("MinHash.maxBucketSize",
        value<int>(&MinHash.maxBucketSize)->
        default_value(10),
        "The maximum bucket size to be used by the MinHash/LowHash algoritm.")

        ("MinHash.minFrequency",
        value<int>(&MinHash.minFrequency)->
        default_value(2),
        "The minimum number of times a pair of reads must be found by the MinHash/LowHash algorithm "
        "in order to be considered a candidate alignment.")

        ("Align.maxSkip",
        value<int>(&Align.maxSkip)->
        default_value(30),
        "The maximum number of markers that an alignment is allowed to skip.")

        ("Align.maxTrim",
        value<int>(&Align.maxTrim)->
        default_value(30),
        "The maximum number of trim markers tolerated at the beginning and end of an alignment.")

        ("Align.maxMarkerFrequency",
        value<int>(&Align.maxMarkerFrequency)->
        default_value(10),
        "Marker frequency threshold.")

        ("Align.minAlignedMarkerCount",
        value<int>(&Align.minAlignedMarkerCount)->
        default_value(100),
        "The minimum number of aligned markers for an alignment to be used.")

        ("ReadGraph.maxAlignmentCount",
        value<int>(&ReadGraph.maxAlignmentCount)->
        default_value(6),
        "The maximum alignments to be kept for each read.")

        ("ReadGraph.minComponentSize",
        value<int>(&ReadGraph.minComponentSize)->
        default_value(100),
        "The minimum size (number of oriented reads) of a connected component "
        "of the read graph to be kept.")

        ("ReadGraph.maxChimericReadDistance",
        value<int>(&ReadGraph.maxChimericReadDistance)->
        default_value(2),
        "Used for chimeric read detection.")

        ("MarkerGraph.minCoverage",
        value<int>(&MarkerGraph.minCoverage)->
        default_value(10),
        "Minimum number of markers for a marker graph vertex.")

        ("MarkerGraph.maxCoverage",
        value<int>(&MarkerGraph.maxCoverage)->
        default_value(100),
        "Maximum number of markers for a marker graph vertex.")

        ("MarkerGraph.lowCoverageThreshold",
        value<int>(&MarkerGraph.lowCoverageThreshold)->
        default_value(0),
        "Used during approximate transitive reduction.")

        ("MarkerGraph.highCoverageThreshold",
        value<int>(&MarkerGraph.highCoverageThreshold)->
        default_value(256),
        "Used during approximate transitive reduction.")

        ("MarkerGraph.maxDistance",
        value<int>(&MarkerGraph.maxDistance)->
        default_value(30),
        "Used during approximate transitive reduction.")

        ("MarkerGraph.edgeMarkerSkipThreshold",
        value<int>(&MarkerGraph.edgeMarkerSkipThreshold)->
        default_value(100),
        "Used during approximate transitive reduction.")

        ("MarkerGraph.pruneIterationCount",
        value<int>(&MarkerGraph.pruneIterationCount)->
        default_value(6),
        "Number of prune iterations.")

        ("MarkerGraph.simplifyMaxLength",
        value<string>(&MarkerGraph.simplifyMaxLength)->
        default_value("10,100,1000"),
        "Maximum lengths (in markers) used at each iteration of simplifyMarkerGraph.")

        ("Assembly.markerGraphEdgeLengthThresholdForConsensus",
        value<int>(&Assembly.markerGraphEdgeLengthThresholdForConsensus)->
        default_value(1000),
        "Controls assembly of long marker graph edges.")

        ("Assembly.consensusCaller",
        value<string>(&Assembly.consensusCaller)->
        default_value("SimpleConsensusCaller"),
        "Selects the consensus caller for repeat counts.\n"
        "SimpleConsensusCaller is the only choice currently\n"
        "supported by the Shasta executable.\n"
        "Other choices are available with the Shasta library.")

        ("Assembly.useMarginPhase",
        value<string>(&Assembly.useMarginPhase)->
        default_value("False"),
        "Used to turn on margin phase.")

        ("Assembly.storeCoverageData",
        value<string>(&Assembly.storeCoverageData)->
        default_value("False"),
        "Used to request storing coverage data.")
        ;
}

void AssemblyOptions::ReadsOptions::PalindromicReadOptions::write(ostream& s) const
{
    s << "palindromicReads.maxSkip = " << maxSkip << "\n";
    s << "palindromicReads.maxMarkerFrequency = " << maxMarkerFrequency << "\n";
    s << "palindromicReads.alignedFractionThreshold = " << alignedFractionThreshold << "\n";
    s << "palindromicReads.nearDiagonalFractionThreshold = " << nearDiagonalFractionThreshold << "\n";
    s << "palindromicReads.deltaThreshold = " << deltaThreshold << "\n";
}

void AssemblyOptions::ReadsOptions::write(ostream& s) const
{
    s << "[Reads]\n";
    s << "minReadLength = " << minReadLength << "\n";
    palindromicReads.write(s);
}

void AssemblyOptions::KmersOptions::write(ostream& s) const
{
    s << "[Kmers]\n";
    s << "k = " << k << "\n";
    s << "probability = " << probability << "\n";
}

void AssemblyOptions::MinHashOptions::write(ostream& s) const
{
    s << "[MinHash]\n";
    s << "m = " << m << "\n";
    s << "hashFraction = " << hashFraction << "\n";
    s << "minHashIterationCount = " << minHashIterationCount << "\n";
    s << "maxBucketSize = " << maxBucketSize << "\n";
    s << "minFrequency = " << minFrequency << "\n";
}

void AssemblyOptions::AlignOptions::write(ostream& s) const
{
    s << "[Align]\n";
    s << "maxSkip = " << maxSkip << "\n";
    s << "maxTrim = " << maxTrim << "\n";
    s << "maxMarkerFrequency = " << maxMarkerFrequency << "\n";
    s << "minAlignedMarkerCount = " << minAlignedMarkerCount << "\n";
}

void AssemblyOptions::ReadGraphOptions::write(ostream& s) const
{
    s << "[ReadGraph]\n";
    s << "maxAlignmentCount = " << maxAlignmentCount << "\n";
    s << "minComponentSize = " << minComponentSize << "\n";
    s << "maxChimericReadDistance = " << maxChimericReadDistance << "\n";
}

void AssemblyOptions::MarkerGraphOptions::write(ostream& s) const
{
    s << "[MarkerGraph]\n";
    s << "minCoverage = " << minCoverage << "\n";
    s << "maxCoverage = " << maxCoverage << "\n";
    s << "lowCoverageThreshold = " << lowCoverageThreshold << "\n";
    s << "highCoverageThreshold = " << highCoverageThreshold << "\n";
    s << "maxDistance = " << maxDistance << "\n";
    s << "edgeMarkerSkipThreshold = " << edgeMarkerSkipThreshold << "\n";
    s << "pruneIterationCount = " << pruneIterationCount << "\n";
    s << "simplifyMaxLength = " << simplifyMaxLength << "\n";
}

void AssemblyOptions::AssemblyOptionsInner::write(ostream& s) const
{
    s << "[Assembly]\n";
    s << "markerGraphEdgeLengthThresholdForConsensus = " <<
        markerGraphEdgeLengthThresholdForConsensus << "\n";
    s << "consensusCaller = " <<
        consensusCaller << "\n";
    s << "useMarginPhase = " <<
        useMarginPhase << "\n";
    s << "storeCoverageData = " <<
        storeCoverageData << "\n";
}

void AssemblyOptions::write(ostream& s) const
{
    Reads.write(s);
    s << "\n";
    Kmers.write(s);
    s << "\n";
    MinHash.write(s);
    s << "\n";
    Align.write(s);
    s << "\n";
    ReadGraph.write(s);
    s << "\n";
    MarkerGraph.write(s);
    s << "\n";
    Assembly.write(s);
    s << endl;
}

void AssemblyOptions::MarkerGraphOptions::parseSimplifyMaxLength()
{
    simplifyMaxLengthVector.clear();

    boost::tokenizer< boost::char_separator<char> > tokenizer(
        simplifyMaxLength, boost::char_separator<char>(","));
    for(const string token: tokenizer) {
        try {
            size_t numberEndsHere;
            const size_t value = std::stoi(token, &numberEndsHere);
            if(numberEndsHere != token.size()) {
                throw runtime_error("Error parsing MarkerGraph.simplifyMaxLength " +
                    simplifyMaxLength);
            }
            simplifyMaxLengthVector.push_back(value);
        } catch(std::invalid_argument e) {
            throw runtime_error("Error parsing MarkerGraph,simplifyMaxLength " +
                simplifyMaxLength);
        }
    }

}


/*! \file demo_1d_containers.cpp
  \brief An example to demonstrate simple 1D plotting using a range of different STL containers.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A Bristow 2008, 2012, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_1d_containers_1

/*`First a few includes to use Boost.Plot and various STL containers:
*/
#include <boost/svg_plot/svg_1d_plot.hpp>
  // using namespace boost::svg;
#include <boost/array.hpp>
  using boost::array;
#include <vector>
  using std::vector;
#include <set>
  using std::set; // Automatically sorted - though this is not useful to the plot process.
  using std::multiset; // At present using std::multiset, allowing duplicates, plot does not indicate duplicates.
  // With 2_D data in std::multimap, duplicate values are usefully displayed.
#include <list>
  using std::list;
#include <deque>
  using std::deque;

//] [/demo_1d_containers_1]

int main()
{
  {
//[demo_1d_containers_2

/*`STL vector is used as the container,
and fictional values are inserted using push_back.
Since this is a 1-D plot the order of data values is not important.
*/
  std::vector<float> values;
  values.push_back(3.1f);
  values.push_back(-5.5f);
  values.push_back(8.7f);
  values.push_back(0.5f);

/*`The constructor initializes a new 1D plot, called `my_plot`, and also sets all the many default values.
*/

  using namespace boost::svg;
  boost::svg::svg_1d_plot my_plot;

/*`Setting (member) functions are fairly self-explanatory:
Title provides a title at the top for the whole plot,
and plot adds a (unnamed) data series (naming isn't very useful if there is only one data series).
*/
  my_plot.title("vector<float> example");
/*`
[note One must insert the XML character entity equivalents of < for < and > for >).]
*/
  my_plot.plot(values);

/*`Write the SVG to a file.
*/
  my_plot.write("./demo_1d_vector_float.svg");
//] [/demo_1d_containers_2]
  }

  {
//[demo_1d_containers_3

  static boost::array<const long double, 4> values {3.1L,-5.5L, 8.7L, 0.5L};
  //! \note Using @c long double provokes an expected warning about conversion from 'T' to 'const double', possible loss of data.
  boost::svg::svg_1d_plot my_plot;
  my_plot.title("array<long double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_array_long_double.svg");
//] [/demo_1d_containers_3]
  }

  {
//[demo_1d_containers_4
/*`If the container type is a set, then it can be filled with insert:*/
  std::set<double> values;
  values.insert(-8.4);
  values.insert(-2.3);
  values.insert(0.1);
  values.insert(5.6);
  values.insert(7.8);

  boost::svg::svg_1d_plot my_plot;
  my_plot.title("set<double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_set_double.svg");
//] [/demo_1d_containers_4]
  }

  {
//[demo_1d_containers_5
/*`If the container type is a list, then it can be filled with push_back or push_front:*/
  std::list<double> values;
  values.push_back(-8.4);
  values.push_back(-2.3);
  values.push_back(0.1);
  values.push_back(5.6);
  values.push_back(7.8);
  boost::svg::svg_1d_plot my_plot;
  my_plot.title("list<double> example");
  my_plot.plot(values);
  my_plot.write("./demo_1d_list_double.svg");
//] [/demo_1d_containers_5]
  }

  {
//[demo_1d_containers_6
/*`If the container type is a deque, then it can be filled with push_back or push_front:*/
  std::deque<double> values;
  values.push_front(-8.4);
  values.push_front(-2.3);
  values.push_front(0.1);
  values.push_front(5.6);
  values.push_front(7.8);

  boost::svg::svg_1d_plot my_plot;
  my_plot.title("deque<double> example");
  my_plot.plot(values);
  my_plot.x_label("X values as doubles");

  my_plot.write("./demo_1d_deque_double.svg");
//] [/demo_1d_containers_6]
  }
  return 0;
} // int main()

/*
Sample output:
//[demo_1d_containers_output

demo_1d_containers.cpp
demo_1d_containers.vcxproj -> I:\Cpp\SVG_plot\svg_plot\x64\Release\demo_1d_containers.exe
Autorun "I:\Cpp\SVG_plot\svg_plot\x64\Release\demo_1d_containers.exe"
Plot written to file ./demo_1d_vector_float.svg.
Plot written to file ./demo_1d_array_long_double.svg.
Plot written to file ./demo_1d_set_double.svg.
Plot written to file ./demo_1d_list_double.svg.
Plot written to file ./demo_1d_deque_double.svg.
//] [/demo_1d_containers_output]
*/


/*! \file demo_2d_autoscaling_vector.cpp
    \brief Demonstration of autoscaling in 2D plots.
    \details  An example to demonstrate simplest 2d *default* settings.
     See also auto_2d_plot.cpp for a wider range of use.

    \date 20 Mar 2009
    \author Paul A. Bristow
*/
// Copyright Paul A Bristow 2009, 2021

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

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[demo_2d_autoscaling_vector_1

/*`As always, we need a few includes to use Boost.Plot:
*/

#include <boost/svg_plot/svg_2d_plot.hpp>
//using namespace boost::svg;
//using boost::svg::svg_2d_plot;
#include <boost/svg_plot/detail/pair.hpp>
//  using boost::svg::detail::operator<<; // Output pair as, for example: 1.23, 4.56

#include <iostream>
  //using std::cout;
  //using std::endl;
  //using std::dec;
  //using std::hex;

#include <vector>
//  using std::vector
#include <tuple>
// using std::pair;
//
//] [demo_2d_autoscaling_vector_1]

int main()
{

  //[demo_2d_autoscaling_vector_2
  /*`Some fictional data is pushed into an STL container, here `std::map`:\n

    This example uses a single map to demonstrate autoscaling.
    We construct a `std::map` to hold our data-series, and insert some fictional values (that also sorts the data).
    The 'index' value in [ ] is the X value.
    */
  std::vector< std::pair<double, double> > my_vector_data(4);
//  my_vector_data.push_back(1.1, 3.2);
  std::pair<double, double> p0 = std::make_pair(1.1, 3.2);
  my_vector_data[0] = p0;
  my_vector_data[1] = std::make_pair(7.3, 9.1);
  my_vector_data[2] = std::make_pair(2.12, 2.4394);
  my_vector_data[3] = std::make_pair(5.47, 5.3861);
  my_vector_data.push_back(p0);

  for (std::size_t i = 0; i < my_vector_data.size(); i++)
  {
    using boost::svg::detail::operator<<; // Output pairs as, for example: 1.23, 4.56
    std::cout << my_vector_data[i] << std::endl;
  }

  try
  {
    // try'n'catch blocks are needed to ensure error messages from any exceptions are shown by the catch block below.
    using boost::svg::svg_2d_plot;
    svg_2d_plot my_2d_plot; // Construct a plot with all the default constructor values.
    my_2d_plot.title("Autoscaling 2d Values in a vector."); // Add a string title of the plot.

/*`We can show the ranges before  autoscaling;
*/
    std::cout << "X-min " << my_2d_plot.x_range().first << ", X-max " << my_2d_plot.x_range().second << std::endl;
    std::cout << "Y-min " << my_2d_plot.y_range().first << ", Y-max "  << my_2d_plot.y_range().second << std::endl;

/*` With the defaults ranges would be -10 to +10 for both X and Y axes.  We could chose our own ranges thus:
    `.x_range(0, 6) // Add a range for the X-axis.
    .y_range(0, 10) // Add a range for the Y-axis.`
  Or we can use autoscaling.

*/
    my_2d_plot.xy_autoscale(my_vector_data); // Autoscale both X and Y axes.

    std::cout << "X-axis autoscaled minimum tick value = " << my_2d_plot.x_auto_min_value()
      << " to max " << my_2d_plot.x_auto_max_value() << " with " << my_2d_plot.x_auto_ticks() << " ticks at "
      << my_2d_plot.x_auto_tick_interval() << " tick intervals." << std::endl;
    std::cout << "Y-axis autoscaled minimum tick value = " << my_2d_plot.y_auto_min_value_
        << " to max " << my_2d_plot.y_auto_max_value() << " with " << my_2d_plot.y_auto_ticks() << " ticks at "
      << my_2d_plot.y_auto_tick_interval() << " tick intervals."<< std::endl;

    my_2d_plot.x_autoscale(true); // Confirm that want to use default of using the autoscaling.
  //  my_2d_plot.y_autoscale(false); // Revert to using any range selected, or the default range -10 to +10.

    std::cout << "Y-axis autoscaled minimum tick value = " << my_2d_plot.y_auto_min_value_
        << " to max " << my_2d_plot.y_auto_max_value() << " with " << my_2d_plot.y_auto_ticks() << " ticks at "
      << my_2d_plot.y_auto_tick_interval() << " tick intervals."<< std::endl;

 /*`This says use the entire STL `std::map` container `my_vector_data` to set both X and Y ranges.
 (The data used to autoscale the range(s) does not have to be the same as the data being plotted.
 For example, if we have analysed a product and know that an attribute like strength can only decline as the product ages,
 it would make sense to use the reference 'as new' data to scale the plot for the 'aged' product samples).
 */

    std::cout << std::boolalpha
      << "X-autoscale " << my_2d_plot.x_autoscale() << ", X-autoscale_check_limits " << my_2d_plot.autoscale_check_limits() << std::endl;
    std::cout << "Y-min " << my_2d_plot.y_range().first << ", Y-max "  << my_2d_plot.y_range().second << std::endl;

    // std::cout << "X-min " << my_2d_plot.

    /*`Then add the (one but could be more) data-series, @c my_vector_data and a description, and how the data-points are to be marked,
    here a circle with a diameter of 5 pixels, without a line joining the points (also the default).
    */
    my_2d_plot.plot(my_vector_data, "2d Values").size(8);

    /*`To use all these settings, finally write the plot to file.
    */
    my_2d_plot.write("./demo_2d_autoscaling_vector.svg");

    std::cout << "Wrote SVG XML to file " << "./demo_2d_autoscaling_vector.svg" << std::endl;

    //] [demo_2d_autoscaling_vector_2]

  }
  catch (const std::exception& e)
  {
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }
  return 0;
} // int main()

/*

//[demo_2d_autoscaling_vector_output

Output:
Checked: x_min 1.1, x_max 7.3, y_min 2.4394, y_max 9.1, 5 'good' values, 0 values at limits.
X-axis autoscaled minimum tick value = 1 to max 8
Y-axis autoscaled minimum value = 2 to max 2
X min -10, X max 10
Y min -10, Y max 10

  Xautoscale true, X-autoscale_check_limits true
*/


//  Copyright (c) 2007-2012 Hartmut Kaiser
//  Copyright (c) 2014 Agustin Berge
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#if !defined(HPX_UTIL_DATE_TIME_CHRONO_APR_10_2012_0449PM)
#define HPX_UTIL_DATE_TIME_CHRONO_APR_10_2012_0449PM

#include <hpx/hpx_fwd.hpp>
#include <boost/chrono/chrono.hpp>
#include <boost/date_time/posix_time/posix_time_duration.hpp>
#include <boost/date_time/posix_time/ptime.hpp>

#if defined(HPX_WITH_CXX11_CHRONO)
#include <chrono>
#endif

namespace hpx { namespace util
{
    template <typename Clock, typename Duration>
    boost::posix_time::ptime
    to_ptime(boost::chrono::time_point<Clock, Duration> const& from)
    {
        typedef boost::chrono::nanoseconds duration_type;
        typedef duration_type::rep rep_type;
        rep_type d = boost::chrono::duration_cast<duration_type>(
            from.time_since_epoch()).count();
        rep_type sec = d / 1000000000;
        rep_type nsec = d % 1000000000;
        return boost::posix_time::from_time_t(0) +

            boost::posix_time::seconds(static_cast<long>(sec)) +
#ifdef BOOST_DATE_TIME_HAS_NANOSECONDS
            boost::posix_time::nanoseconds(nsec);
#else
            boost::posix_time::microseconds((nsec+500)/1000);
#endif
    }

    template <typename Clock, typename Duration>
    boost::posix_time::time_duration
    to_time_duration(boost::chrono::duration<Clock, Duration> const& from)
    {
        typedef boost::chrono::nanoseconds duration_type;
        typedef duration_type::rep rep_type;
        rep_type d = boost::chrono::duration_cast<duration_type>(from).count();
        rep_type sec = d / 1000000000;
        rep_type nsec = d % 1000000000;
        return boost::posix_time::seconds(static_cast<long>(sec)) +
#ifdef BOOST_DATE_TIME_HAS_NANOSECONDS
            boost::posix_time::nanoseconds(nsec);
#else
            boost::posix_time::microseconds((nsec+500)/1000);
#endif
    }

    ///////////////////////////////////////////////////////////////////////////
    struct posix_clock
    {
        typedef boost::int_least64_t rep;
        typedef boost::nano period;
        typedef boost::chrono::duration<rep, period> duration;
        typedef boost::chrono::time_point<posix_clock, duration> time_point;
        BOOST_STATIC_CONSTEXPR bool is_steady = false;

        static BOOST_FORCEINLINE time_point now() BOOST_NOEXCEPT
        {
            return from_ptime(boost::get_system_time());
        }

        static boost::posix_time::ptime to_ptime(time_point const& t) BOOST_NOEXCEPT
        {
            return boost::posix_time::from_time_t(0) +
#if defined(BOOST_DATE_TIME_HAS_NANOSECONDS)
                boost::posix_time::nanoseconds(t.time_since_epoch().count());
#else
                boost::posix_time::microseconds(t.time_since_epoch().count() / 1000);
#endif
        }

        static time_point from_ptime(boost::posix_time::ptime const& t) BOOST_NOEXCEPT
        {
            return time_point(duration(
              (t - boost::posix_time::from_time_t(0)).total_nanoseconds()));
        }
    };

    class steady_time_point
    {
        typedef boost::chrono::steady_clock::time_point value_type;

    public:
        steady_time_point(value_type const& abs_time)
          : _abs_time(abs_time)
        {}

        template <typename Clock, typename Duration>
        steady_time_point(boost::chrono::time_point<Clock, Duration> const& abs_time)
          : _abs_time(boost::chrono::steady_clock::now()
              + (abs_time - Clock::now()))
        {}

        steady_time_point(boost::posix_time::ptime const& abs_time)
          : _abs_time(boost::chrono::steady_clock::now()
              + (posix_clock::from_ptime(abs_time) - posix_clock::now()))
        {}

#if defined(HPX_WITH_CXX11_CHRONO)
        template <typename Clock, typename Duration>
        steady_time_point(std::chrono::time_point<Clock, Duration> const& std_abs_time)
        {
            boost::chrono::nanoseconds rel_time(
                std::chrono::duration_cast<std::chrono::nanoseconds>(
                    std_abs_time - Clock::now()).count());

            _abs_time = boost::chrono::steady_clock::now() + rel_time;
        }
#endif

        value_type const& value() const BOOST_NOEXCEPT
        {
            return _abs_time;
        }

    private:
        value_type _abs_time;
    };

    class steady_duration
    {
        typedef boost::chrono::steady_clock::duration value_type;

    public:
        steady_duration(value_type const& rel_time)
          : _rel_time(rel_time)
        {}

        template <typename Rep, typename Period>
        steady_duration(boost::chrono::duration<Rep, Period> const& rel_time)
          : _rel_time(boost::chrono::duration_cast<value_type>(rel_time))
        {
            if (_rel_time < rel_time)
                ++_rel_time;
        }

        steady_duration(boost::posix_time::time_duration const& rel_time)
          : _rel_time(rel_time.total_nanoseconds())
        {}

#if defined(HPX_WITH_CXX11_CHRONO)
        template <typename Rep, typename Period>
        steady_duration(std::chrono::duration<Rep, Period> const& std_rel_time)
        {
            boost::chrono::nanoseconds rel_time(
                std::chrono::duration_cast<std::chrono::nanoseconds>(
                    std_rel_time).count());

            _rel_time = boost::chrono::duration_cast<value_type>(rel_time);
            if (_rel_time < rel_time)
                ++_rel_time;
        }
#endif

        value_type const& value() const BOOST_NOEXCEPT
        {
            return _rel_time;
        }

        boost::chrono::steady_clock::time_point from_now() const BOOST_NOEXCEPT
        {
            return boost::chrono::steady_clock::now() + _rel_time;
        }

    private:
        value_type _rel_time;
    };

    ///////////////////////////////////////////////////////////////////////////
    template <typename Clock>
    struct chrono_traits
    {
        typedef typename Clock::duration duration_type;
        typedef typename Clock::time_point time_type;

        static time_type now() BOOST_NOEXCEPT
        {
            return Clock::now();
        }

        static time_type add(time_type t, duration_type d)
        {
            return t + d;
        }

        static duration_type subtract(time_type t1, time_type t2)
        {
            return t1 - t2;
        }

        static bool less_than(time_type t1, time_type t2)
        {
            return t1 < t2;
        }

        static boost::posix_time::time_duration to_posix_duration(duration_type d)
        {
#ifdef BOOST_DATE_TIME_HAS_NANOSECONDS
            return boost::posix_time::nanoseconds(
                boost::chrono::duration_cast<boost::chrono::nanoseconds>(d).count());
#else
            return boost::posix_time::microseconds(
                boost::chrono::duration_cast<boost::chrono::microseconds>(d).count());
#endif
        }
    };
}}

#endif


//
// Created by erik on 9/27/16.
//

#include "PhysicsWorld.h"
#include "HitTests.h"
#include "Fixture.h"
#include <boost/optional.hpp>
#include <boost/variant.hpp>
#include <iostream>

using namespace spatacs::physics;
using spatacs::Vec;

namespace
{
    template<class Base>
    struct GetUnderlyingContainer : public Base
    {
        using container_t = typename Base::container_type;
        container_t& container() { return Base::c; }
        const container_t& container() const  { return Base::c; }
    };
}

void PhysicsWorld::pushEvent(Collision evt)
{
    mEventQueue.push( std::move(evt) );
}

const Object& PhysicsWorld::getObject(ObjectID id) const
{
    return getObjectRec(id).object;
}

void PhysicsWorld::setCollisionCallback(PhysicsWorld::collision_callback_fn cb)
{
    mCollisionCallback = std::move(cb);
}

void PhysicsWorld::simulate(time_t time_step)
{
    mNow = 0.0_s;

    // detect all collisions
    for(auto& obj : mObjects)
        detectCollisionsOf(obj.first, time_step);

    // now handle all events
    while( !mEventQueue.empty() )
    {
        auto ev = std::move(mEventQueue.top());
        mEventQueue.pop();

        ImpactInfo info;
        info.time = ev.time;
        info.fixture_A = ev.fA;
        info.fixture_B = ev.fB;
        mNow = ev.time;
        if(ev.time <= time_step) {
            /// \todo maybe we should update A and B here to the ev.time state
            mCollisionCallback(*this, getObject(ev.A), getObject(ev.B), info);
        }
    }

    mNow = time_step;

    // update positions
    for(auto& obr : mObjects)
    {
        auto dt = mNow - obr.second.update;
        obr.second.object.setPosition( obr.second.object.position() + dt * obr.second.object.velocity());
        obr.second.object.setVelocity( obr.second.object.velocity() + obr.second.acceleration );
        obr.second.acceleration = velocity_vec{Vec{0, 0, 0}};
        obr.second.update = 0.0_s;
    }
}

void PhysicsWorld::detectCollisionsOf(ObjectID id, time_t max_dt, bool all)
{
    auto& orec = getObjectRec(id);
    auto& obj = orec.object;
    for(auto& fixa : obj) {
        length_vec original_position = obj.position() - orec.update * obj.velocity();
        MovingSphere ppath = {original_position, obj.velocity(), fixa.radius(), speed_t(0)};
        // check ship impact
        for (auto& target : mObjects) {
            // no self-intersection, check each pair only once
            if (target.first == id || (id < target.first && !all))
                continue;
            /// \todo disable projectile-projectile hit tests
            Object& tobj = target.second.object;
            for (auto& fix : tobj)
            {
                auto opos = tobj.position() - target.second.update * tobj.velocity();
                MovingSphere tpath = {opos, tobj.velocity(), fix.radius(), speed_t(0)};
                auto hit = intersect(tpath, ppath);
                if (hit) {
                    time_t time = hit.get();
                    if (time >= mNow && time < max_dt) {
                        pushEvent(Collision{id, target.first, fixa.userdata(), fix.userdata(), time});
                    }
                }
            }
        }
    }
}

void PhysicsWorld::filterCollisions(ObjectID id)
{
    std::vector<Collision> container = std::move(((GetUnderlyingContainer<decltype(mEventQueue)>&)(mEventQueue)).container());
    auto nend = std::remove_if(begin(container), end(container), [id](auto& e){ return e.A == id || e.B == id;});
    container.resize( std::distance(begin(container), nend) );

    // construct a new queue, because I don't know whether we might hav destroyed some invariants in
    // the process above.
    mEventQueue = queue_t(Compare(), std::move(container));
}

const PhysicsWorld::ObjectRecord& PhysicsWorld::getObjectRec(ObjectID id) const
{
    return mObjects.at(id);
}

PhysicsWorld::ObjectRecord& PhysicsWorld::getObjectRec(ObjectID id)
{
    return mObjects.at(id);
}

ObjectID PhysicsWorld::spawn(const Object& object)
{
    ObjectRecord new_rec;
    new_rec.object = object;
    ObjectID id{mFreeID};
    new_rec.object.setID( id );
    new_rec.acceleration = velocity_vec(Vec{0, 0, 0});
    mObjects[id] = std::move(new_rec);
    ++mFreeID;
    return id;
    /// \todo trigger calculation of collision events.
}

bool PhysicsWorld::despawn(ObjectID id)
{
    auto e = mObjects.erase(id);
    if( e != 1 )
    {
        return false;
    }

    filterCollisions(id);
    return true;
}

void PhysicsWorld::applyForce(ObjectID id, force_vec force)
{
    /// \todo this is a fixed end time! Change!
    time_t application_time = 0.1_s;
    auto& target = getObjectRec(id);
    target.acceleration += force * application_time / target.object.mass();
}

void PhysicsWorld::setMass(ObjectID id, mass_t mass)
{
    auto& target = getObjectRec(id);
    target.object.setMass( mass );
}

void PhysicsWorld::applyImpulse(ObjectID id, impulse_vec impulse)
{
    auto& target = getObjectRec(id);
    auto vel_old = target.object.velocity();
    target.object.setVelocity( vel_old + impulse / target.object.mass() );

    // perform part of the integration now
    auto dt = mNow - target.update;
    target.object.setPosition( target.object.position() + dt * vel_old);
    target.update = mNow;

    // now we can update the collision tests
    // remove old collisions of id
    filterCollisions(id);
    detectCollisionsOf( id, 1.0_s, true );
}

void PhysicsWorld::updateObject(ObjectID id, length_vec new_position, velocity_vec new_veloctiy, mass_t new_mass)
{
    auto& target = getObjectRec(id);
    target.object.setMass( new_mass );
    target.object.setPosition(new_position);
    target.object.setVelocity(new_veloctiy);
}

bool PhysicsWorld::Compare::operator()(const Collision& a, const Collision& b) const
{
    return a.time > b.time;
}


// -*- mode:c++;tab-width:2;indent-tabs-mode:t;show-trailing-whitespace:t;rm-trailing-spaces:t -*-
// vi: set ts=2 noet:

#ifndef INCLUDED_riflib_HBondedPairGenerator_hh
#define INCLUDED_riflib_HBondedPairGenerator_hh

// headers

	// #include <core/pack/rotamer_set/RotamerSet.hh>
	// #include <core/pack/rotamer_set/RotamerSetFactory.hh>
	// #include <core/pack/dunbrack/RotamerLibrary.hh>
	#include <core/pack/dunbrack/RotamerLibraryScratchSpace.hh>
	// #include <core/pack/dunbrack/SingleResidueDunbrackLibrary.hh>
	// #include <core/pack/task/TaskFactory.hh>
	// #include <core/graph/Graph.hh>
	// #include <core/pack/packer_neighbors.hh>

	#include <ObjexxFCL/format.hh>
	#include <ObjexxFCL/string.functions.hh>

	#include <core/chemical/AtomType.hh>
	#include <core/chemical/orbitals/OrbitalType.hh>
	#include <core/chemical/ChemicalManager.hh>
	#include <core/chemical/ResidueTypeSet.hh>
	// #include <core/conformation/symmetry/util.hh>
	#include <core/conformation/ResidueFactory.hh>
	// #include <core/conformation/util.hh>
	// #include <core/import_pose/import_pose.hh>
	// #include <core/io/silent/SilentFileData.hh>
	// #include <core/pose/PDBInfo.hh>
	#include <core/pose/Pose.hh>
	// #include <core/pose/annotated_sequence.hh>
	#include <core/pose/motif/reference_frames.hh>
	// #include <core/pose/util.hh>
	// #include <core/pose/symmetry/util.hh>
	// #include <core/import_pose/import_pose	.hh>
	// #include <core/kinematics/MoveMap.hh>
	#include <core/scoring/Energies.hh>
	// #include <core/scoring/EnergyGraph.hh>
	#include <core/scoring/ScoreFunction.hh>
	#include <core/scoring/ScoreFunctionFactory.hh>
	// #include <core/scoring/ScoreTypeManager.hh>
	// #include <core/scoring/dssp/Dssp.hh>
	// #include <core/scoring/etable/Etable.hh>
	#include <core/scoring/hbonds/HBondOptions.hh>
	// #include <core/scoring/hbonds/HBondSet.hh>
	// #include <core/scoring/hbonds/hbonds.hh>
	#include <core/scoring/motif/util.hh>

	#include <core/scoring/methods/EnergyMethodOptions.hh>
	// #include <core/scoring/packing/compute_holes_score.hh>
	// #include <core/scoring/rms_util.hh>
	// #include <core/scoring/sasa.hh>
	// #include <core/scoring/symmetry/SymmetricScoreFunction.hh>
	#include <numeric/conversions.hh>
	#include <numeric/model_quality/rms.hh>
	// #include <numeric/random/random.hh>
	#include <numeric/xyz.functions.hh>
	#include <numeric/xyz.io.hh>
	#include <numeric/xyzVector.hh>
	// #include <protocols/idealize/IdealizeMover.hh>
	// #include <protocols/sicdock/Assay.hh>
	// #include <protocols/sic_dock/SICFast.hh>
	// #include <protocols/sic_dock/util.hh>
	// #include <protocols/sic_dock/read_biounit.hh>
	// #include <protocols/simple_moves/MinMover.hh>
	// #include <protocols/simple_moves/AddConstraintsToCurrentConformationMover.hh>
	// #include <utility/io/izstream.hh>
	// #include <utility/io/ozstream.hh>
	#include <utility/tools/make_vector1.hh>
	// #include <utility/fixedsizearray1.hh>
	// #include <utility/file/file_sys_util.hh>
	// #include <numeric/geometry/hashing/SixDHasher.hh>
	// #include <numeric/HomogeneousTransform.hh>

	#include <riflib/RotamerGenerator.hh>

	// #include <apps/pilot/will/will_util.ihh>

	#include <boost/foreach.hpp>
	#include <boost/iterator/iterator_facade.hpp>

namespace devel {
namespace scheme {

core::Real
get_rot_score(
	core::pose::Pose & pose,
	core::Size ir,
	core::pack::dunbrack::RotamerLibraryScratchSpaceOP scratch
);

void dump_pdb_atom(
	std::ostream & out,
	double x, double y, double z
);

///@brief generate BCC lattice "hyper-ring" to sample rotationr around
/// the hbond axis as well as tilting the axis a up to short_tol
utility::vector1<numeric::xyzMatrix<double> >
get_rotation_samples( double short_tol, double long_tol, double reslang );

class HBondedPairGenerator
    : public boost::iterator_facade< HBondedPairGenerator,
                                     core::pose::Pose,
                                     boost::forward_traversal_tag,
                                     core::pose::Pose const &
             						>
{
public:
	core::pose::Pose pose_,pose0_;
	core::pack::dunbrack::RotamerLibraryScratchSpaceOP scratch_;
	// core::pack::rotamer_set::RotamerSetOP rotset1_,rotset2_;
	utility::vector1< utility::vector1< float > > rotset1_, rotset2_;
	core::Size irot1_,irot2_,idon_,iacc_,iorb_,ihbr_,irotindex_start1_,irotindex_start2_;
	core::Size nrots1_,nrots2_;
	core::Size clash_atom_start1_, clash_atom_start2_;
	utility::vector1<core::Size> donor_atoms_;
	utility::vector1<core::Size> donor_bases_;
	utility::vector1<core::Size> acceptor_atoms_;
	utility::vector1<utility::vector1<core::Size> > acceptor_orbitals_;
	utility::vector1<numeric::xyzMatrix<double> > rot_samples_;
	core::scoring::ScoreFunctionOP score_func_;
	core::id::AtomID align_atom1_, align_atom2_, align_atom3_;
	core::Real score_;
	bool fix_donor_, fix_acceptor_;
	HBondedPairGenerator(){ irot1_=0; irot2_=0; idon_=0; iacc_=0; iorb_=0; ihbr_=0; }
	core::chemical::ResidueTypeOP rtype1op_, rtype2op_;
	virtual ~HBondedPairGenerator(){}
	void init(
		RotamerIndex const & rot_index,
		std::string resn1,
		std::string resn2,
		double tip_tol=20.0,
		double rot_resl=5.0,
		double rot_range=360.0,
		bool fix_donor=false,
		bool fix_acceptor=false,
		core::id::AtomID align_atom1 = core::id::AtomID(1,1),
		core::id::AtomID align_atom2 = core::id::AtomID(2,1),
		core::id::AtomID align_atom3 = core::id::AtomID(3,1),
		std::map<std::string,core::pose::Pose> const & exemplars = std::map<std::string,core::pose::Pose>()
	);
	bool has_more_samples() const { return irot1_!=0; }
	core::Size raw_num_samples() const {
		core::Size nacc_orbs = 0;
		for(core::Size i = 1; i <= acceptor_orbitals_.size(); ++i)
			nacc_orbs += acceptor_orbitals_[i].size();
		return rot_samples_.size()              *
			   nacc_orbs                        *
		       nrots2_                          *
		       donor_atoms_.size()              *
		       nrots1_                          ;
	}
	size_t rot1_of_current() const { return irot1_ + irotindex_start1_ - 1 ; }
	size_t rot2_of_current() const { return irot2_ + irotindex_start2_ - 1 ; }
	core::Real score_of_current() const { return score_; }
private:
    friend class boost::iterator_core_access;
    core::pose::Pose const & dereference() const {
    	return pose_;
    }
    void update_chi(){
    	if( ! fix_donor_ )
			for(core::Size ichi=1; ichi <= pose0_.residue(1).nchi(); ++ichi)
				pose0_.set_chi(ichi,1, rotset1_[irot1_][ichi] );
		if( ! fix_acceptor_ )
			for(core::Size ichi=1; ichi <= pose0_.residue(2).nchi(); ++ichi)
				pose0_.set_chi(ichi,2, rotset2_[irot2_][ichi] );
    }

    virtual bool update_hb_and_score(bool=true) { utility_exit_with_message("Abstract base class"); }

    void increment(){
    	bool clash = true;
    	while(clash){
			// std::cout << "INCR " << irot1_ << " " << idon_ << " " << irot2_ << " " << iacc_ << " " << iorb_ << " " << ihbr_ << std::endl;
    		++ihbr_;
	    	if( ihbr_  > rot_samples_.size()              ){ ihbr_  = 1; ++iorb_ ; }
    		if( iorb_  > acceptor_orbitals_[iacc_].size() ){ iorb_  = 1; ++iacc_ ; }
    		if( iacc_  > acceptor_atoms_.size()           ){ iacc_  = 1; ++irot2_; }
			if( irot2_ > nrots2_                          ){ irot2_ = 1; ++idon_ ; }
			if( idon_  > donor_atoms_.size()              ){ idon_  = 1; ++irot1_;  }
			if( irot1_ > nrots1_                          ){ irot1_ = irot2_ = idon_ = iacc_ = iorb_ = ihbr_ = 0;  return; }
			this->update_chi();
			clash = ! this->update_hb_and_score(ihbr_==1);
		}
    }
    bool equal(HBondedPairGenerator const& o) const {
    	return o.irot1_==irot1_ && o.irot2_==irot2_ && o.iacc_==iacc_ && o.idon_==idon_ && iorb_==o.iorb_ && ihbr_==o.ihbr_;
    }

};

// typedef utility::pointer::shared_ptr<HBondedPairGenerator> HBondedPairGeneratorOP;

class SingleHbondedPairGenerator : public HBondedPairGenerator {

    virtual bool update_hb_and_score(bool realign=true);

};

class BidentateHbondedPairGenerator : public HBondedPairGenerator {

   virtual bool update_hb_and_score(bool realign=true);

};

}
}

#endif


// -*- mode:c++;tab-width:2;indent-tabs-mode:t;show-trailing-whitespace:t;rm-trailing-spaces:t -*-
// vi: set ts=2 noet:
//
// (c) Copyright Rosetta Commons Member Institutions.
// (c) This file is part of the Rosetta software suite and is made available under license.
// (c) The Rosetta software is developed by the contributing members of the Rosetta Commons.
// (c) For more information, see http://wsic_dockosettacommons.org. Questions about this casic_dock
// (c) addressed to University of Waprotocolsgton UW TechTransfer, email: license@u.washington.eprotocols

#ifndef INCLUDED_riflib_CBTooCloseManager_hh
#define INCLUDED_riflib_CBTooCloseManager_hh

#include <riflib/types.hh>
#include <riflib/rifdock_typedefs.hh>

#include "scheme/util/SimpleArray.hh"
#include <riflib/RotamerGenerator.hh>
#include <riflib/ScoreRotamerVsTarget.hh>
#include <scheme/objective/voxel/VoxelArray.hh>

#include <core/pose/Pose.hh>

#include <string>
#include <vector>

#include <boost/any.hpp>
#include <boost/format.hpp>

#include <riflib/util.hh>

#include <ObjexxFCL/format.hh>

namespace devel {
namespace scheme {

struct CBTooCloseManager {

    float resl_;

    shared_ptr<::scheme::objective::voxel::VoxelArray<3>> voxel_array_;

    CBTooCloseManager(
        core::pose::Pose const & target,
        float resl,
        float too_close_dist,
        float penalty,
        size_t max_target_res_atom_idx
    ) : resl_( resl )
    {
        std::cout << "Creating CB too close grid" << std::endl;
        prepare_bounds( target, too_close_dist, max_target_res_atom_idx );
        create_and_fill_voxel_map( target, too_close_dist, penalty, max_target_res_atom_idx );

        // voxel_array_->dump_pdb("CB_too_close.pdb", 0.1, true, 0.1);
    }

    void
    prepare_bounds( core::pose::Pose const & target, float too_close_dist, size_t max_target_res_atom_idx ) {

        Eigen::Vector3f lbs( 9e9, 9e9, 9e9 );
        Eigen::Vector3f ubs( -9e9, -9e9, -9e9 );

        for ( core::Size seqpos = 1; seqpos <= target.size(); seqpos++ ) {
            core::conformation::Residue const & res = target.residue(seqpos);
            size_t loop_ub = std::min<size_t>( res.nheavyatoms(), max_target_res_atom_idx );
            for ( core::Size atno = 1; atno <= loop_ub; atno++ ) {

                numeric::xyzVector<core::Real> xyz = res.xyz(atno);
                for ( int i = 0; i < 3; i++ ) {
                    lbs[i] = std::min<float>( lbs[i], xyz[i] );
                    ubs[i] = std::max<float>( ubs[i], xyz[i] );
                }
            }

        }

        for ( int i = 0; i < 3; i++ ) {
            lbs[i] -= too_close_dist + resl_ * 2;
            ubs[i] += too_close_dist + resl_ * 2;
        }

        // lb_ = lbs;
        // ub_ = ubs;
        // cs_ = Eigen::Vector3f( resl_, resl_, resl_ );

        voxel_array_ = make_shared<::scheme::objective::voxel::VoxelArray<3>>( lbs, ubs, Eigen::Vector3f( resl_, resl_, resl_ ) );

        // Indices extents = floats_to_index( ub_ );
        // shape_ = extents + Indices(1);

    }

    void
    create_and_fill_voxel_map(
        core::pose::Pose const & target,
        float too_close_dist,
        float penalty,
        size_t max_target_res_atom_idx
        ) {

        // size_t elements = shape_[0] * shape_[1] * shape_[2];
        // std::cout << shape_ << std::endl;
        // std::cout << floats_to_index( ub_ ) << std::endl;
        // std::cout << elements << " " << index_to_map_index( floats_to_index( ub_ ) ) << std::endl;
        // runtime_assert( elements - 1 == index_to_map_index( floats_to_index( ub_ ) ) );

        // voxel_map_.resize( elements, 0 );

        ::scheme::util::SimpleArray<3,float> lb = voxel_array_->lb_;
        ::scheme::util::SimpleArray<3,float> ub = voxel_array_->ub_;

        float const too_close_dist2 = too_close_dist * too_close_dist;

        for ( size_t seqpos = 1; seqpos <= target.size(); seqpos ++  ) {
            core::conformation::Residue const & res = target.residue(seqpos);

            size_t loop_ub = std::min<size_t>( res.nheavyatoms(), max_target_res_atom_idx );
            for ( core::Size atno = 1; atno <= loop_ub; atno++ ) {

                numeric::xyzVector<core::Real> _xyz = res.xyz( atno );
                Eigen::Vector3f xyz; xyz[0] = _xyz[0]; xyz[1] = _xyz[1]; xyz[2] = _xyz[2];

                Eigen::Vector3f lbs( xyz[0] - too_close_dist, xyz[1] - too_close_dist, xyz[2] - too_close_dist );
                Eigen::Vector3f ubs( xyz[0] + too_close_dist, xyz[1] + too_close_dist, xyz[2] + too_close_dist );

                const float step = resl_;

                Eigen::Vector3f worker;

                for ( float x = lbs[0] - step/2; x < ubs[0] + step; x += step ) {
                    if ( x < lb[0] || x > ub[0] ) continue;
                    worker[0] = x;

                    for ( float y = lbs[1] - step/2; y < ubs[1] + step; y += step ) {
                        if ( y < lb[1] || y > ub[1] ) continue;
                        worker[1] = y;

                        for ( float z = lbs[2] - step/2; z < ubs[2] + step; z += step ) {
                            if ( z < lb[2] || z > ub[2] ) continue;
                            worker[2] = z;

                            const float squared_dist = ( xyz - worker ).squaredNorm();

                            if ( squared_dist < too_close_dist2 ) {
                                (*voxel_array_)[worker] = penalty;
                                // size_t offset = index_to_map_index( floats_to_index( worker ) );
                                // voxel_map_.at(offset) = penalty;
                            }
                        }
                    }
                }
            }
        }
    }

/////////////////////////////////////////////////////////////////////////
    float
    get_CB_penalty(
        EigenXform const & bbpos
    ) const {

        Eigen::Matrix<float,3,1> CB = bbpos * Eigen::Matrix<float,3,1>( 1.0264273 ,  0.25245885, -0.308907 );

        return voxel_array_->at( CB );

    }

    // template<class Floats> Indices floats_to_index(Floats const & f) const {
    //     Indices ind;
    //     for(int i = 0; i < 3; ++i){
    //         float tmp = ((f[i]-lb_[i])/cs_[i]);
    //         ind[i] = tmp;
    //     }
    //     return ind;
    // }

    // size_t index_to_map_index( Indices const & ind ) const {

    //     size_t accum = ind[0];
    //     accum = accum * shape_[1] + ind[1];
    //     accum = accum * shape_[2] + ind[2];

    //     return accum;

    // }

    // size_t index_to_offset( Indices const & ind ) const {

    //     return index_to_map_index( ind );

    // }

    // float const &
    // at( float f, float g, float h ) const {
    //     Indices idx = floats_to_index( Bounds( f, g, h ) );
    //     if( idx[0] < shape_[0] && idx[1] < shape_[1] && idx[2] < shape_[2] )
    //         return voxel_map_.at( index_to_offset(idx) );
    //     else return 0;
    // }

    // template<class V>
    // float const &
    // at( V const & v ) const {
    //     Indices idx = floats_to_index( Bounds( v[0], v[1], v[2] ) );
    //     if( idx[0] < shape_[0] && idx[1] < shape_[1] && idx[2] < shape_[2] )
    //         return voxel_map_.at( index_to_offset(idx) );
    //     else return 0;
    // }

};

}}

#endif


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

  Aggregate SIMD numerical and type limits for X86 AVX

  @copyright 2012 - 2015 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_AVX_LIMITS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_AVX_LIMITS_HPP_INCLUDED

#include <boost/simd/arch/x86/tags.hpp>
#include <boost/simd/arch/common/limits.hpp>
#include <boost/simd/detail/brigand.hpp>

namespace boost { namespace simd
{
  template<> struct limits<boost::simd::avx_>
  {
    struct largest_integer
    {
      template<typename Sign> struct apply { using type = brigand::no_such_type_; };
    };

    struct smallest_integer
    {
      template<typename Sign> struct apply { using type = brigand::no_such_type_; };
    };

    using parent = boost::simd::sse2_;

    using smallest_real = float;
    using largest_real  = double;

    enum { bits = 256, bytes = 32 };
  };
} }

#endif


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

#include <ros/ros.h>
#include <string>
#include <algorithm>
#include <memory>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <lanelet2_core/geometry/Point.h>
#include <trajectory_utils/trajectory_utils.h>
#include <trajectory_utils/conversions/conversions.h>
#include <sstream>
#include <carma_utils/containers/containers.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/LU>
#include <Eigen/SVD>
#include <inlanecruising_plugin/smoothing/SplineI.h>
#include <inlanecruising_plugin/smoothing/CubicSpline.h>
#include <inlanecruising_plugin/inlanecruising_plugin.h>
#include <inlanecruising_plugin/log/log.h>
#include <carma_utils/containers/containers.h>
#include <inlanecruising_plugin/smoothing/filters.h>
#include <unordered_set>

using oss = std::ostringstream;

namespace inlanecruising_plugin
{
InLaneCruisingPlugin::InLaneCruisingPlugin(carma_wm::WorldModelConstPtr wm, InLaneCruisingPluginConfig config,
                                           PublishPluginDiscoveryCB plugin_discovery_publisher)
  : wm_(wm), config_(config), plugin_discovery_publisher_(plugin_discovery_publisher)
{
  plugin_discovery_msg_.name = "InLaneCruisingPlugin";
  plugin_discovery_msg_.versionId = "v1.0";
  plugin_discovery_msg_.available = true;
  plugin_discovery_msg_.activated = false;
  plugin_discovery_msg_.type = cav_msgs::Plugin::TACTICAL;
  plugin_discovery_msg_.capability = "tactical_plan/plan_trajectory";
}

bool InLaneCruisingPlugin::onSpin()
{
  plugin_discovery_publisher_(plugin_discovery_msg_);
  return true;
}

bool InLaneCruisingPlugin::plan_trajectory_cb(cav_srvs::PlanTrajectoryRequest& req,
                                              cav_srvs::PlanTrajectoryResponse& resp)
{
  ros::WallTime start_time = ros::WallTime::now(); // Start timeing the execution time for planning so it can be logged

  lanelet::BasicPoint2d veh_pos(req.vehicle_state.X_pos_global, req.vehicle_state.Y_pos_global);
  double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;

  auto points_and_target_speeds = maneuvers_to_points(req.maneuver_plan.maneuvers, current_downtrack, wm_); // Convert maneuvers to points

  ROS_DEBUG_STREAM("points_and_target_speeds: " << points_and_target_speeds.size());

  auto downsampled_points =
      carma_utils::containers::downsample_vector(points_and_target_speeds, config_.downsample_ratio);

  ROS_DEBUG_STREAM("downsample_points: " << downsampled_points.size());

  ROS_DEBUG_STREAM("PlanTrajectory");

  cav_msgs::TrajectoryPlan trajectory;
  trajectory.header.frame_id = "map";
  trajectory.header.stamp = ros::Time::now();
  trajectory.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());

  trajectory.trajectory_points = compose_trajectory_from_centerline(downsampled_points, req.vehicle_state); // Compute the trajectory
  trajectory.initial_longitudinal_velocity = std::max(req.vehicle_state.longitudinal_vel, config_.minimum_speed);

  resp.trajectory_plan = trajectory;
  resp.related_maneuvers.push_back(cav_msgs::Maneuver::LANE_FOLLOWING);
  resp.maneuver_status.push_back(cav_srvs::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);

  ros::WallTime end_time = ros::WallTime::now(); // Planning complete

  ros::WallDuration duration = end_time - start_time;
  ROS_DEBUG_STREAM("ExecutionTime: " << duration.toSec());

  return true;
}

std::vector<double> InLaneCruisingPlugin::apply_speed_limits(const std::vector<double> speeds,
                                                             const std::vector<double> speed_limits)
{
  ROS_DEBUG_STREAM("Speeds list size: " << speeds.size());
  ROS_DEBUG_STREAM("SpeedLimits list size: " << speed_limits.size());

  if (speeds.size() != speed_limits.size())
  {
    throw std::invalid_argument("Speeds and speed limit lists not same size");
  }
  std::vector<double> out;
  for (size_t i = 0; i < speeds.size(); i++)
  {
    out.push_back(std::min(speeds[i], speed_limits[i]));
  }

  return out;
}

Eigen::Isometry2d InLaneCruisingPlugin::compute_heading_frame(const lanelet::BasicPoint2d& p1,
                                                              const lanelet::BasicPoint2d& p2)
{
  Eigen::Rotation2Dd yaw(atan2(p2.y() - p1.y(), p2.x() - p1.x()));

  return carma_wm::geometry::build2dEigenTransform(p1, yaw);
}

std::vector<DiscreteCurve> InLaneCruisingPlugin::compute_sub_curves(const std::vector<PointSpeedPair>& map_points)
{
  if (map_points.size() < 2)
  {
    throw std::invalid_argument("Not enough points");
  }

  std::vector<DiscreteCurve> curves;
  DiscreteCurve curve;
  curve.frame = compute_heading_frame(map_points[0].point, map_points[1].point);
  Eigen::Isometry2d map_in_curve = curve.frame.inverse();

  for (size_t i = 0; i < map_points.size() - 1; i++)
  {
    lanelet::BasicPoint2d p1 = map_in_curve * map_points[i].point;
    lanelet::BasicPoint2d p2 = map_in_curve * map_points[i + 1].point;  // TODO Optimization to cache this value

    PointSpeedPair initial_pair;
    initial_pair.point = p1;
    initial_pair.speed = map_points[i].speed;
    curve.points.push_back(initial_pair);

    bool x_dir = (p2.x() - p1.x()) > 0;
    if (!x_dir)  // If x starts going backwards we need a new curve
    {
      // New Curve
      curves.push_back(curve);

      curve = DiscreteCurve();
      curve.frame = compute_heading_frame(map_points[i].point, map_points[i + 1].point);
      map_in_curve = curve.frame.inverse();

      PointSpeedPair pair;
      pair.point = map_in_curve * map_points[i].point;
      pair.speed = map_points[i].speed;

      curve.points.push_back(pair);  // Include first point in curve
    }
  }

  curves.push_back(curve);

  return curves;
}

std::vector<PointSpeedPair> InLaneCruisingPlugin::constrain_to_time_boundary(const std::vector<PointSpeedPair>& points,
                                                                             double time_span)
{
  std::vector<lanelet::BasicPoint2d> basic_points;
  std::vector<double> speeds;
  splitPointSpeedPairs(points, &basic_points, &speeds);

  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(basic_points);

  size_t time_boundary_exclusive_index =
      trajectory_utils::time_boundary_index(downtracks, speeds, config_.trajectory_time_length);

  if (time_boundary_exclusive_index == 0)
  {
    throw std::invalid_argument("No points to fit in timespan");
  }

  std::vector<PointSpeedPair> time_bound_points;
  time_bound_points.reserve(time_boundary_exclusive_index);

  if (time_boundary_exclusive_index == points.size())
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.end());  // All points fit within time boundary
  }
  else
  {
    time_bound_points.insert(time_bound_points.end(), points.begin(),
                             points.begin() + time_boundary_exclusive_index - 1);  // Limit points by time boundary
  }

  return time_bound_points;
}

std::vector<cav_msgs::TrajectoryPlanPoint> InLaneCruisingPlugin::compose_trajectory_from_centerline(
    const std::vector<PointSpeedPair>& points, const cav_msgs::VehicleState& state)
{
  ROS_DEBUG_STREAM("VehicleState: "
                   << " x: " << state.X_pos_global << " y: " << state.Y_pos_global << " yaw: " << state.orientation
                   << " speed: " << state.longitudinal_vel);

  ROS_DEBUG_STREAM("points size: " << points.size());

  log::printDebugPerLine(points, &log::pointSpeedPairToStream);

  int nearest_pt_index = getNearestPointIndex(points, state);

  ROS_DEBUG_STREAM("NearestPtIndex: " << nearest_pt_index);

  std::vector<PointSpeedPair> future_points(points.begin() + nearest_pt_index + 1, points.end()); // Points in front of current vehicle position

  auto time_bound_points = constrain_to_time_boundary(future_points, config_.trajectory_time_length);

  ROS_DEBUG_STREAM("time_bound_points: " << time_bound_points.size());

  log::printDebugPerLine(time_bound_points, &log::pointSpeedPairToStream);

  ROS_DEBUG("Got basic points ");
  std::vector<DiscreteCurve> sub_curves = compute_sub_curves(time_bound_points);

  ROS_DEBUG_STREAM("Got sub_curves " << sub_curves.size());

  std::vector<double> final_yaw_values;
  std::vector<double> final_actual_speeds;
  std::vector<lanelet::BasicPoint2d> all_sampling_points;

  for (const auto& discreet_curve : sub_curves)
  {
    ROS_DEBUG("SubCurve");

    std::vector<double> speed_limits;
    std::vector<lanelet::BasicPoint2d> curve_points;
    splitPointSpeedPairs(discreet_curve.points, &curve_points, &speed_limits);

    std::unique_ptr<smoothing::SplineI> fit_curve = compute_fit(curve_points); // Compute splines based on curve points

    if (!fit_curve)
    {  // TODO how better to handle this case
      for (size_t i = 0; i < discreet_curve.points.size() - 1; i++)
      {
        Eigen::Isometry2d point_in_map =
            curvePointInMapTF(discreet_curve.frame, discreet_curve.points[i].point, final_yaw_values.back());
        all_sampling_points.push_back(point_in_map.translation());
        final_yaw_values.push_back(final_yaw_values.back());
        final_actual_speeds.push_back(final_actual_speeds.back());
      }
      continue;
    }

    ROS_DEBUG("Got fit");

    ROS_DEBUG_STREAM("speed_limits.size() " << speed_limits.size());

    std::vector<lanelet::BasicPoint2d> sampling_points;
    sampling_points.reserve(1 + discreet_curve.points.size() * 2);

    std::vector<double> distributed_speed_limits;
    distributed_speed_limits.reserve(1 + discreet_curve.points.size() * 2);

    double max_x = curve_points.back().x();
    double current_dist = 0;
    double step_size = config_.curve_resample_step_size;
    int current_speed_index = 0;

    while (current_dist < max_x - step_size) // Resample curve at tighter resolution
    {
      double x = current_dist;
      double y = (*fit_curve)(x);
      lanelet::BasicPoint2d p(x, y);
      sampling_points.push_back(p);

      for (size_t i = current_speed_index; i < curve_points.size(); i++)
      {
        if (curve_points[i].x() >= current_dist)
        {
          current_speed_index = i;
          break;
        }
      }

      distributed_speed_limits.push_back(speed_limits[current_speed_index]); // Identify speed limits for resampled points
      current_dist += step_size;
    }

    log::printDebugPerLine(sampling_points, &log::basicPointToStream);

    std::vector<double> yaw_values = carma_wm::geometry::compute_tangent_orientations(sampling_points);

    std::vector<double> curvatures = carma_wm::geometry::local_circular_arc_curvatures(
        sampling_points, config_.curvature_calc_lookahead_count);

    curvatures = smoothing::moving_average_filter(curvatures, config_.moving_average_window_size);

    log::printDoublesPerLineWithPrefix("curvatures[i]: ", curvatures);

    std::vector<double> ideal_speeds =
        trajectory_utils::constrained_speeds_for_curvatures(curvatures, config_.lateral_accel_limit);

    log::printDoublesPerLineWithPrefix("ideal_speeds: ", ideal_speeds);

    std::vector<double> actual_speeds = apply_speed_limits(ideal_speeds, distributed_speed_limits);

    log::printDoublesPerLineWithPrefix("actual_speeds: ", actual_speeds);

    log::printDoublesPerLineWithPrefix("yaw_values[i]: ", yaw_values);

    for (int i = 0; i < yaw_values.size() - 1; i++)
    {  // Drop last point

      Eigen::Isometry2d point_in_map = curvePointInMapTF(discreet_curve.frame, sampling_points[i], yaw_values[i]);
      Eigen::Rotation2Dd new_rot(point_in_map.rotation());
      final_yaw_values.push_back(new_rot.smallestAngle());
      all_sampling_points.push_back(point_in_map.translation());
    }

    final_actual_speeds.insert(final_actual_speeds.end(), actual_speeds.begin(), actual_speeds.end() - 1);

    ROS_DEBUG("Appended to final");
  }

  ROS_DEBUG("Processed all curves");

  if (all_sampling_points.size() == 0)
  {
    ROS_WARN_STREAM("No trajectory points could be generated");
    return {};
  }

  log::printDoublesPerLineWithPrefix("final_actual_speeds[i]: ", final_actual_speeds);

  log::printDoublesPerLineWithPrefix("final_yaw_values[i]: ", final_yaw_values);

  // Find Lookahead Distance based on Velocity
  double lookahead_distance = get_adaptive_lookahead(state.longitudinal_vel);

  ROS_DEBUG_STREAM("Lookahead distance at current speed: " << lookahead_distance);

  // Apply lookahead speeds
  final_actual_speeds = get_lookahead_speed(all_sampling_points, final_actual_speeds, lookahead_distance);

  // Add current vehicle point to front of the trajectory
  lanelet::BasicPoint2d cur_veh_point(state.X_pos_global, state.Y_pos_global);
  all_sampling_points.insert(all_sampling_points.begin(),
                             cur_veh_point);  // Add current vehicle position to front of sample points

  final_actual_speeds.insert(final_actual_speeds.begin(), std::max(state.longitudinal_vel, config_.minimum_speed));

  final_yaw_values.insert(final_yaw_values.begin(), state.orientation);

  log::printDoublesPerLineWithPrefix("pre_smoot[i]: ", final_actual_speeds);

  // Compute points to local downtracks
  std::vector<double> downtracks = carma_wm::geometry::compute_arc_lengths(all_sampling_points);

  log::printDoublesPerLineWithPrefix("post_shift[i]: ", final_actual_speeds);

  // Apply accel limits
  final_actual_speeds = trajectory_utils::apply_accel_limits_by_distance(downtracks, final_actual_speeds,
                                                                         config_.max_accel, config_.max_accel);
  log::printDoublesPerLineWithPrefix("postAccel[i]: ", final_actual_speeds);

  final_actual_speeds = smoothing::moving_average_filter(final_actual_speeds, config_.moving_average_window_size);
  log::printDoublesPerLineWithPrefix("post_average[i]: ", final_actual_speeds);

  for (auto& s : final_actual_speeds)  // Limit minimum speed. TODO how to handle stopping?
  {
    s = std::max(s, config_.minimum_speed);
  }

  log::printDoublesPerLineWithPrefix("post_min_speed[i]: ", final_actual_speeds);
  // Convert speeds to times
  std::vector<double> times;
  trajectory_utils::conversions::speed_to_time(downtracks, final_actual_speeds, &times);

  log::printDoublesPerLineWithPrefix("times[i]: ", times);

  // Build trajectory points
  // TODO When more plugins are implemented that might share trajectory planning the start time will need to be based
  // off the last point in the plan if an earlier plan was provided
  std::vector<cav_msgs::TrajectoryPlanPoint> traj_points =
      trajectory_from_points_times_orientations(all_sampling_points, times, final_yaw_values, ros::Time::now());

  return traj_points;
}

double InLaneCruisingPlugin::get_adaptive_lookahead(double velocity){

  // lookahead:
  // v<10kph:  5m
  // 10kph<v<50kph:  0.5*v
  // v>50kph:  25m

  double lookahead = config_.minimum_lookahead_distance;

  if (velocity < config_.minimum_lookahead_speed)
  {
    lookahead = config_.minimum_lookahead_distance;
  }
  else if (velocity >= config_.minimum_lookahead_speed && velocity < config_.maximum_lookahead_speed)
  {
    lookahead = config_.lookahead_ratio * velocity;
  }
  else lookahead = config_.maximum_lookahead_distance;

  return lookahead;

}

std::vector<double> InLaneCruisingPlugin::get_lookahead_speed(const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& speeds, const double& lookahead){

  if (lookahead < config_.minimum_lookahead_distance)
  {
    throw std::invalid_argument("Invalid lookahead value");
  }

  if (speeds.size() < 1)
  {
    throw std::invalid_argument("Invalid speeds vector");
  }

  if (speeds.size() != points.size())
  {
    throw std::invalid_argument("Speeds and Points lists not same size");
  }

  std::vector<double> out;
  out.reserve(speeds.size());

  for (int i = 0; i < points.size(); i++)
  {
    int idx = i;
    double min_dist = std::numeric_limits<double>::max();
    for (int j=i+1; j < points.size(); j++){
      double dist = lanelet::geometry::distance2d(points[i],points[j]);
      if (abs(lookahead - dist) <= min_dist){
        idx = j;
        min_dist = abs(lookahead - dist);
      }
    }
    out.push_back(speeds[idx]);
  }

  return out;
}

Eigen::Isometry2d InLaneCruisingPlugin::curvePointInMapTF(const Eigen::Isometry2d& curve_in_map,
                                                          const lanelet::BasicPoint2d& p, double yaw) const
{
  Eigen::Rotation2Dd yaw_rot(yaw);
  Eigen::Isometry2d point_in_c = carma_wm::geometry::build2dEigenTransform(p, yaw_rot);
  Eigen::Isometry2d point_in_map = curve_in_map * point_in_c;
  return point_in_map;
}

std::vector<cav_msgs::TrajectoryPlanPoint> InLaneCruisingPlugin::trajectory_from_points_times_orientations(
    const std::vector<lanelet::BasicPoint2d>& points, const std::vector<double>& times, const std::vector<double>& yaws,
    ros::Time startTime)
{
  if (points.size() != times.size() || points.size() != yaws.size())
  {
    throw std::invalid_argument("All input vectors must have the same size");
  }

  std::vector<cav_msgs::TrajectoryPlanPoint> traj;
  traj.reserve(points.size());

  for (int i = 0; i < points.size(); i++)
  {
    cav_msgs::TrajectoryPlanPoint tpp;
    ros::Duration relative_time(times[i]);
    tpp.target_time = startTime + relative_time;
    tpp.x = points[i].x();
    tpp.y = points[i].y();
    tpp.yaw = yaws[i];

    tpp.controller_plugin_name = "default";
    tpp.planner_plugin_name = plugin_discovery_msg_.name;

    traj.push_back(tpp);
  }

  return traj;
}

std::vector<PointSpeedPair> InLaneCruisingPlugin::maneuvers_to_points(const std::vector<cav_msgs::Maneuver>& maneuvers,
                                                                      double max_starting_downtrack,
                                                                      const carma_wm::WorldModelConstPtr& wm)
{
  std::vector<PointSpeedPair> points_and_target_speeds;
  std::unordered_set<lanelet::Id> visited_lanelets;

  bool first = true;
  ROS_DEBUG_STREAM("VehDowntrack: " << max_starting_downtrack);
  for (const auto& manuever : maneuvers)
  {
    if (manuever.type != cav_msgs::Maneuver::LANE_FOLLOWING)
    {
      throw std::invalid_argument("In-Lane Cruising does not support this maneuver type");
    }

    cav_msgs::LaneFollowingManeuver lane_following_maneuver = manuever.lane_following_maneuver;

    double starting_downtrack = lane_following_maneuver.start_dist;
    if (first)
    {
      if (starting_downtrack > max_starting_downtrack)
      {
        starting_downtrack = max_starting_downtrack;
      }
      first = false;
    }

    ROS_DEBUG_STREAM("Used downtrack: " << starting_downtrack);

    auto lanelets = wm->getLaneletsBetween(starting_downtrack, lane_following_maneuver.end_dist, true);

    ROS_DEBUG_STREAM("Maneuver");
    std::vector<lanelet::ConstLanelet> lanelets_to_add;
    for (auto l : lanelets)
    {
      ROS_DEBUG_STREAM("Lanelet ID: " << l.id());
      if (visited_lanelets.find(l.id()) == visited_lanelets.end())
      {
        lanelets_to_add.push_back(l);
        visited_lanelets.insert(l.id());
      }
    }

    lanelet::BasicLineString2d route_geometry = carma_wm::geometry::concatenate_lanelets(lanelets_to_add);

    first = true;
    for (auto p : route_geometry)
    {
      if (first && points_and_target_speeds.size() != 0)
      {
        first = false;
        continue;  // Skip the first point if we have already added points from a previous maneuver to avoid duplicates
      }
      PointSpeedPair pair;
      pair.point = p;
      pair.speed = lane_following_maneuver.end_speed;
      points_and_target_speeds.push_back(pair);
    }
  }

  return points_and_target_speeds;
}

int InLaneCruisingPlugin::getNearestPointIndex(const std::vector<PointSpeedPair>& points,
                                               const cav_msgs::VehicleState& state)
{
  lanelet::BasicPoint2d veh_point(state.X_pos_global, state.Y_pos_global);
  ROS_DEBUG_STREAM("veh_point: " << veh_point.x() << ", " << veh_point.y());
  double min_distance = std::numeric_limits<double>::max();
  int i = 0;
  int best_index = 0;
  for (const auto& p : points)
  {
    double distance = lanelet::geometry::distance2d(p.point, veh_point);
    ROS_DEBUG_STREAM("distance: " << distance);
    ROS_DEBUG_STREAM("p: " << p.point.x() << ", " << p.point.y());
    if (distance < min_distance)
    {
      best_index = i;
      min_distance = distance;
    }
    i++;
  }

  return best_index;
}

void InLaneCruisingPlugin::splitPointSpeedPairs(const std::vector<PointSpeedPair>& points,
                                                std::vector<lanelet::BasicPoint2d>* basic_points,
                                                std::vector<double>* speeds)
{
  basic_points->reserve(points.size());
  speeds->reserve(points.size());

  for (const auto& p : points)
  {
    basic_points->push_back(p.point);
    speeds->push_back(p.speed);
  }
}

std::unique_ptr<smoothing::SplineI>
InLaneCruisingPlugin::compute_fit(const std::vector<lanelet::BasicPoint2d>& basic_points)
{
  if (basic_points.size() < 3)
  {
    ROS_WARN_STREAM("Insufficient Spline Points");
    return nullptr;
  }

  std::unique_ptr<smoothing::SplineI> spl = std::make_unique<smoothing::CubicSpline>();
  spl->setPoints(basic_points);

  return spl;
}
}  // namespace inlanecruising_plugin