/*
 *            Copyright 2009-2020 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

// Third party includes
#include <boost/math/constants/constants.hpp>

// Local VOTCA includes
#include "votca/xtp/aobasis.h"
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/qmmolecule.h"
#include "votca/xtp/radial_euler_maclaurin_rule.h"

namespace votca {
namespace xtp {

std::vector<double> EulerMaclaurinGrid::CalculatePruningIntervals(
    const std::string& element) {
  std::vector<double> r;
  // get Bragg-Slater Radius for this element
  double BSradius = _BraggSlaterRadii.at(element);
  // row type of element
  Index RowType = _pruning_set.at(element);

  if (RowType == 1) {
    r.push_back(0.25 * BSradius);
    r.push_back(0.5 * BSradius);
    r.push_back(1.0 * BSradius);
    r.push_back(4.5 * BSradius);
  } else if (RowType == 2) {
    r.push_back(0.1667 * BSradius);
    r.push_back(0.5 * BSradius);
    r.push_back(0.9 * BSradius);
    r.push_back(3.5 * BSradius);
  } else if (RowType == 3) {
    r.push_back(0.1 * BSradius);
    r.push_back(0.4 * BSradius);
    r.push_back(0.8 * BSradius);
    r.push_back(2.5 * BSradius);
  } else {
    throw std::runtime_error(
        "EulerMaclaurinGrid::CalculatePruningIntervals:Pruning unsupported for "
        "RowType");
  }
  return r;
}

void EulerMaclaurinGrid::FillElementRangeMap(const AOBasis& aobasis,
                                             const QMMolecule& atoms,
                                             double eps) {
  std::map<std::string, min_exp>::iterator it;
  for (const QMAtom& atom : atoms) {
    std::string name = atom.getElement();
    // is this element already in map?
    it = _element_ranges.find(name);
    // only proceed, if element data does not exist yet
    if (it == _element_ranges.end()) {
      min_exp this_atom;
      double range_max = std::numeric_limits<double>::min();
      double decaymin = std::numeric_limits<double>::max();
      Index lvalue = std::numeric_limits<Index>::min();
      const std::vector<const AOShell*> shells =
          aobasis.getShellsofAtom(atom.getId());
      // and loop over all shells to figure out minimum decay constant and
      // angular momentum of this function
      for (const AOShell* shell : shells) {
        Index lmax = Index(shell->getL());
        if (shell->getMinDecay() < decaymin) {
          decaymin = shell->getMinDecay();
          lvalue = lmax;
        }
        double range = DetermineCutoff(2 * decaymin, 2 * lvalue + 2, eps);
        if (range > range_max) {
          this_atom.alpha = decaymin;
          this_atom.l = lvalue;
          this_atom.range = range;
          range_max = range;
        }
      }  // shells
      _element_ranges[name] = this_atom;
    }  // new element
  }    // atoms
}

void EulerMaclaurinGrid::RefineElementRangeMap(const AOBasis& aobasis,
                                               const QMMolecule& atoms,
                                               double eps) {
  AOOverlap overlap;
  overlap.Fill(aobasis);

  // get collapsed index list
  std::vector<Index> idxstart;
  const std::vector<Index>& idxsize = aobasis.getFuncPerAtom();
  Index start = 0;
  for (Index size : idxsize) {
    idxstart.push_back(start);
    start += size;
  }
  // refining by going through all atom combinations
  for (Index i = 0; i < atoms.size(); ++i) {
    const QMAtom& atom_a = atoms[i];
    Index a_start = idxstart[i];
    Index a_size = idxsize[i];
    double range_max = std::numeric_limits<double>::min();
    // get preset values for this atom type
    double alpha_a = _element_ranges.at(atom_a.getElement()).alpha;
    Index l_a = _element_ranges.at(atom_a.getElement()).l;
    const Eigen::Vector3d& pos_a = atom_a.getPos();
    // Cannot iterate only over j<i because it is not symmetric due to shift_2g
    for (Index j = 0; j < atoms.size(); ++j) {
      if (i == j) {
        continue;
      }
      const QMAtom& atom_b = atoms[j];
      Index b_start = idxstart[j];
      Index b_size = idxsize[j];
      const Eigen::Vector3d& pos_b = atom_b.getPos();
      // find overlap block of these two atoms
      Eigen::MatrixXd overlapblock =
          overlap.Matrix().block(a_start, b_start, a_size, b_size);
      // determine abs max of this block
      double s_max = overlapblock.cwiseAbs().maxCoeff();

      if (s_max > 1e-5) {
        double range = DetermineCutoff(
            alpha_a + _element_ranges.at(atom_b.getElement()).alpha,
            l_a + _element_ranges.at(atom_b.getElement()).l + 2, eps);
        // now do some update trickery from Gaussian product formula
        double dist = (pos_b - pos_a).norm();
        double shift_2g =
            dist * alpha_a /
            (alpha_a + _element_ranges.at(atom_b.getElement()).alpha);
        range += (shift_2g + dist);
        if (range > range_max) {
          range_max = range;
        }
      }
    }
    if (std::round(range_max) > _element_ranges.at(atom_a.getElement()).range) {
      _element_ranges.at(atom_a.getElement()).range = std::round(range_max);
    }
  }
}

void EulerMaclaurinGrid::CalculateRadialCutoffs(const AOBasis& aobasis,
                                                const QMMolecule& atoms,
                                                const std::string& gridtype) {

  double eps = Accuracy[gridtype];
  FillElementRangeMap(aobasis, atoms, eps);
  RefineElementRangeMap(aobasis, atoms, eps);
  return;
}

std::map<std::string, GridContainers::radial_grid>
    EulerMaclaurinGrid::CalculateAtomicRadialGrids(const AOBasis& aobasis,
                                                   const QMMolecule& atoms,
                                                   const std::string& type) {

  CalculateRadialCutoffs(aobasis, atoms, type);
  std::map<std::string, GridContainers::radial_grid> result;
  for (const auto& element : _element_ranges) {
    result[element.first] = CalculateRadialGridforAtom(type, element);
  }
  return result;
}

GridContainers::radial_grid EulerMaclaurinGrid::CalculateRadialGridforAtom(
    const std::string& type, const std::pair<std::string, min_exp>& element) {
  GridContainers::radial_grid result;
  Index np = getGridParameters(element.first, type);
  double cutoff = element.second.range;
  result.radius = Eigen::VectorXd::Zero(np);
  result.weight = Eigen::VectorXd::Zero(np);
  double alpha =
      -cutoff /
      (log(1.0 - std::pow((1.0 + double(np)) / (2.0 + double(np)), 3)));
  double factor = 3.0 / (1.0 + double(np));

  for (Index i = 0; i < np; i++) {
    double q = double(i + 1) / (double(np) + 1.0);
    double r = -alpha * std::log(1.0 - std::pow(q, 3));
    double w = factor * alpha * r * r / (1.0 - std::pow(q, 3)) * std::pow(q, 2);
    result.radius[i] = r;
    result.weight[i] = w;
  }
  return result;
}

double EulerMaclaurinGrid::DetermineCutoff(double alpha, Index l, double eps) {
  // determine norm of function
  /* For a function f(r) = r^k*exp(-alpha*r^2) determine
     the radial distance r such that the fraction of the
     function norm that is neglected if the 3D volume
     integration is terminated at a distance r is less
     than or equal to eps. */

  double cutoff = 1.0;     // initial value
  double increment = 0.5;  // increment

  while (increment > 0.01) {
    double residual = CalcResidual(alpha, l, cutoff);
    if (residual > eps) {
      cutoff += increment;
    } else {
      cutoff -= increment;
      if (cutoff < 0.0) {
        cutoff = 0.0;
      }
      increment = 0.5 * increment;
      cutoff += increment;
    }
  }
  return cutoff;
}

double EulerMaclaurinGrid::CalcResidual(double alpha, Index l, double cutoff) {
  return RadialIntegral(alpha, l + 2, cutoff) /
         RadialIntegral(alpha, l + 2, 0.0);
}

double EulerMaclaurinGrid::RadialIntegral(double alpha, Index l,
                                          double cutoff) {
  const double pi = boost::math::constants::pi<double>();
  Index ilo = l % 2;
  double value = 0.0;
  double valexp;
  if (ilo == 0) {
    double expo = std::sqrt(alpha) * cutoff;
    if (expo <= 40.0) {
      value = 0.5 * std::sqrt(pi / alpha) * std::erfc(expo);
    }
  }
  double exponent = alpha * cutoff * cutoff;
  if (exponent > 500.0) {
    valexp = 0.0;
    value = 0.0;
  } else {
    valexp = std::exp(-exponent);
    value = valexp / 2.0 / alpha;
  }
  for (Index i = ilo + 2; i <= l; i += 2) {
    value = (double(i - 1) * value + std::pow(cutoff, i - 1) * valexp) / 2.0 /
            alpha;
  }
  return value;
}

Index EulerMaclaurinGrid::getGridParameters(const std::string& element,
                                            const std::string& type) {
  if (type == "medium") {
    return MediumGrid.at(element);
  } else if (type == "coarse") {
    return CoarseGrid.at(element);
  } else if (type == "xcoarse") {
    return XcoarseGrid.at(element);
  } else if (type == "fine") {
    return FineGrid.at(element);
  } else if (type == "xfine") {
    return XfineGrid.at(element);
  }
  throw std::runtime_error("Grid type " + type + " is not implemented");
  return -1;
}
}  // namespace xtp
}  // namespace votca


#include "NumberValue.hpp"
#include <boost/lexical_cast.hpp>
#include "string_traits.hpp"

namespace aslse = axis::services::language::syntax::evaluation;

aslse::NumberValue::NumberValue( long value ) :
_longValue(value), _doubleValue((double)value), _isInteger(true)
{
	/* nothing to do here */
}

aslse::NumberValue::NumberValue( double value ) :
_longValue((long)value), _doubleValue(value), _isInteger(false)
{
	/* nothing to do here */
}

aslse::NumberValue::~NumberValue( void )
{
	/* nothing to do here */
}

bool aslse::NumberValue::IsString( void ) const
{
	return false;
}

bool aslse::NumberValue::IsId( void ) const
{
	return false;
}

bool aslse::NumberValue::IsNumeric( void ) const
{
	return true;
}

bool aslse::NumberValue::IsInteger( void ) const
{
	return _isInteger;
}

long aslse::NumberValue::GetLong( void ) const
{
	return _longValue;
}

double aslse::NumberValue::GetDouble( void ) const
{
	return _doubleValue;
}

axis::String aslse::NumberValue::ToString( void ) const
{
	axis::String str;
	if (_isInteger)
	{
		str = boost::lexical_cast<axis::String>(_longValue);
	}
	else
	{
		str = boost::lexical_cast<axis::String>(_doubleValue);
	}
	return str;
}

aslse::ParameterValue& aslse::NumberValue::Clone( void ) const
{
	if (_isInteger)
	{
		return *new NumberValue(_longValue);
	}
	else
	{
		return *new NumberValue(_doubleValue);
	}
}


/**
 * @file sample_attractors.cpp
 *
 * This file contains example code explained in mini-tutorial @ref sample_attractors.
 */

#include <cstdlib>
#include <iostream>
#include <algorithm>

#include <boost/ptr_container/indirect_fun.hpp>

#include <BnSimulator/core/BooleanNetwork.hpp>
#include <BnSimulator/util/utility.hpp>
#include <BnSimulator/experiment/GardenerRunner.hpp>
#include <BnSimulator/lab/PrintAttractor.hpp>

namespace bn {

/**
 * @internal
 * Namespace for classes and function used in the examples.
 *
 * This namespace is used to avoid cluttering the global namespace.
 */
namespace example {

/**
 * Adapter class to print Attractor objects referred to by a pointer.
 *
 * It is implemented only for didactical purposes since it is more concise and
 * safe to use a combination of bn::PrintAttractor with
 * Boost Pointer Container
 * library functionalities.
 */
struct MyPrintAdapter {
	/**
	 * Adapted object.
	 */
	bn::PrintAttractor adapted;

	/**
	 * Constructor for the adaptor.
	 *
	 * Has the same signature as the constructor of bn::PrintAttractor::PrintAttractor(Rbn&,std::ostream&).
	 * @param net a Boolean Network
	 * @param out an output stream
	 */
	MyPrintAdapter(BooleanNetwork& net, std::ostream& out) :
		adapted(net, out) {
	}

	/**
	 * Method wrapper around bn::PrintAttractor::operator()().
	 * @param a an attractor to print
	 */
	void operator()(Attractor* a) const {
		adapted(*a);
	}
};

}

}

/**
 * Entry point for this program.
 *
 * It accepts the following required parameters in order:
 * @li number of nodes in the network
 * @li path to topology file
 * @li path to node function file
 * @li maximum number of steps before the simulator gives up on finding
 * an attractor
 * @li number of initial random states (non necessairly different)
 * @li seed for the random number generator
 */
int main(int argc, char **argv) {
	using namespace bn;
	// network and parameter initialization
	const uint maxSteps = std::atoi(argv[4]);
	const uint samples = std::atoi(argv[5]);
	BooleanNetwork net = BooleanNetwork::makeNetwork(std::atoi(argv[1]),
			argv[2], argv[3]);
	std::srand(std::atoi(argv[6]));
	// experiment initialization
	GardenerRunner gardener(maxSteps, samples);
	// run experiment
	GardenerRunner::Garden g = gardener.findGarden(net);
	// post-process results
	std::for_each(g.begin(), g.end(), example::MyPrintAdapter(net, std::cout));
	std::for_each(g.begin(), g.end(), boost::make_indirect_fun(PrintAttractor(
			net, std::cout)));
	// clean-up code
	util::purge(g); // free memory used by Attractor objects in g
	return EXIT_SUCCESS;
}


#include <cmath>
#include <iostream>
#include <iomanip>
#include <string>
#include <vector>
#include <algorithm>
#include <boost/range/algorithm.hpp>
#include <boost/integer/common_factor_rt.hpp>

using namespace std;

int N;
int main() {
    while (cin >> N) {
        int A[N], max_i = 0, max_2nd_i = 0;
        cin >> A[0];
        cin >> A[1];
        if (A[0] < A[1]) {
            max_i = 1;
            max_2nd_i = 0;
        } else {
            max_i = 0;
            max_2nd_i = 1;
        }
        for (int i = 2; i < N; i++) {
            cin >> A[i];
            if (A[max_i] < A[i]) {
                max_2nd_i = max_i;
                max_i = i;
            } else if (A[max_2nd_i] < A[i]) {
                max_2nd_i = i;
            }
        }
        for (int i = 0; i < N; i++) {
            cout << A[(i == max_i) ? max_2nd_i : max_i]  << endl;
        }
    }
    return 0;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

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

 /*!

    @ingroup group-predicates
    Function object implementing is_flint capabilities

    Returns @ref True or @ref False according x is a flint or not.

    @par Semantic:

    @code
    as_logical_t<T> r = is_flint(x);
    @endcode

    @par Note:

    A flint is a 'floating integer' i.e. a floating number
    representing exactly an integer value.

    Be conscious that all sufficiently great floating points values
    are flint and even are even...

  **/
  const boost::dispatch::functor<tag::is_flint_> is_flint = {};
} }
#endif

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

#endif


//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012        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 NT2_SDK_UNIT_TESTS_ULP_HPP_INCLUDED
#define NT2_SDK_UNIT_TESTS_ULP_HPP_INCLUDED

/*!
  @file
  @brief Precision testing macros
**/

#include <iomanip>
#include <iostream>
#include <nt2/sdk/unit/stats.hpp>
#include <nt2/sdk/unit/details/eval.hpp>
#include <nt2/sdk/unit/details/ulp.hpp>
#include <boost/current_function.hpp>

namespace nt2 { namespace details
{
  template<typename A, typename B>
  void test_ulp_equal ( const char* desc
                      , const char* func, int line
                      , A const& a, B const& b
                      , double N
                      )
  {
    typedef BOOST_TYPEOF(nt2::unit::eval(a))                        a_t;
    typedef BOOST_TYPEOF(nt2::unit::eval(b))                        b_t;
    typedef typename nt2::details::max_ulp_<a_t,b_t>::failure_type  f_t;

    std::vector< f_t > ulps;
    double ulpd = 0;

    bool ok = nt2::unit::max_ulp( nt2::unit::eval(a)
                                , nt2::unit::eval(b)
                                , N, ulps, ulpd
                                );
    if(ok)
    {
      if( ulps.empty() )
      {
        ::nt2::details::ulp_pass( desc, ulpd, N );
      }
      else
      {
        ::nt2::details::ulp_fail( desc, func, line, ulps.size(),N, ok);

        BOOST_FOREACH ( f_t const & f, ulps )
        {
          std::cout << std::setprecision(20)
                    << "\tlhs: "  << f.value
                    << ", rhs: "  << f.desired_value
                    << ", ULP: "  << f.ulp_error
                    << ", @( "    << f.index << " )";
          std::cout << std::endl;
        }

        std::cout << std::endl;
      }
    }
    else
    {
      ::nt2::details::ulp_fail( desc, func, line, ulps.size(),N, ok);
    }
  }
} }

/*!
  @brief Check for absolute precision

  For two given values @c A and @c B, consider the test successful if and only
  if the absolute difference between @c A and @b is less or equal to @c N.

  @usage
  @include test_ulp.cpp
**/
#define NT2_TEST_ULP_EQUAL(A, B, N)                               \
do                                                                \
{                                                                 \
  nt2::unit::test_count()++;                                      \
                                                                  \
  nt2::details::test_ulp_equal( #A " == " #B                      \
                              , BOOST_CURRENT_FUNCTION, __LINE__  \
                              , A, B, N                           \
                              );                                  \
}                                                                 \
while(0)                                                          \
/**/

#endif


#include <openrave/openrave.h>
#include <openrave/plugin.h>
#include <openrave/planningutils.h>

#include <boost/bind.hpp>
#include <boost/algorithm/string/classification.hpp> // Include boost::for is_any_of
#include <boost/algorithm/string/split.hpp> // Include for boost::split

#include <iostream>
#include <string.h>
#include <tuple>
#include <ctime>
#include <cstdio>

#include "myplugin.h"

using namespace OpenRAVE;
using namespace std;

size_t MAX_ITERATIONS = 200;//used for smoothing
int RRTNode::CurrentID = 0;
dReal GOAL_BIAS = 0.16;//biasing towards goal 16% OPTIMAL
dReal delta_Q = 0.35;//step size
int flag = 0;//checks if goal is reached

typedef std::vector<OpenRAVE::dReal> Config; //configration for robot
typedef boost::shared_ptr<RRTNode> Node; //node containing configration and parents address
typedef boost::shared_ptr<NodeTree> Tree;//tree to hold vector of nodes

//main RRT class
class RRTModule : public ModuleBase
{

public:
    //to connect with python
    RRTModule(EnvironmentBasePtr penv, std::istream& ss) : ModuleBase(penv) {
        _penv = penv;
        RegisterCommand("MyCommand",boost::bind(&RRTModule::MyCommand,this,_1,_2),
                        "This is an example command hello");
    }
    virtual ~RRTModule() {}

    bool MyCommand(std::ostream& sout, std::istream& sinput)
    {
        cout << "*PLUGIN STARTED" << endl;
        //setting up clocks
        // clock_t start;

        // start = clock();
        //setenvironment and robot
        _penv->GetRobots(_robots);
        _robot = _robots.at(0);
        _robot->GetActiveDOFValues(_startConfig);

        // set goal config
         _goalConfig.push_back(0.449);
         _goalConfig.push_back(-0.201);
         _goalConfig.push_back(-0.151);
         _goalConfig.push_back(-0.11);
         _goalConfig.push_back(0);
         _goalConfig.push_back(-0.11);
         _goalConfig.push_back(0);

         //get active dof limits
        _robot->GetActiveDOFLimits(_lowerLimit, _upperLimit);

        //changing the limits of last and second last DOF
        _lowerLimit[4] = -3.14;
        _lowerLimit[6] = -3.14;

        _upperLimit[4] = 3.14;
        _upperLimit[6] = 3.14;

        /*
        Lower Limits:  Config:  -0.564602 -0.3536 -2.12131 -2.00001 -10000 -2.00001 -10000
        Upper Limits:  Config:   2.13539   1.2963 -0.15    -0.1      10000 -0.1      10000
        */
        //weights used while calculating neirest node
        _weight.push_back(3.17104);
        _weight.push_back(2.75674);
        _weight.push_back(2.2325);
        _weight.push_back(2.2325);
        _weight.push_back(1.78948);
        _weight.push_back(0);
        _weight.push_back(0.809013);
        _weight.push_back(0);

        //initialising tree
        _startNode = Node(new RRTNode());
        _startNode->setConfig(_startConfig);
        _startNode->setParent(nullptr);
        Config test = _startNode->getConfig();

        //adding start to tree
        _mainTree.setrootNode(_startNode);
        _mainTree.nodeAdd(_startNode);

        _randomConfig = _startConfig;
        //Main Algorithm Starts Here
        count = 0;
        int cc=0;

        //do until goal is reached
        while(count++ < 100000){

            //------------------------get random node
            _randomConfig = randomnodeGenerator();

            //------------------------check for nearest node in tree
            _nearestNode = NNNode();

            //------------------------Get the step size
             _stepSize = step();

            if(flag){
            //-----------------------if goal is reached, break out of while loop
                cout << "Goal Found" << endl;
                break;

            }else{
            //------------------------else get the next node
                EXTEND(count);
                //--------------------just to get an idea where we stand currently
                if(cc % 1000 == 0){
                    cout << "Searching.." << count << endl;
                }
                cc++;

            }

        }

        //clock stops as, we are out of loop with path searched :)
        // duration = (clock() - start)/(double)CLOCKS_PER_SEC;

        //get the path trajector
        getPath();

        //exectue the path using robots controller
        ExecuteTrajectory();

        cout << "*PLUGIN FINISHED" << endl;

        //draw the path
        Draw();

        //finish the loop
        return true;

    }

    //getPath from last found goal as nearest neibhour, used to get path from reached tree
    void getPath(){
        //save in _path
        _path.push_back(_nearestNode);

        while(_nearestNode->getParent() != nullptr){
            _nearestNode = _nearestNode->getParent();
            _path.push_back(_nearestNode);
        }

    }

    //Print config function
    void printConfig(string s ,Config config){
        cout << s <<" Config:  " ;
        for(size_t i = 0; i < 7 ; i++){
                cout << config[i] << ' ';
            }
            cout << endl;
    }

    //getStringVector,used to read vector from python as string
    vector<dReal> getStringVector(std::ostream& sout, std::istream& sinput){
        vector<std::string> words;
        string s;
        ostringstream os;
        os<<sinput.rdbuf();
        s=os.str();
        std::vector<dReal> goal_config;
        boost::split(words, s, boost::is_any_of(", "), boost::token_compress_on);
        for(int i=0; i<7; i++){
            dReal num = atof(words.at(i).c_str());
            goal_config.push_back(num);
        }
        return goal_config;
    }

    //check if config are in limits
    bool checkConfigLimits(Config config){
    /*
        Lower Limits:  Config:  -0.564602 -0.3536 -2.12131 -2.00001 -10000 -2.00001 -10000
        Upper Limits:  Config:   2.13539   1.2963 -0.15    -0.1      10000 -0.1      10000
    */
        int sum = 0;
        for(size_t i = 0; i<7 ;i++){
            if(config[i] < _upperLimit[i] && config[i] > _lowerLimit[i]){
                sum++;
                // cout << " i : " << i;
            }
        }
        if(sum==7)
            return true;
        else
            return false;

    }

    //generate random node
    Config randomnodeGenerator(){

        //generate random number
        dReal bias= RaveRandomFloat();

        Config temp_config = _goalConfig;

        if(bias < GOAL_BIAS){
            temp_config = _goalConfig;
        }else{
            do{

                for (size_t i = 0; i < 7; ++i) {

                    temp_config[i] = static_cast<dReal>(bias*(_upperLimit[i] - _lowerLimit[i]) + _lowerLimit[i]);
                    bias = RaveRandomFloat();//change random configration next time <common sense , not so common :'D  :| >

                   }
                   //check for validity
                }while(CheckCollision(temp_config)||!inLimits(temp_config));
        }
        return temp_config;

    }

    //nearest node
   boost::shared_ptr<RRTNode> NNNode(){

        dReal temp_val = 0;
        dReal min_val = 1000000;
        dReal diff;
        size_t range = _mainTree.getTreeSize();
        vector< boost::shared_ptr<RRTNode> > temp_tree = _mainTree.getfullPath();

        for(size_t i=0; i<range; i++){

            Config temp_config = temp_tree[i]->getConfig();

            for(size_t j=0; j<7; j++){

                  temp_val += pow((-temp_config[j] + _randomConfig[j])*_weight[j], 2);
            }

            temp_val = sqrt(temp_val);

            diff = min_val - temp_val;

            if(diff > 0){
                min_val = temp_val;
                _nearestNode = temp_tree[i];
                _nearestNode->setUniqueId(i);
            }
        }

        return _nearestNode;
    }

   //difference berween node and goal
   dReal differenceCost(Config config){
        dReal temp_val;

        for(size_t j=0; j<7; j++){
                  temp_val += pow((-_goalConfig[j] + config[j]), 2);//*_weight[j]
            }

            temp_val = sqrt(temp_val);
            return temp_val;
   }

//difference between node and random config
    dReal differencebetweenTwo(Config config){
        dReal temp_val;
        for(size_t j=0; j<7; j++){
                  temp_val += pow((-_randomConfig[j] + config[j]), 2);//*_weight[j]
            }

            temp_val = sqrt(temp_val);
            return temp_val;
    }

    //find step size as vector in direcion toward random node from nearest node
   Config step(){

        Config temp_step = _randomConfig;
        dReal magnitude = 0.0;

        for(size_t i=0 ; i<7 ; i++){
            temp_step[i] = _randomConfig[i] - _nearestNode->getConfig()[i];
        }

        for(size_t i=0; i<7; i++){
            magnitude += pow(temp_step[i], 2.0);
        }
        magnitude = sqrt(magnitude);

        for (size_t i = 0; i < 7; ++i) {
            temp_step[i] = ((temp_step[i]) * delta_Q) / magnitude;
        }

        return temp_step;
   }

   //extend in the direction of nearest node
    void EXTEND(int count){
        string s;
        //flag = 0;
        Config temp_new_config = _nearestNode->getConfig();

        do{

            temp_new_config = _nearestNode->getConfig();

            for(size_t i=0; i<7 ; i++){
                temp_new_config[i] += _stepSize[i];

            }
            //if not in limits start over again
            if(!inLimits(temp_new_config) || CheckCollision(temp_new_config)){

                break;
            }

             _mainTree.nodeAdd(Node(new RRTNode(temp_new_config, _nearestNode)));
             _nearestNode = _mainTree.getLast();
            _diffConfig = differencebetweenTwo(temp_new_config);
            if(_diffConfig < delta_Q){
                _mainTree.nodeAdd(Node(new RRTNode(_randomConfig, _nearestNode)));
                _diffGoal = differenceCost(_randomConfig);

            if(_diffGoal < delta_Q){
                _mainTree.nodeAdd(Node(new RRTNode(_goalConfig, _nearestNode)));
                flag = 1;
                break;

            }

            }

        }while(!CheckCollision(temp_new_config) && inLimits(temp_new_config));// && checkConfigLimits(temp_new_config));
    }

    //check if config is in limit or not
    bool inLimits(Config config){
        for(size_t i=0; i<7; i++){
            if(config[i] < _lowerLimit[i] || config[i] > _upperLimit[i]){
                return false;
            }
        }
        return true;

    }
    //check for collision
    bool CheckCollision(Config config){

        _robot->SetActiveDOFValues(config);
        bool flag1 = _penv->CheckCollision(_robot);
        //bool flag2 = _penv->CheckSelfCollision(_robot);
        _robot->SetActiveDOFValues(_startConfig);
        return flag1 ;//|| flag2;
    }

    //execute trajectory Refference : OpenRave Examples
    void ExecuteTrajectory(){

        EnvironmentMutex& lock = _penv->GetMutex();
        lock.lock();
        TrajectoryBasePtr traj = RaveCreateTrajectory(_penv);
        traj->Init(_robot->GetActiveConfigurationSpecification());

        for(size_t i=0; i< _path.size(); i++){

            traj->Insert(0, _path[i]->getConfig());
        }
            traj->Insert(0, _startConfig);
            // traj->Insert(0, _ggoalConfig);
            planningutils::RetimeActiveDOFTrajectory(traj, _robot);
            _robot->GetController()->SetPath(traj);
            lock.unlock();
    }

    //Draw a point at the end effector position
    void Draw(){

        std::vector<float> endEffector;
        float red[4] = {1,0,0,1};

        for(size_t i=0 ; i<_path.size() ; i++ ){
            _robot->SetActiveDOFValues(_path[i]->getConfig());
            _robot->SetActiveManipulator("leftarm");
            RobotBase::ManipulatorPtr hand;
            hand = _robot->GetActiveManipulator();
            RaveVector<dReal> point = hand->GetEndEffectorTransform().trans;
            std::vector<float> endEffector;
            endEffector.push_back(point.x);
            endEffector.push_back(point.y);
            endEffector.push_back(point.z);

            handler.push_back(_penv->plot3(&endEffector[0],1,1,5,red,0,true));
        }
    }

private:
    //openrave variables
    vector<RobotBasePtr> _robots;

    EnvironmentBasePtr _penv;
    RobotBasePtr _robot;

    Config _startConfig ;
    Config _goalConfig;
    Config _randomConfig; //get it from random generator
    Config _lowerLimit;
    Config _upperLimit;
    Config _stepSize; //get from config
    Config _extendConfig_new; //has new config from extend function
    Config _weight; //weight matrix used while calculating cost
    Config _ggoalConfig;

    Node _startNode;
    Node _nearestNode;
    Node _extendNode_new;
    Node _goalNode;
    // Node _temp;

    NodeTree _mainTree;

    dReal _diffGoal;
    dReal _diffConfig;

    std::vector<GraphHandlePtr> handler;

    size_t count;

    vector< boost::shared_ptr<RRTNode> > _path;
};

// called to create a new plugin
InterfaceBasePtr CreateInterfaceValidated(InterfaceType type, const std::string& interfacename, std::istream& sinput, EnvironmentBasePtr penv)
{
    if( type == PT_Module && interfacename == "rrtmodule" ) {
        return InterfaceBasePtr(new RRTModule(penv,sinput));
    }

    return InterfaceBasePtr();
}

// called to query available plugins
void GetPluginAttributesValidated(PLUGININFO& info)
{
info.interfacenames[PT_Module].push_back("RRTModule");

}

// called before plugin is terminated
OPENRAVE_PLUGIN_API void DestroyPlugin()
{
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ored/marketdata/fxtriangulation.hpp>
#include <ql/errors.hpp>
#include <ql/quotes/compositequote.hpp>
#include <ql/quotes/derivedquote.hpp>
#include <ql/quotes/simplequote.hpp>

using namespace QuantLib;
using std::string;

namespace {
// helper classes to enable the building
// of composite cross-currency fx quotes
class Triangulation {
public:
    Triangulation() {}
    Real operator()(Real a, Real b) const { return a / b; }
};

class Product {
public:
    Product() {}
    Real operator()(Real a, Real b) const { return a * b; }
};

class InverseProduct {
public:
    InverseProduct() {}
    Real operator()(Real a, Real b) const { return 1.0 / (a * b); }
};

// Inverse a single quote
class Inverse {
public:
    Inverse() {}
    Real operator()(Real a) const { return 1.0 / a; }
};
} // namespace

namespace ore {
namespace data {

Handle<Quote> FXTriangulation::getQuote(const string& pair) const {
    // First, look for the pair in the map
    auto it = map_.find(pair);
    if (it != map_.end())
        return it->second;

    // Now we have to break the pair up and search for it.
    QL_REQUIRE(pair.size() == 6, "Invalid ccypair " << pair);
    string domestic = pair.substr(0, 3);
    string foreign = pair.substr(3);

    // check reverse
    string reverse = foreign + domestic;
    it = map_.find(reverse);
    if (it != map_.end()) {
        Handle<Quote> invertedQuote(boost::make_shared<DerivedQuote<Inverse>>(it->second, Inverse()));
        map_[pair] = invertedQuote;
        return invertedQuote;
    }

    // check EUREUR
    if (foreign == domestic) {
        static Handle<Quote> unity(boost::make_shared<SimpleQuote>(1.0));
        return unity;
    }

    // Now we search for a pair of quotes that we can combine to construct the quote required.
    // We only search for a pair of quotes a single step apart.
    //
    // Suppose we want a USDJPY quote and we have EUR based data, there are 4 combinations to
    // consider:
    // EURUSD, EURJPY  => we want EURJPY / EURUSD [Triangulation]
    // EURUSD, JPYEUR  => we want 1 / (EURUSD * JPYEUR) [InverseProduct]
    // USDEUR, EURJPY  => we want USDEUR * EURJPY [Product]
    // USDEUR, JPYEUR  => we want USDEUR / JPYEUR [Triangulation (but in the reverse order)]
    //
    // Loop over the map, look for domestic then use the map to find the other side of the pair.
    for (const auto& kv : map_) {
        string keyDomestic = kv.first.substr(0, 3);
        string keyForeign = kv.first.substr(3);
        const Handle<Quote>& q1 = kv.second;

        if (domestic == keyDomestic) {
            // we have domestic, now look for foreign/keyForeign
            // USDEUR, JPYEUR  => we want USDEUR / JPYEUR [Triangulation (but in the reverse order)]
            it = map_.find(foreign + keyForeign);
            if (it != map_.end()) {
                // Here q1 is USDEUR and it->second is JPYEUR
                map_[pair] =
                    Handle<Quote>(boost::make_shared<CompositeQuote<Triangulation>>(q1, it->second, Triangulation()));
                return map_[pair];
            }
            // USDEUR, EURJPY  => we want USDEUR * EURJPY [Product]
            it = map_.find(keyForeign + foreign);
            if (it != map_.end()) {
                map_[pair] = Handle<Quote>(boost::make_shared<CompositeQuote<Product>>(q1, it->second, Product()));
                return map_[pair];
            }
        }

        if (domestic == keyForeign) {
            // EURUSD, JPYEUR  => we want 1 / (EURUSD * JPYEUR) [InverseProduct]
            it = map_.find(foreign + keyDomestic);
            if (it != map_.end()) {
                map_[pair] =
                    Handle<Quote>(boost::make_shared<CompositeQuote<InverseProduct>>(q1, it->second, InverseProduct()));
                return map_[pair];
            }
            // EURUSD, EURJPY  => we want EURJPY / EURUSD [Triangulation]
            it = map_.find(keyDomestic + foreign);
            if (it != map_.end()) {
                // Here q1 is EURUSD and it->second is EURJPY
                map_[pair] =
                    Handle<Quote>(boost::make_shared<CompositeQuote<Triangulation>>(it->second, q1, Triangulation()));
                return map_[pair];
            }
        }
    }

    QL_FAIL("Unable to build FXQuote for ccy pair " << pair);
}
} // namespace data
} // namespace ore


//==============================================================================
//         Copyright 2015 J.T. Lapreste
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/bitwise/include/functions/bitset.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/three.hpp>

NT2_TEST_CASE_TPL ( bitset_uit,  BOOST_SIMD_UNSIGNED_TYPES)
{

  using boost::simd::bitset;
  using boost::simd::tag::bitset_;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type iT;
  typedef typename boost::dispatch::meta::call<bitset_(T,iT)>::type r_t;
  typedef T wished_r_t;

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

  // specific values tests
  NT2_TEST_EQUAL(bitset(T(2),1), T(3));
  NT2_TEST_EQUAL(bitset(T(3),2), T(3));
  NT2_TEST_EQUAL(bitset(T(3),3), T(7));
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( bitset_si,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::bitset;
  using boost::simd::tag::bitset_;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type iT;
  typedef typename boost::dispatch::meta::call<bitset_(T,iT)>::type r_t;
  typedef iT wished_r_t;

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

  // specific values tests
  NT2_TEST_EQUAL(bitset(T(2),1),T(3));
  NT2_TEST_EQUAL(bitset(T(2),2), T(2));
  NT2_TEST_EQUAL(bitset(T(-1), 1), T(-1));
} // end of test for unsigned_int_


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE MergeConstSgdVarUpdatesTransformation0

#include <../../random_util.hpp>
#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/devicemanager.hpp>
#include <popart/inputshapeinfo.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/session.hpp>
#include <popart/sgd.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>
#include <popart/testdevice.hpp>

#include <algorithm>
#include <map>
#include <memory>
#include <tuple>
#include <vector>

using namespace popart;

namespace {
TensorId conv(Builder *b, TensorId act, ConstVoidData wdata) {
  auto aiOnnx  = b->aiOnnxOpset9();
  auto weights = b->addInitializedInputTensor(wdata);
  act = aiOnnx.conv({act, weights}, {1, 1}, 1, {}, {1, 1, 1, 1}, {1, 1});
  return act;
}
} // namespace

BOOST_AUTO_TEST_CASE(Transformation_MergeConstSGD0) {

  auto test = [](MergeVarUpdateType mvu) {
    // we will generate random input data
    int seed = 1013;
    DefaultRandomEngine eng(seed);
    UniformRealDistribution<float> fdis(-4.f, +4.f);

    // construct onnx model
    auto builder = Builder::create();
    auto aiOnnx  = builder->aiOnnxOpset9();

    int nInChans  = 3;
    int batchsize = 1;
    TensorInfo in0info{"FLOAT",
                       std::vector<int64_t>{batchsize, nInChans, 32, 32}};
    auto in0 = builder->addInputTensor(in0info);
    std::vector<float> in0data(in0info.nelms());
    for (auto &val : in0data) {
      val = fdis(eng);
    }

    // The model will be reduce(conv(conv(conv(conv(....(conv(input))...)))),
    // nConv convolutions chained together, with the number of
    // channels increasing by 1 at each subsequent conv.
    constexpr int nConv = 11;
    std::array<std::vector<float>, nConv> weights;
    std::array<ConstVoidData, nConv> cvds;
    std::array<TensorId, nConv + 1> actIds;
    actIds[0] = in0;

    int64_t totalWeightElms = 0;

    for (int i = 0; i < nConv; ++i) {
      int nOutChans = nInChans + 1;
      TensorInfo weight_info{"FLOAT",
                             std::vector<int64_t>{nOutChans, nInChans, 1, 1}};

      totalWeightElms += nOutChans * nInChans;
      weights[i] = std::vector<float>(nOutChans * nInChans, 0);
      for (auto &x : weights[i]) {
        x = fdis(eng);
      }
      cvds[i]       = {weights[i].data(), weight_info};
      actIds[i + 1] = conv(builder.get(), actIds[i], cvds[i]);
      nInChans      = nOutChans;
    }

    auto reduced =
        aiOnnx.reducesum({actIds[nConv]}, std::vector<int64_t>{{1, 2, 3}});
    float lossLambda = 0.26;
    auto l1 = builder->aiGraphcoreOpset1().l1loss({reduced}, lossLambda);

    std::string proto = builder->getModelProto();
    auto modelProto   = io::getModelFromString(proto);

    // create the IR
    auto art      = AnchorReturnType("All");
    auto dataFlow = DataFlow(1, {{reduced, art}});

    auto device = popart::createTestDevice(TEST_TARGET);

    auto opts                       = SessionOptions();
    opts.enableOutlining            = false;
    opts.mergeVarUpdate             = mvu;
    opts.mergeVarUpdateMemThreshold = 100;

    float learnRate = 0.1;
    auto optimizer  = ConstSGD(learnRate);

    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                l1,
                &optimizer,
                *device,
                opts,
                Patterns({}).enableRuntimeAsserts(false)});

    // Check the ir
    //
    // For case MergeVarUpdateType::All, all the ConstSgdVarUpdates have the
    // same learning rate, weight decay, so should all be merged into a single
    // group
    if (mvu == MergeVarUpdateType::All) {
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate).size(), 1);
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::ReshapeInplace).size(),
          nConv * 2);

      // 2 ConcatInplace, one for all the Vars and one for all the Grads
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::ConcatInplace).size(), 2);

      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::CopyVarUpdate).size(), 0);

    } else if (mvu == MergeVarUpdateType::None) {
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate).size(), nConv);
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::ReshapeInplace).size(), 0);
      BOOST_CHECK_EQUAL(
          ir.opsOfType(Onnx::CustomOperators::ConcatInplace).size(), 0);
    }

    else if (mvu == MergeVarUpdateType::AutoTight) {

      auto nConstSgds =
          ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate).size();
      std::cout << "total weight elms: " << totalWeightElms
                << " threshold: " << opts.mergeVarUpdateMemThreshold
                << " nConstSgds : " << nConstSgds;
      auto weightsMem   = 4 * totalWeightElms;
      bool notDivisible = (weightsMem % opts.mergeVarUpdateMemThreshold) != 0;
      BOOST_CHECK_EQUAL(
          (notDivisible + weightsMem / opts.mergeVarUpdateMemThreshold),
          nConstSgds);
    }
  };

  std::cout << "Test AutoTight" << std::endl;
  test(MergeVarUpdateType::AutoTight);

  std::cout << "Test All" << std::endl;
  test(MergeVarUpdateType::All);

  std::cout << "Test None" << std::endl;
  test(MergeVarUpdateType::None);
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh, IRI: CSIC-UPC
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef EAGLE_MPC_FACTORY_COST_HPP_
#define EAGLE_MPC_FACTORY_COST_HPP_

#include <vector>
#include <string>
#include <map>

#include <Eigen/Dense>

#include "pinocchio/spatial/se3.hpp"

#include "crocoddyl/core/cost-base.hpp"
#include "crocoddyl/core/costs/residual.hpp"

#include "crocoddyl/core/residual-base.hpp"
#include "crocoddyl/core/residuals/control.hpp"

#include "crocoddyl/multibody/residuals/state.hpp"
#include "crocoddyl/multibody/residuals/frame-placement.hpp"
#include "crocoddyl/multibody/residuals/frame-velocity.hpp"
#include "crocoddyl/multibody/residuals/frame-translation.hpp"
#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
#include "crocoddyl/multibody/residuals/contact-friction-cone.hpp"

#include "eagle_mpc/stage.hpp"
#include "eagle_mpc/utils/params_server.hpp"
#include "eagle_mpc/factory/activation.hpp"

namespace eagle_mpc
{
enum class CostModelTypes {
    CostModelState,
    CostModelControl,
    CostModelFramePlacement,
    CostModelFrameTranslation,
    CostModelFrameRotation,
    CostModelFrameVelocity,
    CostModelContactFrictionCone,
    NbCostModelTypes
};

static std::map<std::string, CostModelTypes> CostModelTypes_init_map()
{
    std::map<std::string, CostModelTypes> m;
    m.clear();
    m.insert({"CostModelState", CostModelTypes::CostModelState});
    m.insert({"CostModelControl", CostModelTypes::CostModelControl});
    m.insert({"CostModelFramePlacement", CostModelTypes::CostModelFramePlacement});
    m.insert({"CostModelFrameTranslation", CostModelTypes::CostModelFrameTranslation});
    m.insert({"CostModelFrameRotation", CostModelTypes::CostModelFrameRotation});
    m.insert({"CostModelFrameVelocity", CostModelTypes::CostModelFrameVelocity});
    m.insert({"CostModelContactFrictionCone", CostModelTypes::CostModelContactFrictionCone});
    return m;
}

static const std::map<std::string, CostModelTypes> CostModelTypes_map = CostModelTypes_init_map();

class Stage;
class CostModelFactory
{
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    explicit CostModelFactory();
    ~CostModelFactory();

    boost::shared_ptr<crocoddyl::CostModelResidual> create(const std::string&                     path_to_cost,
                                                           const boost::shared_ptr<ParamsServer>& server,
                                                           const boost::shared_ptr<crocoddyl::StateMultibody>& state,
                                                           const std::size_t&                                  nu,
                                                           CostModelTypes& cost_type) const;

    boost::shared_ptr<ActivationModelFactory> activation_factory_;
};

}  // namespace eagle_mpc

#endif


/* =================================================================================================

(c - GPLv3) T.W.J. de Geus (Tom) | tom@geus.me | www.geus.me | github.com/tdegeus/GooseDEM

================================================================================================= */

#include <Eigen/Eigen>
#include <cppmat/cppmat.h>

#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <cppmat/pybind11.h>

#include "Friction.h"

// =================================================================================================

// abbreviate name-space
namespace py = pybind11;
namespace M  = GooseDEM;
namespace E  = GooseDEM::Ext::Friction;

// abbreviate types(s)
// - Eigen types
typedef GooseDEM::ColD ColD;
typedef GooseDEM::ColS ColS;
typedef GooseDEM::MatD MatD;
typedef GooseDEM::MatS MatS;
// - const arrays
typedef const GooseDEM::ColD cColD;
typedef const GooseDEM::ColS cColS;
typedef const GooseDEM::MatD cMatD;
typedef const GooseDEM::MatS cMatS;

// =================================================================================================

PYBIND11_MODULE(GooseDEM_Ext_Friction, m) {

m.doc() = "Friction simulation";

// =========================== GooseDEM/Ext/Friction/PotentialAdhsion.h ============================

py::class_<E::PotentialAdhesion>(m, "PotentialAdhesion")
  // constructor
  .def(
    py::init<cMatS &, cColD &, cColD &, cColD &, cColD &>(),
    "PotentialAdhesion",
    py::arg("particles"),
    py::arg("k"),
    py::arg("b"),
    py::arg("r0"),
    py::arg("e")
  )
  // methods
  .def("force"       , &E::PotentialAdhesion::force       )
  .def("coordination", &E::PotentialAdhesion::coordination)
  .def("potential"   , &E::PotentialAdhesion::potential   )
  // print to screen
  .def("__repr__",
    [](const E::PotentialAdhesion &a){ return "<GooseDEM_Ext_Friction.PotentialAdhesion>"; }
  );

// =============================== GooseDEM/Ext/Friction/Geometry.h ================================

py::class_<E::Geometry, M::Geometry>(m, "Geometry")
  // constructor
  .def(
    py::init<cColD &, cMatD &, cMatS &>(),
    "Geometry",
    py::arg("m"),
    py::arg("x"),
    py::arg("dofs")
  )
  // methods
  // -
  .def("set", py::overload_cast<const M::Spring            &>(&E::Geometry::set))
  .def("set", py::overload_cast<const M::Dashpot           &>(&E::Geometry::set))
  .def("set", py::overload_cast<const E::PotentialAdhesion &>(&E::Geometry::set))
  // -
  .def("fix_v"    , &E::Geometry::fix_v   )
  .def("set_fext" , &E::Geometry::set_fext)
  .def("fext"     , &E::Geometry::fext    )
  .def("fres"     , &E::Geometry::fres    )
  // -
  .def("x"           , &E::Geometry::x           )
  .def("v"           , &E::Geometry::v           )
  .def("a"           , &E::Geometry::a           )
  .def("f"           , &E::Geometry::f           )
  .def("m"           , &E::Geometry::m           )
  .def("coordination", &E::Geometry::coordination)
  // -
  .def("dofs_v", &E::Geometry::dofs_v)
  .def("dofs_a", &E::Geometry::dofs_a)
  .def("dofs_f", &E::Geometry::dofs_f)
  .def("dofs_m", &E::Geometry::dofs_m)
  // -
  .def("set_v", py::overload_cast<cColD &>(&E::Geometry::set_v))
  .def("set_a", py::overload_cast<cColD &>(&E::Geometry::set_a))
  .def("set_x",                            &E::Geometry::set_x )
  .def("set_v", py::overload_cast<cMatD &>(&E::Geometry::set_v))
  .def("set_a", py::overload_cast<cMatD &>(&E::Geometry::set_a))
  // print to screen
  .def("__repr__",
    [](const E::Geometry &a){ return "<GooseDEM_Ext_Friction.Geometry>"; }
  );

// =================================================================================================

}


#ifndef HAAR_H
#define HAAR_H

#include <opencv2/opencv.hpp>

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

using std::vector;
using namespace cv;

class Haar{
public:
    Haar();
	~Haar();
	vector<vector<Rect> > features;
	vector<vector<float> > featuresWeight;
	Mat sampleFeatureValue;
	int featureNum;
private:
	int featureMinNumRect;
	int featureMaxNumRect;
	Mat imageIntegral;
	Mat detectFeatureValue;
	RNG rng;

private:
	void HaarFeature(Rect& _objectBox, int _numFeature);

public:
	void getFeatureValue(Mat& _frame, vector<Rect>& _sampleBox);
	void init(Mat& _frame, Rect& _objectBox,vector<Rect>& _sampleBox);

};
#endif


#include "../include/dataio.h"
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <iomanip>
#include <random>
#include <chrono>
#include <Eigen/Dense>
#include <tbb/tbb.h>
#include "../include/util.h"

using namespace std;
using namespace Eigen;

bool BALProblem::LoadFile(const char* filename) {
  FILE* fptr = fopen(filename, "r");
  if (fptr == NULL) {
    return false;
  };

  FscanfOrDie(fptr, "%d", &n_keyframes_);
  FscanfOrDie(fptr, "%d", &n_points_);
  FscanfOrDie(fptr, "%d", &n_edges_);

  point_index_ = new unsigned[n_edges_];
  camera_index_ = new unsigned[n_edges_];
  observations_ = new double[2 * n_edges_];

  num_parameters_ = 6 * n_keyframes_ + 3 * n_points_;
  parameters_ = new double[num_parameters_];

  fx_ = new double[1];
  fy_ = new double[1];
  cx_ = new double[1];
  cy_ = new double[1];
  FscanfOrDie(fptr, "%lf", fx_);
  FscanfOrDie(fptr, "%lf", fy_);
  FscanfOrDie(fptr, "%lf", cx_);
  FscanfOrDie(fptr, "%lf", cy_);

  for (int i = 0; i < n_edges_; ++i) {
    FscanfOrDie(fptr, "%d", camera_index_ + i);
    FscanfOrDie(fptr, "%d", point_index_ + i);
    for (int j = 0; j < 2; ++j) {
      FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
    }
  }

  for (int i = 0; i < num_parameters_; ++i) {
    FscanfOrDie(fptr, "%lf", parameters_ + i);
  }

  fclose(fptr);
  return true;
}

template<typename T>
void BALProblem::FscanfOrDie(FILE *fptr, const char *format, T *value) {
  int num_scanned = fscanf(fptr, format, value);
  if (num_scanned != 1) {
    cout << "Invalid UW data file.";
  }
}

void set_prior_lambda(BALProblem& bal_problem, std::vector<float> K,
                      float reproj_meas_var,
                      std::vector<float> cam_priors_mean_,
                      std::vector<float>& cam_priors_eta_,
                      std::vector<float>& cam_priors_lambda_,
                      std::vector<float> lmk_priors_mean_,
                      std::vector<float>& lmk_priors_eta_,
                      std::vector<float>& lmk_priors_lambda_)
{
  for (int c = 0; c < bal_problem.n_keyframes(); ++c) {
    float max_jac = 0.;
    for (int i = 0; i < bal_problem.n_edges(); ++i) {
      Matrix<double,6,1> cam = Map<Matrix<double,6,1>>(bal_problem.camera(c));
      if (bal_problem.camera_index(i) == c) {
        Vector3d lmk = Map<Vector3d>(bal_problem.mutable_point_for_observation(i));

        MatrixXf jac = reprojectionJacFn(cam.cast <float> (), lmk.cast <float> (), K);
        float this_max = jac.cwiseAbs().maxCoeff();
        max_jac = max(max_jac, this_max);
      }
    }
    float lam_scaled = pow(max_jac, 2) / reproj_meas_var;
    Matrix<float,6,1> prior_mean = Map<Matrix<float,6,1>>(&cam_priors_mean_[c*6]);
    Matrix<float,6,1> prior_eta = prior_mean * lam_scaled;
    for (int i = 0; i < 6; ++i) {
      cam_priors_eta_[6*c + i] = prior_eta(i);
      cam_priors_lambda_[36*c + i*6 + i] = lam_scaled;
    }
  }

  for (int c = 0; c < bal_problem.n_points(); ++c) {
    float max_jac = 0.;
    for (int i = 0; i < bal_problem.n_edges(); ++i) {
      Vector3d lmk = Map<Vector3d>(bal_problem.point(c));
      if (bal_problem.point_index(i) == c) {
        Matrix<double,6,1> cam = Map<Matrix<double,6,1>>(bal_problem.mutable_camera_for_observation(i));

        MatrixXf jac = reprojectionJacFn(cam.cast <float> (), lmk.cast <float> (), K);
        float this_max = jac.cwiseAbs().maxCoeff();
        max_jac = max(max_jac, this_max);
      }
    }
    float lam_scaled = pow(max_jac, 2) / reproj_meas_var;
    Vector3f prior_mean = Map<Vector3f>(&lmk_priors_mean_[c*3]);
    Vector3f prior_eta = prior_mean * lam_scaled;
    for (int i = 0; i < 3; ++i) {
      lmk_priors_eta_[3*c + i] = prior_eta(i);
      lmk_priors_lambda_[9*c + i*3 + i] = lam_scaled;
    }
  }
}

// Functions to load data
static unsigned readUnsignedInt(string fname) {

  ifstream input (fname);
  unsigned num;
  input >> num;

  return num;
}

vector<unsigned> readUnsignedIntVector(string fname) {

  string line;
  ifstream myfile (fname);
  vector<unsigned> vec;
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      vec.push_back(stoul(line));
    }
    myfile.close();
  }
  else cout << "Unable to open file " << fname << " !!!! \n";

  return vec;
}

vector<float> readFloatVector(string fname) {

  string line;
  ifstream myfile (fname);
  vector<float> vec;
  if (myfile.is_open())
  {
    while ( getline (myfile,line) )
    {
      vec.push_back(stof(line));
    }
    myfile.close();
  }
  else cout << "Unable to open file " << fname << " !!!! \n";

  return vec;
}

void saveFloatVector(string fname, float* vec, unsigned len) {
  ofstream f;
  f.open(fname);

  for(unsigned j=0; j<len; j++)
  {
      f << fixed << setprecision(7) << vec[j] << endl;
  }

  f.close();
}

void saveBeliefs(string dir, unsigned itr, unsigned n_keyframes,
                unsigned n_points, unsigned cam_dofs, float* cam_beliefs_eta_,
                float* cam_beliefs_lambda_, float* lmk_beliefs_eta_,
                float* lmk_beliefs_lambda_)
{
  string fce = dir + "/beliefs/cb_eta" + std::to_string(itr) + ".txt";
  string fcl = dir + "/beliefs/cb_lambda" + std::to_string(itr) + ".txt";
  string fle = dir + "/beliefs/lb_eta" + std::to_string(itr) + ".txt";
  string fll = dir + "/beliefs/lb_lambda" + std::to_string(itr) + ".txt";
  saveFloatVector(fce, &cam_beliefs_eta_[0], cam_dofs * n_keyframes);
  saveFloatVector(fcl, &cam_beliefs_lambda_[0], cam_dofs * cam_dofs * n_keyframes);
  saveFloatVector(fle, &lmk_beliefs_eta_[0], 3 * n_points);
  saveFloatVector(fll, &lmk_beliefs_lambda_[0], 9 * n_points);
}

void save_cam_means(ofstream &cmeans_file, int itr, float* cam_beliefs_eta_,
                    float* cam_beliefs_lambda_, unsigned n_keyframes,
                    unsigned* cam_active_flag_) {
  float cam_means_ [n_keyframes * 6] = {};

  // Use as many threads as there are cores on the CPU:
  tbb::task_scheduler_init init(tbb::task_scheduler_init::automatic);

  tbb::parallel_for(0U, n_keyframes, [&](unsigned cID) {
    if (cam_active_flag_[cID] == 5) {

      Matrix<float,6,1> cam_eta = Map<Matrix<float,6,1>>(&cam_beliefs_eta_[cID * 6]);
      Matrix<float,6,6> cam_lam = Map<Matrix<float,6,6>>(&cam_beliefs_lambda_[cID * 36]);
      VectorXf cam_mu = cam_lam.transpose().inverse() * cam_eta;

      for (unsigned i = 0; i < 6; ++i) {
        cam_means_[cID * 6 + i] = cam_mu(i);
      }
    }
  });

  cmeans_file << "Iteration " << itr << "\n";
  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    if (cam_active_flag_[cID] == 5) {
      for (unsigned i = 0; i < 6; ++i) {
        cmeans_file << fixed << setprecision(7) << cam_means_[cID * 6 + i] << "\n";
      }
    }
  }

}

void save_lmk_means(ofstream &lmeans_file, int itr, float* lmk_beliefs_eta_,
                    float* lmk_beliefs_lambda_, unsigned n_points,
                    unsigned* lmk_active_flag_) {
  float lmk_means_ [n_points * 3] = {};

  // Use as many threads as there are cores on the CPU:
  tbb::task_scheduler_init init(tbb::task_scheduler_init::automatic);

  tbb::parallel_for(0U, n_points, [&](unsigned lID) {
    if (lmk_active_flag_[lID] == 5) {

      Vector3f lmk_eta = Map<Vector3f>(&lmk_beliefs_eta_[lID * 3]);
      Matrix3f lmk_lam = Map<Matrix3f>(&lmk_beliefs_lambda_[lID * 9]);
      Vector3f lmk_mu = lmk_lam.transpose().inverse() * lmk_eta;

      for (unsigned i = 0; i < 3; ++i) {
        lmk_means_[lID * 3 + i] = lmk_mu(i);
      }
    }
  });

  lmeans_file << "Iteration " << itr << "\n";
  for (unsigned lID = 0; lID < n_points; ++lID) {
    if (lmk_active_flag_[lID] == 5) {
      for (unsigned i = 0; i < 3; ++i) {
        lmeans_file << fixed << setprecision(7) << lmk_means_[lID * 3 + i] << "\n";
      }
    }
  }

}

void save_message_KL(ofstream &kl_file, int itr, unsigned n_edges,
                    std::vector<unsigned int> active_flag,
                    unsigned* measurements_camIDs, unsigned* measurements_lIDs,
                    int data_counter, unsigned max_nkfedges, unsigned max_nlmkedges,
                    unsigned cam_dofs, float* cam_messages_lambda_,
                    float* lmk_messages_lambda_)
{
  // float* cam_messages_eta_, float* cam_messages_lambda_, float* lmk_messages_eta_, float* lmk_messages_lambda_,
  // float* pcam_messages_eta_, float* pcam_messages_lambda_, float* plmk_messages_eta_, float* plmk_messages_lambda_) {

  // Use as many threads as there are cores on the CPU:
  tbb::task_scheduler_init init(tbb::task_scheduler_init::automatic);

  unsigned n_active_edges = 0;
  for (unsigned e = 0; e < n_edges; ++e) {
    n_active_edges += active_flag[data_counter * n_edges +e];
  }

  float cam_norm [n_active_edges] = {};
  float lmk_norm [n_active_edges] = {};

  tbb::parallel_for(0U, n_active_edges, [&](unsigned e) {
  // for (unsigned e = 0; e < n_active_edges; ++e) {

    unsigned adj_cID = measurements_camIDs[e];
    unsigned adj_lID = measurements_lIDs[e];

    unsigned edges_c = 0;
    unsigned edges_l = 0;
    for (unsigned i = 0; i < e; ++i) {
      if (measurements_camIDs[i] == adj_cID) {
        edges_c += 1;
      }
    }
    unsigned count_ledges_l = 0;
    for (unsigned i = 0; i < e; ++i) {
      if (measurements_lIDs[i] == adj_lID) {
        edges_l += 1;
      }
    }

    // Matrix<float,6,1> cam_mess_eta = Map<Matrix<float,6,1>>(&cam_messages_eta_[(max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 1)]);
    Matrix<float,6,6> cam_mess_lambda = Map<Matrix<float,6,6>>(&cam_messages_lambda_[(max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 1)]);
    // Matrix<float,6,1> pcam_mess_eta = Map<Matrix<float,6,1>>(&pcam_messages_eta_[(max_nkfedges + 1) * cam_dofs * adj_cID + cam_dofs * (edges_c + 1)]);
    // Matrix<float,6,6> pcam_mess_lambda = Map<Matrix<float,6,6>>(&pcam_messages_lambda_[(max_nkfedges + 1) * cam_dofs * cam_dofs * adj_cID + cam_dofs * cam_dofs * (edges_c + 1)]);

    // Matrix<float,3,1> lmk_mess_eta = Map<Matrix<float,3,1>>(&lmk_messages_eta_[(max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 1)]);
    Matrix<float,3,3> lmk_mess_lambda = Map<Matrix<float,3,3>>(&lmk_messages_lambda_[(max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 1)]);
    // Matrix<float,3,1> plmk_mess_eta = Map<Matrix<float,3,1>>(&plmk_messages_eta_[(max_nlmkedges + 1) * 3 * adj_lID + 3 * (edges_l + 1)]);
    // Matrix<float,3,3> plmk_mess_lambda = Map<Matrix<float,3,3>>(&plmk_messages_lambda_[(max_nlmkedges + 1) * 9 * adj_lID + 9 * (edges_l + 1)]);

    // camKL[e] = symmetricKL(cam_mess_eta, pcam_mess_eta, cam_mess_lambda, pcam_mess_lambda);
    // lmkKL[e] = symmetricKL(lmk_mess_eta, plmk_mess_eta, lmk_mess_lambda, plmk_mess_lambda);
    cam_norm[e] = cam_mess_lambda.squaredNorm();
    lmk_norm[e] = lmk_mess_lambda.squaredNorm();

  });

  kl_file << "Iteration " << itr << "\n";
  float cam_KL_tot = 0.0;
  float lmk_KL_tot = 0.0;
  for (unsigned e = 0; e < n_active_edges; ++e) {
    kl_file << cam_norm[e] << " " << lmk_norm[e] << "\n";
    cam_KL_tot += cam_norm[e];
    lmk_KL_tot += lmk_norm[e];
  }

  cout << "Average cam norm " << cam_KL_tot / n_active_edges << "  Average lmk norm " << lmk_KL_tot / n_active_edges << "\n";
}

void add_cam_trans_noise(std::vector<float>& cam_priors_mean_, unsigned n_keyframes,
                        float cam_trans_noise_std, unsigned k)
{
  std::cout << "\nAdding Gaussian noise with std: " << cam_trans_noise_std << "m to the keyframe translaton intialisations\n";
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<float> trans_noise(0.f, cam_trans_noise_std);

  for (unsigned cID = k; cID < n_keyframes; ++cID) {
    for (unsigned i = 0; i < 3; ++i) {
      cam_priors_mean_[cID * 6 + i] += trans_noise(generator);
    }
  }
}

void add_cam_rot_noise(std::vector<float>& cam_priors_mean_, unsigned n_keyframes,
                        float cam_rot_noise_std, unsigned k)
{
  std::cout << "Adding Gaussian noise with std: " << cam_rot_noise_std << " to the keyframe rotation intialisations\n";
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<float> rot_noise(0.f, cam_rot_noise_std);

  for (unsigned cID = k; cID < n_keyframes; ++cID) {

    float rot_angle_radians = rot_noise(generator) * M_PI / 180;
    int axis = (std::rand()%3);

    Matrix3f R;
    if (axis == 0) {
      // Matrix3f R;
      R << 1.f, 0.f, 0.f,
           0.f, cos(rot_angle_radians), -sin(rot_angle_radians),
           0.f, sin(rot_angle_radians), cos(rot_angle_radians);
    }
    else if (axis == 1) {
      // Matrix3f R;
      R << cos(rot_angle_radians), 0.f, sin(rot_angle_radians),
           0.f, 1.f, 0.f,
           -sin(rot_angle_radians), 0.f, cos(rot_angle_radians);
    }
    else if (axis == 2) {
      // Matrix3f R;
      R << cos(rot_angle_radians), -sin(rot_angle_radians), 0.f,
           sin(rot_angle_radians), cos(rot_angle_radians), 0.f,
           0.f, 0.f, 1.f;
    }

    Vector3f rot_component;
    rot_component << cam_priors_mean_[cID*6 + 3], cam_priors_mean_[cID*6 + 4], cam_priors_mean_[cID*6 + 5];
    Matrix3f Rw2c = eigenso3exp(rot_component);
    Matrix4f Tw2c;
    Tw2c << Rw2c(0,0), Rw2c(0,1), Rw2c(0,2), cam_priors_mean_[cID*6],
            Rw2c(1,0), Rw2c(1,1), Rw2c(1,2), cam_priors_mean_[cID*6 + 1],
            Rw2c(2,0), Rw2c(2,1), Rw2c(2,2), cam_priors_mean_[cID*6 + 2],
            0.0, 0.0, 0.0, 1.0;
    Matrix4f Tc2w = Tw2c.inverse();
    Tc2w.block<3,3>(0,0) = R * Tc2w.block<3,3>(0,0);
    Tw2c = Tc2w.inverse();

    cam_priors_mean_[cID*6] = Tw2c(0,3);
    cam_priors_mean_[cID*6 + 1] = Tw2c(1,3);
    cam_priors_mean_[cID*6 + 2] = Tw2c(2,3);

    Rw2c = Tw2c.block<3,3>(0,0);
    Vector3f w = so3log(Rw2c);
    cam_priors_mean_[cID*6 + 3] = w(0);
    cam_priors_mean_[cID*6 + 4] = w(1);
    cam_priors_mean_[cID*6 + 5] = w(2);
  }
}

void add_lmk_noise(std::vector<float>& lmk_priors_mean_, unsigned n_points,
                          float lmk_trans_noise_std)
{
  std::cout << "Adding Gaussian noise with std: " << lmk_trans_noise_std << "m to the landmark intialisations\n";
  unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
  std::default_random_engine generator (seed);
  std::normal_distribution<float> noise(0.f, lmk_trans_noise_std);

  for (unsigned lID = 0; lID < n_points; ++lID) {
    for (unsigned i = 0; i < 3; ++i) {
      lmk_priors_mean_[lID * 3 + i] += noise(generator);
    }
  }
}

void av_depth_init(float av_depth, unsigned n_keyframes, unsigned n_edges, unsigned n_points,
                  std::vector<unsigned> measurements_camIDs,
                  std::vector<unsigned> measurements_lIDs,
                  std::vector<float> cam_priors_mean_,
                  std::vector<float>& lmk_priors_mean_)
{
  std::cout << "Initialising all landmarks at an average depth of: " << av_depth << "\n";
  std::vector<unsigned> lmk_done(n_points, 0);

  for (unsigned cID = 0; cID < n_keyframes; ++cID) {
    Vector3f rot_component;
    rot_component << cam_priors_mean_[cID*6 + 3], cam_priors_mean_[cID*6 + 4], cam_priors_mean_[cID*6 + 5];
    Matrix3f Rw2c = eigenso3exp(rot_component);
    Matrix4f Tw2c;
    Tw2c << Rw2c(0,0), Rw2c(0,1), Rw2c(0,2), cam_priors_mean_[cID*6],
            Rw2c(1,0), Rw2c(1,1), Rw2c(1,2), cam_priors_mean_[cID*6 + 1],
            Rw2c(2,0), Rw2c(2,1), Rw2c(2,2), cam_priors_mean_[cID*6 + 2],
            0.0, 0.0, 0.0, 1.0;

    Vector4f loc_cam_frame;
    loc_cam_frame << 0.0, 0.0, 1.0, 1.0;
    Vector4f new_lmk_mu_wf_homog = Tw2c.inverse() * loc_cam_frame;
    Vector3f new_lmk_mu_wf;
    new_lmk_mu_wf << new_lmk_mu_wf_homog(0), new_lmk_mu_wf_homog(1), new_lmk_mu_wf_homog(2);

    for (unsigned e = 0; e < n_edges; ++e) {
      if (measurements_camIDs[e] == cID) {
        if (lmk_done[measurements_lIDs[e]] == 0) {
          lmk_priors_mean_[measurements_lIDs[e] * 3] = new_lmk_mu_wf(0);
          lmk_priors_mean_[measurements_lIDs[e] * 3 + 1] = new_lmk_mu_wf(1);
          lmk_priors_mean_[measurements_lIDs[e] * 3 + 2] = new_lmk_mu_wf(2);
          lmk_done[measurements_lIDs[e]] = 1;
        }
      }
    }
  }
}

void create_flags(BALProblem& bal_problem, std::vector<unsigned int>& active_flag_,
                  std::vector<unsigned int>& cam_weaken_flag_,
                  std::vector<unsigned int>& lmk_weaken_flag_,
                  std::vector<unsigned int>& lmk_active_flag, int steps)
{
  // Begin with first 2 keyframes
  cam_weaken_flag_[0] = steps;
  cam_weaken_flag_[1] = steps;

  for (int i = 0; i < bal_problem.n_edges(); ++i) {
    if ((bal_problem.camera_index(i) == 0) || (bal_problem.camera_index(i) == 1)) {
      active_flag_[i] = 1;
      lmk_weaken_flag_[bal_problem.point_index(i)] = steps;
    }
  }

  for (int i=0; i<lmk_weaken_flag_.size(); i++) {
    lmk_active_flag[i] = lmk_weaken_flag_[i];
  }

}

int update_flags(BALProblem& bal_problem, std::vector<unsigned int>& active_flag_,
             std::vector<unsigned int>& lmk_weaken_flag_,
             std::vector<unsigned int>& cam_weaken_flag_,
             std::vector<unsigned int>& lmk_active_flag,
             unsigned steps, unsigned data_counter)
{
  for (int i = 0; i < bal_problem.n_edges(); ++i) {
    if (bal_problem.camera_index(i) == data_counter + 1) {
      active_flag_[i] = 1;
    }
    if (bal_problem.camera_index(i) <= data_counter + 1) {
      lmk_weaken_flag_[bal_problem.point_index(i)] = steps;
    }
  }

  for (int c=0; c < bal_problem.n_keyframes(); ++c) {
    cam_weaken_flag_[c] = 0;
  }
  cam_weaken_flag_[data_counter+1] = steps;

  for (int c = 0; c < bal_problem.n_points(); ++c) {
    lmk_weaken_flag_[c] -= lmk_active_flag[c];
    lmk_active_flag[c] += lmk_weaken_flag_[c];
  }

  int n_new_lmks = 0;
  for (unsigned i = 0; i < bal_problem.n_points(); ++i) {
    n_new_lmks += lmk_weaken_flag_[i];
  }
  n_new_lmks /= steps;
  return n_new_lmks;
}


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/VectorIntRNGTestRig.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>

class PoissonTestRig : public VectorIntRNGTestRig {
 public:
  PoissonTestRig()
      : VectorIntRNGTestRig(10000, 10, {0, 1, 2, 3, 4, 5, 6}, {0.1, 1.1, 4.99},
                            {1, 2, 3}, {-3.0, -2.0, 0.0}, {-3, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& lambda, const T2&, const T3&,
                        T_rng& rng) const {
    return stan::math::poisson_rng(lambda, rng);
  }

  template <typename T1>
  double pmf(int y, T1 lambda, double, double) const {
    return std::exp(stan::math::poisson_lpmf(y, lambda));
  }
};

TEST(ProbDistributionsPoisson, errorCheck) {
  check_dist_throws_all_types(PoissonTestRig());
}

TEST(ProbDistributionsPoisson, distributionCheck) {
  check_counts_real(PoissonTestRig());
}

TEST(ProbDistributionsPoisson, error_check) {
  using std::log;

  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::poisson_rng(6, rng));

  EXPECT_THROW(stan::math::poisson_rng(-6, rng), std::domain_error);

  EXPECT_NO_THROW(stan::math::poisson_rng(1e9, rng));

  EXPECT_THROW(stan::math::poisson_rng(pow(2.0, 31), rng), std::domain_error);

  EXPECT_NO_THROW(stan::math::poisson_log_rng(6, rng));

  EXPECT_NO_THROW(stan::math::poisson_log_rng(-6, rng));

  EXPECT_NO_THROW(stan::math::poisson_log_rng(log(1e9), rng));

  EXPECT_THROW(stan::math::poisson_log_rng(log(pow(2.0, 31)), rng),
               std::domain_error);
}

TEST(ProbDistributionsPoisson, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::poisson_distribution<> dist(5);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::poisson_rng(5, rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbDistributionsPoisson, chiSquareGoodnessFitTest2) {
  using std::log;

  boost::random::mt19937 rng;
  int N = 1000;
  int K = stan::math::round(2 * std::pow(N, 0.4));
  boost::math::poisson_distribution<> dist(5);
  boost::math::chi_squared mydist(K - 1);

  int loc[K - 1];
  for (int i = 1; i < K; i++)
    loc[i - 1] = i - 1;

  int count = 0;
  double bin[K];
  double expect[K];
  for (int i = 0; i < K; i++) {
    bin[i] = 0;
    expect[i] = N * pdf(dist, i);
  }
  expect[K - 1] = N * (1 - cdf(dist, K - 1));

  while (count < N) {
    int a = stan::math::poisson_log_rng(log(5), rng);
    int i = 0;
    while (i < K - 1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
  }

  double chi = 0;

  for (int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}

TEST(ProbPoisson, log_matches_lpmf) {
  int y = 3;
  double lambda = 2.3;

  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf(y, lambda)),
                  (stan::math::poisson_log(y, lambda)));
  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf<true>(y, lambda)),
                  (stan::math::poisson_log<true>(y, lambda)));
  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf<false>(y, lambda)),
                  (stan::math::poisson_log<false>(y, lambda)));
  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf<true, int, double>(y, lambda)),
                  (stan::math::poisson_log<true, int, double>(y, lambda)));
  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf<false, int, double>(y, lambda)),
                  (stan::math::poisson_log<false, int, double>(y, lambda)));
  EXPECT_FLOAT_EQ((stan::math::poisson_lpmf<int, double>(y, lambda)),
                  (stan::math::poisson_log<int, double>(y, lambda)));
}


//  Copyright John Maddock 2007.
//  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)

#ifndef BOOST_MATH_ROUND_HPP
#define BOOST_MATH_ROUND_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/tools/config.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

namespace boost{ namespace math{

template <class T, class Policy>
inline T round(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   if(!(boost::math::isfinite)(v))
      return policies::raise_rounding_error("boost::math::round<%1%>(%1%)", 0, v, v, pol);
   return v < 0 ? static_cast<T>(ceil(v - 0.5f)) : static_cast<T>(floor(v + 0.5f));
}
template <class T>
inline T round(const T& v)
{
   return round(v, policies::policy<>());
}
//
// The following functions will not compile unless T has an
// implicit convertion to the integer types.  For user-defined
// number types this will likely not be the case.  In that case
// these functions should either be specialized for the UDT in
// question, or else overloads should be placed in the same
// namespace as the UDT: these will then be found via argument
// dependent lookup.  See our concept archetypes for examples.
//
template <class T, class Policy>
inline int iround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<int>::max)())
      return static_cast<int>(policies::raise_rounding_error("boost::math::iround<%1%>(%1%)", 0, v, 0, pol));
   return static_cast<int>(r);
}
template <class T>
inline int iround(const T& v)
{
   return iround(v, policies::policy<>());
}

template <class T, class Policy>
inline long lround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<long>::max)())
      return static_cast<long int>(policies::raise_rounding_error("boost::math::lround<%1%>(%1%)", 0, v, 0L, pol));
   return static_cast<long int>(r);
}
template <class T>
inline long lround(const T& v)
{
   return lround(v, policies::policy<>());
}

#ifdef BOOST_HAS_LONG_LONG

template <class T, class Policy>
inline boost::long_long_type llround(const T& v, const Policy& pol)
{
   BOOST_MATH_STD_USING
   T r = boost::math::round(v, pol);
   if(fabs(r) > (std::numeric_limits<boost::long_long_type>::max)())
      return static_cast<boost::long_long_type>(policies::raise_rounding_error("boost::math::llround<%1%>(%1%)", 0, v, 0LL, pol));
   return static_cast<boost::long_long_type>(r);
}
template <class T>
inline boost::long_long_type llround(const T& v)
{
   return llround(v, policies::policy<>());
}

#endif

}} // namespaces

#endif // BOOST_MATH_ROUND_HPP