// HEADER
#include <path_follower/controller/robotcontroller_PBR.h>

// THIRD PARTY
#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

// PROJECT
#include <path_follower/parameters/path_follower_parameters.h>
#include <path_follower/utils/extended_kalman_filter.h>
#include <cslibs_navigation_utilities/MathHelper.h>

#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>

// ALGLIB

// SYSTEM
#include <cmath>
#include <deque>
#include <boost/algorithm/clamp.hpp>

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_PBR, PBR, default_collision_avoider);

using namespace Eigen;

RobotController_PBR::RobotController_PBR():
    RobotController(),
    cmd_(this),
    vn_(0),
    Ts_(0.02),
    ind_(0),
    Vl_(0),
    Vr_(0),
    curv_sum_(1e-3),
    distance_to_goal_(1e6),
    distance_to_obstacle_(1e3)
{
    pose_ekf_ = Eigen::Vector3d::Zero();
    ICR_ekf_  = Eigen::Vector3d::Zero();
    last_time_ = ros::Time::now();

    wheel_vel_sub_ = nh_.subscribe<std_msgs::Float64MultiArray>("wheel_velocities", 10,
                                                                   &RobotController_PBR::WheelVelocities, this);

    ICR_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("ICR_parameters", 10);

    marker_pub_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10);

    ekf_path_marker_.header.frame_id = getFixedFrame();
    ekf_path_marker_.header.stamp = ros::Time();
    ekf_path_marker_.ns = "ekf_path_namespace";
    ekf_path_marker_.id = 51;
    ekf_path_marker_.type = visualization_msgs::Marker::LINE_STRIP;
    ekf_path_marker_.action = visualization_msgs::Marker::ADD;
    ekf_path_marker_.pose.position.x = 0;
    ekf_path_marker_.pose.position.y = 0;
    ekf_path_marker_.pose.position.z = 0;
    ekf_path_marker_.pose.orientation.x = 0.0;
    ekf_path_marker_.pose.orientation.y = 0.0;
    ekf_path_marker_.pose.orientation.z = 0.0;
    ekf_path_marker_.pose.orientation.w = 1.0;
    ekf_path_marker_.scale.x = 0.1;
    ekf_path_marker_.scale.y = 0.0;
    ekf_path_marker_.scale.z = 0.0;
    ekf_path_marker_.color.a = 1.0;
    ekf_path_marker_.color.r = 0.5;
    ekf_path_marker_.color.g = 0.0;
    ekf_path_marker_.color.b = 1.0;

    path_aug_marker_.header.frame_id = getFixedFrame();
    path_aug_marker_.header.stamp = ros::Time();
    path_aug_marker_.ns = "path_aug_namespace";
    path_aug_marker_.id = 52;
    path_aug_marker_.type = visualization_msgs::Marker::LINE_STRIP;
    path_aug_marker_.action = visualization_msgs::Marker::ADD;
    path_aug_marker_.pose.position.x = 0;
    path_aug_marker_.pose.position.y = 0;
    path_aug_marker_.pose.position.z = 0;
    path_aug_marker_.pose.orientation.x = 0.0;
    path_aug_marker_.pose.orientation.y = 0.0;
    path_aug_marker_.pose.orientation.z = 0.0;
    path_aug_marker_.pose.orientation.w = 1.0;
    path_aug_marker_.scale.x = 0.1;
    path_aug_marker_.scale.y = 0.0;
    path_aug_marker_.scale.z = 0.0;
    path_aug_marker_.color.a = 1.0;
    path_aug_marker_.color.r = 0.0;
    path_aug_marker_.color.g = 0.5;
    path_aug_marker_.color.b = 1.0;

}

void RobotController_PBR::stopMotion()
{

    cmd_.speed = 0;
    cmd_.direction_angle = 0;
    cmd_.rotation = 0;

    MoveCommand mcmd = cmd_;
    publishMoveCommand(mcmd);
}

void RobotController_PBR::initialize()
{
    RobotController::initialize();

    // desired velocity
    vn_ = std::min(PathFollowerParameters::getInstance()->max_velocity(), velocity_);
    ROS_DEBUG_STREAM("velocity_: " << velocity_ << ", vn: " << vn_);

    //reset the ekf path points
    ekf_path_marker_.points.clear();

    //reset the path_aug points
    path_aug_marker_.points.clear();

    //reset the augmented path
    x_aug_.clear();
    y_aug_.clear();

    //reset the index of the orthogonal projection
    ind_ = 0;
}

void RobotController_PBR::WheelVelocities(const std_msgs::Float64MultiArray::ConstPtr& array)
{

    double frw = array->data[0];
    double flw = array->data[1];
    double brw = array->data[2];
    double blw = array->data[3];

    Vl_ = (flw + blw)/2.0;
    Vr_ = (frw + brw)/2.0;

    ros::Time current_time = ros::Time::now();
    double dt = (current_time - last_time_).toSec();
    last_time_ = current_time;
    if(Vl_ > 1e-3 || Vr_ > 1e-3){
        ekf_.predict(array, dt);
        pose_ekf_ << ekf_.x_(0), ekf_.x_(1), ekf_.x_(2);
        ICR_ekf_  << ekf_.x_(3), ekf_.x_(4), ekf_.x_(5);
    }

}

void RobotController_PBR::start()
{

}

void RobotController_PBR::reset()
{
    RobotController::reset();
}

void RobotController_PBR::setPath(Path::Ptr path)
{
    RobotController::setPath(path);
}

void RobotController_PBR::setCurrentPose(const Eigen::Vector3d& pose) {

    ekf_.correct(pose);
    pose_ekf_ << ekf_.x_(0), ekf_.x_(1), ekf_.x_(2);
    ICR_ekf_  << ekf_.x_(3), ekf_.x_(4), ekf_.x_(5);
}

RobotController::MoveCommandStatus RobotController_PBR::computeMoveCommand(MoveCommand *cmd)
{
    // omni drive can rotate.
    *cmd = MoveCommand(true);

    if(path_interpol.n() < 2) {
        ROS_ERROR("[Line] path is too short (N = %d)", (int) path_interpol.n());

        stopMotion();
        return MoveCommandStatus::REACHED_GOAL;
    }

    /// get the pose as pose(0) = x, pose(1) = y, pose(2) = theta
    Eigen::Vector3d current_pose = pose_tracker_->getRobotPose();

    double x_meas = current_pose[0];
    double y_meas = current_pose[1];
    double theta_meas = current_pose[2];
    ///***///

    ///Map the path using current ICR values
    //ICR mapping vector
    double r_x = ICR_ekf_(2);
    double r_y = (ICR_ekf_(1) - ICR_ekf_(0))/2.0;

    //center of the "differential drive"
    x_meas = x_meas + std::cos(theta_meas)*r_x - std::sin(theta_meas)*r_y;
    y_meas = y_meas + std::sin(theta_meas)*r_x + std::cos(theta_meas)*r_y;

    RobotController::findOrthogonalProjection();

    if(RobotController::isGoalReached(cmd)){
       return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    //augmenting the path by the vector r = (x_ICR, (y_ICRl-y_ICRr)/2)
    for(std::size_t i = ind_; i < path_interpol.n(); ++i) {

        //vector of the ICR mapping in world coordinates
        double delta_p =path_interpol.theta_p(i);

        double r_x_m = std::cos(delta_p)*r_x - std::sin(delta_p)*r_y;
        double r_y_m = std::sin(delta_p)*r_x + std::cos(delta_p)*r_y;

        if(i == ind_){
            geometry_msgs::Point pt_aug;
            pt_aug.x = path_interpol.p(i) + r_x_m;
            pt_aug.y = path_interpol.q(i) + r_y_m;
            path_aug_marker_.points.push_back(pt_aug);
        }

        x_aug_.push_back(path_interpol.p(i) + r_x_m);
        y_aug_.push_back(path_interpol.q(i) + r_y_m);
    }

    marker_pub_.publish(path_aug_marker_);

    ///***///

    //find the orthogonal projection to the curve and extract the corresponding index

    double dist = 0;
    double orth_proj = std::numeric_limits<double>::max();

    //this is a hack made for the lemniscate
    int old_ind = ind_;

    for (int i = ind_, n = path_interpol.n(); i < n; i++){

        dist = hypot(x_meas - x_aug_[i], y_meas - y_aug_[i]);
        if((dist < orth_proj) & (i - old_ind >= 0) & (i - old_ind <= 3)){

            orth_proj = dist;
            ind_ = i;

        }

    }

    // distance to the path (path to the right -> positive)
    Eigen::Vector2d path_vehicle(x_meas - x_aug_[ind_], y_meas - y_aug_[ind_]);

    orth_proj = MathHelper::AngleDelta(path_interpol.theta_p(ind_), MathHelper::Angle(path_vehicle)) > 0. ?
                orth_proj : -orth_proj;

    //***//

    //robot direction angle in path coordinates
    double theta_e = MathHelper::AngleDelta(path_interpol.theta_p(ind_), theta_meas);

    ///Check the driving direction, and set the complementary angle in path coordinates, if driving backwards

    if (getDirSign() < 0.0) {
        theta_e = MathHelper::NormalizeAngle(M_PI + theta_e);
        ROS_WARN_THROTTLE(1, "Driving backwards...");
    }

    ///***///

    ///Calculate the parameters for the exponential speed control

    //calculate the curvature, and stop when the look-ahead distance is reached (w.r.t. orthogonal projection)
    double s_cum_sum = 0;
    curv_sum_ = 1e-10;

    for (unsigned int i = ind_ + 1; i < path_interpol.n(); i++){

        s_cum_sum = path_interpol.s(i) - path_interpol.s(ind_);
        //TODO: need two types of curv_sum_, one for the exponential, the other one for the Lyapunov speed control
        //curv_sum_ += fabs(path_interpol.curvature(i));
        curv_sum_ += path_interpol.curvature(i);

        if(s_cum_sum - opt_.look_ahead_dist() >= 0){
            break;
        }
    }

    //calculate the distance from the orthogonal projection to the goal, w.r.t. path
    distance_to_goal_ = path_interpol.s(path_interpol.n()-1) - path_interpol.s(ind_);
    ///***///

    ///Exponential speed control

    //get the robot's current angular velocity
    double angular_vel = pose_tracker_->getVelocity().angular.z;

    //ensure valid values
    if (distance_to_obstacle_ == 0 || !std::isfinite(distance_to_obstacle_)) distance_to_obstacle_ = 1e-10;
    if (distance_to_goal_ == 0 || !std::isfinite(distance_to_goal_)) distance_to_goal_ = 1e-10;

    double exponent = opt_.k_curv()*fabs(curv_sum_)
            + opt_.k_w()*fabs(angular_vel)
            + opt_.k_o()/distance_to_obstacle_
            + opt_.k_g()/distance_to_goal_;

    //TODO: consider the minimum excitation speed
    double v = vn_ * exp(-exponent);

    cmd_.speed = getDirSign()*std::max((double)PathFollowerParameters::getInstance()->min_velocity(), fabs(v));

    ///***///

    ///Direction control

    cmd_.direction_angle = 0;
    double omega = -opt_.k1()*v*orth_proj*std::sin(theta_e)/theta_e - opt_.k2()*std::abs(v)*theta_e;
    omega = boost::algorithm::clamp(omega, -opt_.max_angular_velocity(), opt_.max_angular_velocity());
    cmd_.rotation = omega;
    ///***///

    if(ind_ != path_interpol.n()){
        path_->fireNextWaypointCallback();
    }

    if (visualizer_->hasSubscriber()) {
        visualizer_->drawSteeringArrow(getFixedFrame(), 1, pose_tracker_->getRobotPoseMsg(), cmd_.direction_angle, 0.2, 1.0, 0.2);
    }

    ///***///

    ///Visualize the path estimated by the EKF
    geometry_msgs::Point pt;
    pt.x = pose_ekf_(0);
    pt.y = pose_ekf_(1);
    ekf_path_marker_.points.push_back(pt);

    marker_pub_.publish(ekf_path_marker_);
    ///***///

    //Publish the ICR parameters in a form of an array
    std_msgs::Float64MultiArray ICR_ekf_array;
    ICR_ekf_array.data.resize(3);
    ICR_ekf_array.data[0] = ICR_ekf_(0);
    ICR_ekf_array.data[1] = ICR_ekf_(1);
    ICR_ekf_array.data[2] = ICR_ekf_(2);
    ICR_pub_.publish(ICR_ekf_array);
    //***//

        *cmd = cmd_;

        return MoveCommandStatus::OKAY;
}

void RobotController_PBR::publishMoveCommand(const MoveCommand &cmd) const
{
    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getRotationalVelocity();

    cmd_pub_.publish(msg);
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS
  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/realpow.hpp>
#include <simd_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" realpow",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::realpow;

  using r_t = decltype(realpow(T(), T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(realpow(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<r_t>(), 0);
  STF_ULP_EQUAL(realpow(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<r_t>(), 0);
#endif
  STF_ULP_EQUAL(realpow(bs::Mone<T>(), bs::Mone<T>()), bs::Mone<r_t>(), 0);
  STF_ULP_EQUAL(realpow(bs::One<T>(), bs::One<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(realpow(bs::Zero<T>(), bs::Zero<T>()), bs::One<r_t>(), 0);
  STF_ULP_EQUAL(realpow(T(-1),T(5)), T(-1), 0);
  STF_ULP_EQUAL(realpow(T(-1),T(6)), T(1), 0);
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef _STEADYSTATEEXTRAPOLATIONRUNNER_HPP_
#define _STEADYSTATEEXTRAPOLATIONRUNNER_HPP_

#ifdef CHASTE_CVODE

#include "AbstractSteadyStateRunner.hpp"
#include <boost/circular_buffer.hpp>
//#include "VectorHelperFunctions.hpp"

class SteadyStateExtrapolationRunner : public AbstractSteadyStateRunner
{

  /**
 * This class is to get a cell model to steady state by using extrapolation.
 *
 * We use the Mixed Root Mean Square (MRMS) error to measure how far from the steady state
 * we are. The PMCC of the MRMS errors is then used to check whether we can approxiamte the
 * decay of the state variables with an exponential. We then use simple linear regression
 * to find this exponential and extrapolate the variables.
 */

private:
  /* The number of points to use for our PMCC calculation and simple linear regression*/
  const unsigned int mBufferSize = 50;
  /*The maximum number of times we will attempt to extrapolate*/
  const unsigned int mMaxJumps = 100;
  /*The MRMS value for which we are close enough to the steady state that the absolute error in APD90 is probably less that 0.01ms*/
  const double mThreshold = 1.8e-07;
  /** Relative and absolute tolerances */
  const double TolRel = 1e-8;
  const double TolAbs = 1e-8;
  /** How far to extrapolate towards our approximation of the limit of the state variable. */
  double  mExtrapolationCoefficient = 0.9;
  /** The period and duration of the regular stimulus function */
  double  mPeriod;
  double  mDuration;
  /**The terminal state variables of the last pace we calculated*/
  std::vector<double> mStateVariables;
  /*The final state variables before the last extrapolation. This is used to reset in case an extrapolation causes an error*/
  std::vector<double> mSafeStateVariables;

  /** The stimulus used to pace the model - this must be a RegularStimulus */
  boost::shared_ptr<RegularStimulus> mpStimulus;

  /**Contains mBufferSize previous values of each state variable*/
  boost::circular_buffer<std::vector<double>>  mStatesBuffer;
  /**Contains mBufferSize previous values of each state vMRMS value*/
  boost::circular_buffer<double> mMrmsBuffer;

  /**Keeps track of the number of times we have extrapolated*/
  unsigned int mJumps = 0;

  /**Run the cell to steady state */
  virtual void RunToSteadyStateImplementation();

  /**
     * Calculate the MRMS error between two vectors
     *
     * @param A  The second vector
     * @param B  The first  vector
     */
  double CalculateMrms(std::vector<double> A, std::vector<double> B);

  /**
     * Calculate the PMCC of a circular_buffer of variables. This is used to calculate the PMCC of the previous MRMS values.
     *
     * @param values The values that we are calculating the PMCC of.
     */

  double CalculatePMCC(boost::circular_buffer<double> values){
    if(values.size()<=2 || values.size() <= 2){
      return -NAN;
    }

    const unsigned int N = values.size();
    const double sum_x = N*(N-1)/2;
    const double sum_x2 = (N-1)*N*(2*N-1)/6;

  double  sum_y = 0, sum_y2 = 0, sum_xy = 0;

  for(unsigned int i = 0; i < N; i++){
    sum_y += values[i];
    sum_y2+= values[i]*values[i];
    sum_xy+= i*values[i];
  }
  double pmcc = (N*sum_xy - sum_x*sum_y)/sqrt((N*sum_x2 - sum_x*sum_x)*(N*sum_y2 - sum_y*sum_y));
  return pmcc;
  }

    /**
     * Calculate the PMCC of a circular_buffer of (x,y) pairs. This is used to calculate the PMCC of the previous MRMS values.
     *
     * @param x The x values of the (x,y) pairs
     *
     * @param y The y values of the (x,y) pairs
 */

  double CalculatePMCC(std::vector<double> x, std::vector<double> y){
    const unsigned int N = x.size();
    if(x.size() <= 2){
      /*We don't have enough values*/
      return -NAN;
    }
    double sum_x = 0, sum_x2 = 0, sum_y = 0, sum_y2 = 0, sum_xy = 0;

    /*Calculate the required sums */
    for(unsigned int i = 0; i < N; i++){
      sum_x  += x[i];
      sum_x2 += x[i]*x[i];
      sum_y  += y[i];
      sum_y2 += y[i]*y[i];
      sum_xy += x[i]*y[i];
    }
    /*Return the PMCC*/
    return (N*sum_xy - sum_x*sum_y)/sqrt((N*sum_x2 - sum_x*sum_x)*(N*sum_y2 - sum_y*sum_y));
  }

  /**
   * Applies the extrapolation method to one state variable if it is sensible to do so. Returns
   * true if the state variable has been extrapolated.
   *
   * @state_index The index of the state variable that we wish to extrapolate
   */
  bool ExtrapolateState(unsigned int state_index);

   /**
   * Applies the extrapolation method to every state variable
   *
   * @state_index The index of the state variable that we wish to extrapolate
   */
  bool ExtrapolateStates();

  /**
   * Simulates one pace and extrapolates the state variables if the PMCC
   * of the previous MRMS values is approximately -1
   */
  bool RunPace();

public:
/**
     * Constructor of a helper class for getting action potential models to steady state.
     *
     * @param pModel  The cell model to run to steady state.
     * @param period  The period for the model to be paced at.
     */

  SteadyStateExtrapolationRunner(boost::shared_ptr<AbstractCvodeCell> pModel, double period)
    : AbstractSteadyStateRunner(pModel), mPeriod(period){
    /*This ensures that we are using a regular stimulus*/
    mpStimulus = mpModel->UseCellMLDefaultStimulus();
    mpStimulus->SetPeriod(mPeriod);
    mDuration = mpStimulus->GetDuration();

    /* Set Solver Tolerances */
    mpModel->SetTolerances(TolRel, TolAbs);

    /* Set the maximum number of timesteps for CVODE to use. This prevents
     * CV_TOO_MUCH_WORK errors */
    mpModel->SetMaxSteps(1e5);

    /* Initialise mStateVariables */
    mStateVariables = mpModel->GetStdVecStateVariables();

    /* Set the capacity of the circular buffers */
    mMrmsBuffer.set_capacity(mBufferSize);
    mStatesBuffer.set_capacity(mBufferSize);
  };

};

#endif // CHASTE_CVODE

#endif // _STEADYSTATEEXTRAPOLATIONRUNNER_HPP_


#include <iostream>
#include <vector>
#include <fstream>
#include <string>
#include <algorithm>
#include <thread>
#include <cmath>

#include <boost/lexical_cast.hpp>

#include "Point.hpp"
#include "Function.hpp"
#include "SymbolicRegressionSolver.hpp"
#include "Config.hpp"

//#define OPTIMISE_CONFIG
//#define MUTATE_MATE

int amountOfSimulationsToPerform = 1;
SymbolicRegressionSolver::Config config{};
Function fn{parse("+ 1 x").statement, "x"};
int initialPoint = -10;
int endPoint = 10;
int stepSize = 1;

struct Result
{
    Result(double var, double average, size_t totalSolutions, double stdDev) : var(var), average(average), totalSolutions(totalSolutions), stdDev(stdDev) { }
    Result(double var, double var2, double average, size_t totalSolutions, double stdDev) : var(var), var2(var2), average(average), totalSolutions(totalSolutions), stdDev(stdDev) { }
    double var;
    double var2;
    double average;
    size_t totalSolutions;
    double stdDev;
};

constexpr const int AMOUNT_OF_THREADS = 4;
std::vector<Result> solutions[AMOUNT_OF_THREADS];

std::istream& operator>>(std::istream& is, SymbolicRegressionSolver::Config& config);
std::ostream& operator<<(std::ostream& os, const SymbolicRegressionSolver::Config& config);

void parseArguments(int argc, char *argv[]);
void printValidFlags()
{
    std::cout << "-c <configuration-file> (TODO)\n";
    std::cout << "-f <function>\n";
    std::cout << "-i <initial-point>\n";
    std::cout << "-e <end-point>\n";
    std::cout << "-s <step size>\n";
    std::cout << "-r <amount of times to redo simulation>\n";
}

template <class F>
void runSolver(int N, const PointList& p, SymbolicRegressionSolver::Config c, F f)
{
    SymbolicRegressionSolver solver{c, p};
    for(int i = 0; i < N; ++i)
    {
        f(solver, solver.solve());
        solver.reset();
    }
}

template <class T>
void optimiseConfig(size_t id, const PointList& points, const T& start, const T& end, const T& step)
{
    SymbolicRegressionSolver solver{config, points};
#ifndef MUTATE_MATE
    auto& variableToModify = solver.config().keepPercentage;
#else
    auto& variableToModify = solver.config().matePercent;
#endif

    std::vector<size_t> temp;

#ifdef MUTATE_MATE
    auto& var2 = solver.config().mutationPercent;
    for(var2 = start; var2 <= end; var2 += step)
#endif
#ifndef MUTATE_MATE
    for(variableToModify = start; variableToModify <= end; variableToModify += step)
#else
    for(variableToModify = 0; variableToModify <= 1; variableToModify += step)
#endif
    {
        double currentAverage = 0;
        size_t totalSolutions = 0;

        for(int i = 0; i < amountOfSimulationsToPerform; ++i)
        {
            auto solutions = solver.solve();

            totalSolutions += solutions.size();
            currentAverage += solver.currentGeneration();
            temp.emplace_back(solver.currentGeneration());

            solver.reset();
        }

        currentAverage /= amountOfSimulationsToPerform;

        double stdDev = 0;
        for(auto i : temp)
        {
            auto diff = i - currentAverage;
            stdDev += diff * diff;
        }
        stdDev /= amountOfSimulationsToPerform;

#ifndef MUTATE_MATE
        solutions[id].emplace_back(variableToModify, currentAverage, totalSolutions, stdDev);
#else
        solutions[id].emplace_back(var2, variableToModify, currentAverage, totalSolutions, stdDev);
#endif
    }
}

void optimiseConfig(const PointList& points)
{
    std::vector<std::thread> threads;
    for(int i = 0; i < AMOUNT_OF_THREADS; ++i)
    {
        constexpr double STEP_SIZE = 0.005;
        double start = i * 1.0 / AMOUNT_OF_THREADS;
        double end = (i + 1) * 1.0 / AMOUNT_OF_THREADS;

        threads.emplace_back([=]() { optimiseConfig(i, points, start, end,  STEP_SIZE); } );
    }

    for(auto& t : threads)
    {
        t.join();
    }

    std::vector<Result> sols;
    for(auto& sol : solutions)
    {
        sols.insert(sols.end(), sol.begin(), sol.end());
    }
    std::sort(sols.begin(), sols.end(), [](const Result& r1, const Result& r2) { return r1.var < r2.var; });

    for(auto& sol : sols)
    {
#ifndef MUTATE_MATE
        std::cout << sol.var << ", " << sol.average << ", " << sol.totalSolutions << ", " << sol.stdDev << '\n';
#else
        std::cout << sol.var << ", " << sol.var2 << ", " << sol.average << ", " << sol.totalSolutions << ", ", sol.stdDev << '\n';
#endif // MUTATE_MATE
    }
}

int main(int argc, char *argv[])
{
    parseArguments(argc, argv);

#ifdef VERBOSE_LOG
    std::clog << "function: " << fn << '\n';
    std::clog << "initial point: " << initialPoint << '\n';
    std::clog << "end point: " << endPoint << '\n';
    std::clog << "step size: " << stepSize << '\n';
#endif // VERBOSE_LOG

    std::vector<Point> points;
    points.reserve((endPoint - initialPoint)/stepSize);

    VariableMap map;
    for(int i = initialPoint; i <= endPoint; i += stepSize)
    {
        points.emplace_back(i, fn(map, i));
    }

#ifdef OPTIMISE_CONFIG
    optimiseConfig(points);
#else
    runSolver(amountOfSimulationsToPerform, points, config, [](SymbolicRegressionSolver& solver, const SolutionList& sols)
            {
                for(auto& solution : sols)
                {
                    std::cout << solver.currentGeneration() << ",";
                    std::cout << solution.fitnessLevel << ",";
                    std::cout << solution.mutated << ",";
                    std::cout << solution.mated << '\n';
                }
            });
#endif

    return 0;
}

void parseArguments(int argc, char *argv[])
{
    for(int i = 1; i < argc; ++i)
    {
        auto command = argv[i];
        auto commandSize = strlen(command);
        if(command[0] != '-')
        {
            std::cout << "Invalid format: " << command << ".\n";
            std::cout << "Flags must be prefixed with \"-\" (without quotes).\n";
            std::exit(-2);
            break;
        }

        if(commandSize == 1 || commandSize > 2)
        {
            std::cout << "Invalid flag: \"" << command << "\"\n";
            printValidFlags();
            std::exit(-5);
        }

        if(i + 1 >= argc)
        {
            std::cout << "please provide info with a flag...\n";
            printValidFlags();
            std::exit(-6);
        }

        switch(command[1])
        {
            // assign function
            case 'f':
                {
                    std::string functionAsString;
                    for(int j = i + 1; j < argc && argv[j][0] != '-'; ++j, ++i)
                    {
                        functionAsString += argv[j];
                    }
                    fn = parse(functionAsString).statement;
                }
                break;
            case 'i':
                {
                    std::string str = argv[++i];
                    initialPoint = boost::lexical_cast<int>(str);
                }
                break;
            case 'e':
                {
                    std::string str = argv[++i];
                    endPoint = boost::lexical_cast<int>(str);
                }
                break;
            case 's':
                {
                    std::string str = argv[++i];
                    stepSize = boost::lexical_cast<int>(str);
                }
                break;
            case 'r':
                {
                    std::string str = argv[++i];
                    amountOfSimulationsToPerform = boost::lexical_cast<int>(str);
                }
                break;
            case 'c':
                {
                    try
                    {
                        std::string filepath = argv[++i];
                        std::ifstream file;
                        file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
                        file.open(filepath);
                        file >> config; // read the config
                    }
                    catch(boost::bad_lexical_cast& e)
                    {
                        std::cerr << e.what();
                        std::exit(5);
                    }
                    catch(std::exception& e)
                    {
                        std::cerr << e.what();
                        std::exit(6);
                    }
                }
                break;
            default:
                std::cout << "Invalid flag\n";
                printValidFlags();
                std::exit(-4);
                break;
        }
    }
}

namespace
{
std::string obtainValue(const std::string& line)
{
    return std::string(line.begin() + line.find_last_of('=') + 1, line.end());
}

template <class T>
void read(std::string& buffer, std::istream& is, T& value)
{
    std::getline(is, buffer);
    auto stringValue = obtainValue(buffer);
    stringValue.erase(std::remove_if(stringValue.begin(), stringValue.end(), [](char c) { return std::isspace(c); }), stringValue.end());
    value = boost::lexical_cast<T>(stringValue);
}
}

std::istream& operator>>(std::istream& is, SymbolicRegressionSolver::Config& config)
{
    std::string buffer;

    read(buffer, is, config.initialPopulation);
    read(buffer, is, config.maxGenerations);
    read(buffer, is, config.initialMaxDepth);
    read(buffer, is, config.maxSolutionDepth);
    read(buffer, is, config.keepPercentage);
    read(buffer, is, config.mutationPercent);
    read(buffer, is, config.matePercent);

    read(buffer, is, config.generateConstantNodes);

    // have to do this separately for const dist
    int a, b;
    read(buffer, is, a);
    read(buffer, is, b);

    config.constantDist = decltype(config.constantDist){a, b};

    read(buffer, is, config.solutionCriterea);
    read(buffer, is, config.chanceToChangeConstant);
    read(buffer, is, config.chanceToChangeVar);

    int nearestNeighbour = 0;
    read(buffer, is, nearestNeighbour);
    config.nearestNeighbourOption = static_cast<decltype(config.nearestNeighbourOption)>(nearestNeighbour);

    read(buffer, is, config.chanceToUseNearestNeighbour);
    read(buffer, is, config.stepSize);

    int refillOption = 0;
    read(buffer, is, refillOption);
    config.populationRefillOption = static_cast<decltype(config.populationRefillOption)>(refillOption);

    return is;
}


//==================================================================================================
/*
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/logical.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <algorithm>
#include <numeric>

namespace bs = boost::simd;

template <typename T, std::size_t N, typename Env>
void test(Env& $)
{
  std::array<T, N> ref;
  std::iota(ref.begin(), ref.end(), T(1));
  bs::pack<T, N> p0(ref.begin(), ref.end()), res;

  std::transform(p0.begin(),p0.end(),res.begin(), [](T e) { return e; } );
  STF_ALL_EQUAL(res, ref);
}

STF_CASE_TPL( "Check proxy access" , STF_NUMERIC_TYPES)
{
  test<T,  1>($);
  test<T,  2>($);
  test<T,  4>($);
  test<T,  8>($);
  test<T, 16>($);
  test<T, 32>($);
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;
using Eigen::RowMajor;

static void test_orderings()
{
  Tensor<float, 3> mat1(2,3,7);
  Tensor<float, 3> mat2(2,3,7);
  Tensor<bool, 3> lt(2,3,7);
  Tensor<bool, 3> le(2,3,7);
  Tensor<bool, 3> gt(2,3,7);
  Tensor<bool, 3> ge(2,3,7);

  mat1.setRandom();
  mat2.setRandom();

  lt = mat1 < mat2;
  le = mat1 <= mat2;
  gt = mat1 > mat2;
  ge = mat1 >= mat2;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_EQUAL(lt(i,j,k), mat1(i,j,k) < mat2(i,j,k));
        VERIFY_IS_EQUAL(le(i,j,k), mat1(i,j,k) <= mat2(i,j,k));
        VERIFY_IS_EQUAL(gt(i,j,k), mat1(i,j,k) > mat2(i,j,k));
        VERIFY_IS_EQUAL(ge(i,j,k), mat1(i,j,k) >= mat2(i,j,k));
      }
    }
  }
}

static void test_equality()
{
  Tensor<float, 3> mat1(2,3,7);
  Tensor<float, 3> mat2(2,3,7);

  mat1.setRandom();
  mat2.setRandom();
  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        if (internal::random<bool>()) {
          mat2(i,j,k) = mat1(i,j,k);
        }
      }
    }
  }

  Tensor<bool, 3> eq(2,3,7);
  Tensor<bool, 3> ne(2,3,7);
  eq = (mat1 == mat2);
  ne = (mat1 != mat2);

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_EQUAL(eq(i,j,k), mat1(i,j,k) == mat2(i,j,k));
        VERIFY_IS_EQUAL(ne(i,j,k), mat1(i,j,k) != mat2(i,j,k));
      }
    }
  }
}

void test_cxx11_tensor_comparisons()
{
  CALL_SUBTEST(test_orderings());
  CALL_SUBTEST(test_equality());
}


// Copyright 2018-2019 Josh Pieper, jjp@pobox.com.
//
// 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 "mjlib/base/windowed_average.h"

#include <boost/test/auto_unit_test.hpp>

using namespace mjlib::base;

BOOST_AUTO_TEST_CASE(WindowedAverageTest) {
  WindowedAverage<int16_t, 4, int32_t> dut;
  BOOST_TEST(dut.average() == 0);
  BOOST_TEST(dut.total() == 0);
  BOOST_TEST(dut.size() == 0);

  dut.Add(2);
  BOOST_TEST(dut.average() == 2);
  BOOST_TEST(dut.total() == 2);
  BOOST_TEST(dut.size() == 1);

  dut.Add(4);
  BOOST_TEST(dut.average() == 3);
  BOOST_TEST(dut.total() == 6);
  BOOST_TEST(dut.size() == 2);

  dut.Add(6);
  BOOST_TEST(dut.average() == 4);
  BOOST_TEST(dut.total() == 12);
  BOOST_TEST(dut.size() == 3);

  dut.Add(8);
  BOOST_TEST(dut.average() == 5);
  BOOST_TEST(dut.total() == 20);
  BOOST_TEST(dut.size() == 4);

  // Finally, drop the first element.
  dut.Add(10);
  BOOST_TEST(dut.average() == 7);
  BOOST_TEST(dut.total() == 28);
  BOOST_TEST(dut.size() == 4);
}

BOOST_AUTO_TEST_CASE(WindowedAverageCapacityTest) {
  WindowedAverage<int16_t, 4, int32_t> dut{2};
  BOOST_TEST(dut.average() == 0);
  BOOST_TEST(dut.total() == 0);
  BOOST_TEST(dut.size() == 0);

  dut.Add(2);
  BOOST_TEST(dut.average() == 2);
  BOOST_TEST(dut.total() == 2);
  BOOST_TEST(dut.size() == 1);

  dut.Add(4);
  BOOST_TEST(dut.average() == 3);
  BOOST_TEST(dut.total() == 6);
  BOOST_TEST(dut.size() == 2);

  // This will drop the  2.
  dut.Add(6);
  BOOST_TEST(dut.average() == 5);
  BOOST_TEST(dut.total() == 10);
  BOOST_TEST(dut.size() == 2);
}

BOOST_AUTO_TEST_CASE(BiggerTest) {
  WindowedAverage<int16_t, 256, int32_t> dut;
  for (int i = 0; i < 1024; i++) {
    dut.Add(1000);
  }
  BOOST_TEST(dut.average() == 1000);
  BOOST_TEST(dut.total() == 1000 * 256);
  BOOST_TEST(dut.size() == 256);
}


#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <map>

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

#include "Day6.hpp"

void day6() {
	std::ifstream inputFile("Day6.txt");
	std::string line;
	std::vector<std::string> strings;
	std::map<std::string, std::string> map;
	const std::string CENTER_OF_MASS = "COM";
	const std::string YOU = "YOU";
	const std::string SANTA = "SAN";

	while (inputFile >> line) {
		//DES)SRC
		std::vector<std::string> numbersStr;
		boost::split(numbersStr, line, boost::is_any_of(")"), boost::token_compress_on);

		// Cheating
		std::string dest(numbersStr[0]);
		std::string src(numbersStr[1]);

		map[src] = dest;

		std::cout << "SRC: " << src << " - DEST: " << dest << std::endl;
	}

	int countOfDirectOrbits = 0;
	int countOfIndirectOrbits = 0;

	// For each planet
	for (std::map<std::string, std::string>::iterator it = map.begin(); it != map.end(); ++it) {
		countOfDirectOrbits++;

		//std::string tempDest = map[it->first];
		//std::map<std::string, std::string>::iterator itTemp = it;
		//std::pair<std::string, std::string> tempPair = make_pair(it->first, it->second);
		std::string tempDest = it->second;

		// For each indirect orbit of that planet, until COM
		while (tempDest != CENTER_OF_MASS) {
			tempDest = map[tempDest];

			countOfIndirectOrbits++;
		}
	}
	std::string commonAncestor("");

	std::vector<std::string> pathFromYOUtoCOM;
	int stepsFromYOUtoCommonAncestor = 0;
	{
		std::string tempDest = map[YOU];

		// For each indirect orbit of that planet, until COM
		while (tempDest != CENTER_OF_MASS) {
			tempDest = map[tempDest];
			pathFromYOUtoCOM.push_back(tempDest);
		}
	}
	std::cout << "Path from YOU to COM: " << std::endl;
	for (auto i = pathFromYOUtoCOM.begin(); i != pathFromYOUtoCOM.end(); ++i)
		std::cout << *i << ' ';
	std::cout << std::endl << std::endl;

	std::vector<std::string> pathFromSANtoCOM;
	int stepsFromSANTAtoCommonAncestor = 1;
	{
		std::string tempDest = map[SANTA];

		// For each indirect orbit of that planet, until COM
		while (tempDest != CENTER_OF_MASS) {
			tempDest = map[tempDest];
			pathFromSANtoCOM.push_back(tempDest);

			if (commonAncestor == "") {
				auto t = std::find(pathFromYOUtoCOM.begin(), pathFromYOUtoCOM.end(), tempDest);
				if (t != pathFromYOUtoCOM.end()) {
					// found
					commonAncestor = tempDest;
				}
				else {
					stepsFromSANTAtoCommonAncestor++;
				}
			}
		}
	}
	std::cout << "Path from YOU to COM: " << std::endl;
	for (auto i = pathFromSANtoCOM.begin(); i != pathFromSANtoCOM.end(); ++i)
		std::cout << *i << ' ';
	std::cout << std::endl << std::endl;

	std::cout << commonAncestor << std::endl;

	// Steps from YOU to common ancestor
	std::vector<std::string>::iterator it = std::find(pathFromYOUtoCOM.begin(), pathFromYOUtoCOM.end(), commonAncestor);
	stepsFromYOUtoCommonAncestor = std::distance(pathFromYOUtoCOM.begin(), it) + 1;

	std::cout << "Steps from SANTA to common ancestor: " << stepsFromSANTAtoCommonAncestor << std::endl;
	std::cout << "Steps from YOU to common ancestor: " << stepsFromYOUtoCommonAncestor << std::endl;

	std::cout << (countOfDirectOrbits + countOfIndirectOrbits) << std::endl;
}


/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/
#pragma once

#include <Eigen/Core>

#include "kindr/math/LinearAlgebra.hpp"
#include "kindr/common/common.hpp"
#include "kindr/common/assert_macros.hpp"
#include "kindr/common/assert_macros_eigen.hpp"
#include "kindr/rotations/RotationDiffBase.hpp"
#include "kindr/rotations/Rotation.hpp"
#include "kindr/quaternions/Quaternion.hpp"

namespace kindr {

/*! \class RotationQuaternionDiff
 * \brief Time derivative of a rotation quaternion.
 *
 * This class implements the time derivative of a rotation quaternion using a Quaternion<Scalar> as data storage.
 *
 * \tparam PrimType_  Primitive data type of the coordinates.
 * \ingroup rotations
 */
template<typename PrimType_>
class RotationQuaternionDiff : public RotationDiffBase<RotationQuaternionDiff<PrimType_>>, private Quaternion<PrimType_> {
 private:
  /*! \brief The base type.
   */
  typedef Quaternion<PrimType_> Base;

 public:
  /*! \brief The implementation type.
   *  The implementation type is always an Eigen object.
   */
  typedef typename Base::Implementation Implementation;

  /*! \brief The primitive type.
   *  Float/Double
   */
  typedef PrimType_ Scalar;

  //! the imaginary type, i.e., Eigen::Quaternion<>
  typedef Eigen::Matrix<PrimType_, 3, 1> Imaginary;

  //! quaternion as 4x1 matrix: [w; x; y; z]
  typedef Eigen::Matrix<PrimType_,4,1> Vector4;

  /*! \brief Default constructor sets all derivatives to zero
   *
   */
  RotationQuaternionDiff()
    : Base() {
  }

  explicit RotationQuaternionDiff(const Base& other) // explicit on purpose
    : Base(other) {
  }

  /*! \brief Constructor using four scalars.
   *  \param w     first entry of the derivative of the quaternion
   *  \param x     second entry of the derivative of the quaternion
   *  \param y     third entry of the derivative of the quaternion
   *  \param z     fourth entry of the derivative of the quaternion
   */
  RotationQuaternionDiff(Scalar w, Scalar x, Scalar y, Scalar z)
    : Base(w,x,y,z) {
  }

  RotationQuaternionDiff(const Vector4 & vec)
    : Base(vec(0),vec(1),vec(2),vec(3)) {
  }

  RotationQuaternionDiff(const Scalar& real, const Imaginary& imag)
    : Base(real, imag)  {
  }

  /*! \brief Constructor using Eigen::Quaternion<PrimType_>.
   *  \param other   Eigen::Quaternion<PrimType_>
   */
  explicit RotationQuaternionDiff(const Implementation& other)
      : Base(other.w(),other.x(),other.y(),other.z()) {
  }

  /*! \brief Constructor using a time derivative with a different parameterization
   *
   * \param rotation  rotation
   * \param other     other time derivative
   */
  template<typename RotationDerived_, typename OtherDerived_>
  inline explicit RotationQuaternionDiff(const RotationBase<RotationDerived_>& rotation, const RotationDiffBase<OtherDerived_>& other)
    : Base(internal::RotationDiffConversionTraits<RotationQuaternionDiff, OtherDerived_, RotationDerived_>::convert(rotation.derived(), other.derived()).toQuaternion()){
  }

  /*! \brief Cast to another representation of the time derivative of a rotation
   *  \param other   other rotation
   *  \returns reference
   */
  template<typename OtherDerived_, typename RotationDerived_>
  OtherDerived_ cast(const RotationBase<RotationDerived_>& rotation) const {
    return internal::RotationDiffConversionTraits<OtherDerived_, RotationQuaternionDiff, RotationDerived_>::convert(rotation.derived(), *this);
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline Implementation& toImplementation() {
    return this->toQuaternion().toImplementation();
  }

  /*! \brief Cast to the implementation type.
   *  \returns the implementation (recommended only for advanced users)
   */
  inline const Implementation& toImplementation() const {
    return this->toQuaternion().toImplementation();
  }

  /*! \brief Cast to the base type.
   *  \returns the quaternion that contains the derivative (recommended only for advanced users)
   */
  Base& toQuaternion()  {
    return static_cast<Base&>(*this);
  }

  /*! \brief Cast to the base type.
   *  \returns the quaternion that contains the derivative (recommended only for advanced users)
   */
  const Base& toQuaternion() const {
    return static_cast<const Base&>(*this);
  }

  using Base::w;
  using Base::x;
  using Base::y;
  using Base::z;

  using Base::real;
  using Base::imaginary;
  using Base::vector;

  /*! \brief Sets all time derivatives to zero.
   *  \returns reference
   */
  RotationQuaternionDiff& setZero() {
    Base::setZero();
    return *this;
  }

  /*! \brief Get zero element.
   *  \returns zero element
   */
  static RotationQuaternionDiff Zero() {
    return RotationQuaternionDiff(Base::Zero());
  }

  /*! \brief Used for printing the object with std::cout.
   *  \returns std::stream object
   */
  friend std::ostream& operator << (std::ostream& out, const RotationQuaternionDiff& diff) {
    out << diff.w() << " " << diff.x() << " " << diff.y() << " " << diff.z();
    return out;
  }
};

//! \brief Time derivative of a rotation quaternion with primitive type double
typedef RotationQuaternionDiff<double> RotationQuaternionDiffPD;
//! \brief Time derivative of a rotation quaternion with primitive type float
typedef RotationQuaternionDiff<float> RotationQuaternionDiffPF;
//! \brief Time derivative of a rotation quaternion with primitive type double
typedef RotationQuaternionDiff<double> RotationQuaternionDiffD;
//! \brief Time derivative of a rotation quaternion with primitive type float
typedef RotationQuaternionDiff<float> RotationQuaternionDiffF;

namespace internal {

template<typename PrimType_>
class RotationDiffConversionTraits<RotationQuaternionDiff<PrimType_>, LocalAngularVelocity<PrimType_>, RotationQuaternion<PrimType_>> {
 public:
  inline static RotationQuaternionDiff<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const LocalAngularVelocity<PrimType_>& angularVelocity) {
    return RotationQuaternionDiff<PrimType_>(Quaternion<PrimType_>(0.5*(rquat.getLocalQuaternionDiffMatrix().transpose()*angularVelocity.vector())));
  }
};

template<typename PrimType_>
class RotationDiffConversionTraits<RotationQuaternionDiff<PrimType_>, GlobalAngularVelocity<PrimType_>, RotationQuaternion<PrimType_>> {
 public:
  inline static RotationQuaternionDiff<PrimType_> convert(const RotationQuaternion<PrimType_>& rquat, const GlobalAngularVelocity<PrimType_>& angularVelocity) {
    return RotationQuaternionDiff<PrimType_>(Quaternion<PrimType_>(0.5*(rquat.getGlobalQuaternionDiffMatrix().transpose()*angularVelocity.vector())));
  }
};

} // namespace internal
} // namespace kindr


// The MIT License (MIT)
//
// Copyright (c) 2015 Christopher Jefferson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <iostream>
#include <signal.h>

#include <boost/multiprecision/cpp_int.hpp>

using namespace boost::multiprecision;

//  g++ shiftqueens-boost-1024.cc -I/usr/local/Cellar/boost/1.57.0/include/ -o shiftqueens-boost-1024 -O3
// for i in `ls`; do  ../../shiftqueens-boost-1024 $(cat $i | tr -cd " 0-9") > ../results.d25/$i.info; done

//#define PRINT_INFO

#ifdef PRINT_INFO
#define INFO(...) printf(__VA_ARGS__);
#define PAD(x) pad(x);
#define PRINTBITS(x) printBits(x)
#else
#define INFO(...)
#define PAD(x)
#define PRINTBITS(x)
#endif

//#define PRINT_SOL

#ifdef PRINT_SOL
static int sol[100];
#endif

#include <sys/resource.h>
#include <sys/times.h>

double gettime() {
	struct rusage rusage;
	if ( getrusage( RUSAGE_SELF, &rusage ) != -1 )
		return (double)rusage.ru_utime.tv_sec +
			(double)rusage.ru_utime.tv_usec / 1000000.0;

	std::cerr << "Fatal error in reading time\n";
	exit(1);
}

volatile int trig = false;

void trigger_function(int /* signum */) {
  trig = true;
}

void setup_timelimit(int timeout) {
	signal(SIGXCPU, trigger_function);
	signal(SIGALRM, trigger_function);
	rlimit lim;
	lim.rlim_cur = timeout;
	lim.rlim_max = timeout + 5;
	setrlimit(RLIMIT_CPU, &lim);
}

long long node_count = 0;

static int1024_t all;
static const int1024_t zero = 0;
static const int1024_t one = 1;
static int depth;
static int count = 0;

int fillLeft[1024];
int fillRight[1024];
int skipDepth[1024];

#ifdef PRINT_SOL
void print_sol()
{
	for(int i = 0; i < depth; ++i)
	{
		if(skipDepth[i])
			printf("-");
		else
			printf("|");
		for(int j = 0; j < depth; ++j)
		{
			if(1<<j == sol[i])
				printf("#|");
			else
			  printf(" |");
		}
		printf("\n");
		for(int j = 0; j < depth; ++j)
		printf("--");
		printf("-\n");
	}
	printf("\n");
}
#endif

#ifdef PRINT_INFO
void printBits(int1024_t bits)
{
	for(int i = 0; i < depth; ++i) {
		printf(bits & one ? "1" : "0");
		bits = bits >> 1;
	}
}
#endif

void pad(int curDepth)
{
		for(int i=0; i < curDepth; ++i)
			printf(" ");
}

void print_info() {
        std::cout << "Solutions: " << count << " ";
        std::cout << "Nodes: " << node_count << " ";
        std::cout << "Timeout: " << trig << " ";
        std::cout << "Time: " << gettime() << "\n";
}

void tryFunc(int1024_t ld, int1024_t cols, int1024_t rd, int curDepth)
{
		node_count++;
		if(trig) {
			print_info();
			std::cout << "Timeout Reached. Exiting.\n";
			exit(0);
		}
		PAD(curDepth);
		PRINTBITS(ld);
		INFO(",");
		PRINTBITS(cols);
		INFO(",");
		PRINTBITS(rd);
		INFO(":%d ", curDepth);
	if(curDepth == depth)
	{
		INFO("\n");
		count++;
		std::cout << "Solution Found\n";
#ifdef PRINT_SOL
		print_sol();
#endif
		print_info();

		exit(0);
	}

	int1024_t shiftld = ld << 1;
	int1024_t shiftrd = rd >> 1;

	assert((shiftld & one) == 0);
	assert((shiftrd & (one << (depth - 1))) == 0);

	if(fillLeft[curDepth+1])
	{
		INFO("*");
		shiftld |= one;
	}
	else
	INFO(" ");

	if(fillRight[curDepth+1])
	{
		INFO("*");
		shiftrd |= one << (depth - 1);
	}
	else INFO(" ");

	// If we already put a queen on this depth, skip it.
	if(skipDepth[curDepth])
	{
		INFO(" -- skip\n");
		return tryFunc(shiftld, cols, shiftrd, curDepth + 1);
	}

	PRINTBITS(shiftld);
		INFO(",");
		PRINTBITS(shiftrd);
		INFO("\n");

	{
		int1024_t poss = ~(ld | cols | rd) & all;
		while(poss != zero) {
			int1024_t bit = poss & -poss;
			poss -= bit;
#ifdef PRINT_SOL
			sol[curDepth] = bit;
#endif
			PAD(curDepth);
			PRINTBITS(bit);
			INFO(" added\n");
			tryFunc((bit<<1)|shiftld, cols|bit, (bit>>1)|shiftrd, curDepth + 1);
		}
	}
}

int main(int argc, char** argv)
{
	// SETUP TIMELIMIT HERE
	setup_timelimit(1800);

	if(argc % 2 != 0)
	{
		printf("./shiftqueens n queen1x queen1y queen2x queen2y ...\n");
		exit(1);
	}

	depth = atoi(argv[1]);
	all = (one << depth) - 1;

	int1024_t cols = 0;
	int1024_t ld = 0;
	int1024_t rd = 0;

	printf("%d\n", argc);

	for(int i = 2; i < argc; i+=2)
	{
		int queenx = atoi(argv[i]);
		int queeny = atoi(argv[i+1]);
		// printf("%d,%d\n", queenx, queeny);

		if(skipDepth[queeny]) {
			printf("inconsistent y\n");
			exit(1);
		}

		skipDepth[queeny] = 1;
#ifdef PRINT_SOL
		sol[queeny] = one << queenx;
#endif

		int1024_t colpos = one << queenx;
		if((cols & colpos)!=zero) {
			printf("Inconsistent start\n");
			exit(1);
		}
		cols |= colpos;

		{
			int rightintersect = queenx + queeny;
			if(rightintersect < depth)
			{
				int1024_t newrd = one << rightintersect;
				if((rd & newrd)!=zero) {
					printf("inconsistent diagonal\n");
					exit(1);
				}
				rd |= newrd;
			}
			else
			{
					rightintersect -= (depth - 1);
					if(fillRight[rightintersect]) {
						printf("inconsistent diagonal - type 2\n");
						exit(1);
					}
					fillRight[rightintersect] = 1;
			}
		}

		{
			int leftintersect = queenx - queeny;
			if(leftintersect >= 0)
			{
				int1024_t newld = one << leftintersect;
				if((ld & newld)!=zero) {
					printf("inconsistent left diagonal\n");
					exit(1);
				}
				ld |= newld;
			}
			else
			{
					leftintersect *= -1;
					if(fillLeft[leftintersect]) {
						printf("inconsistent left diagonal - type 2\n");
						exit(1);
					}
					fillLeft[leftintersect] = 1;
			}
		}
}

	count = 0;
	tryFunc(ld, cols, rd, 0);
	std::cout << "No solution" << "\n";
	print_info();
	exit(0);
}


/*-----------------------------------------------------------------------------+
Copyright (c) 2011-2011: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#define BOOST_TEST_MODULE icl::fix_icl_after_thread unit test
#include <libs/icl/test/disable_test_warnings.hpp>
#include "../unit_test_unwarned.hpp"

//#define BOOST_ICL_USE_STATIC_BOUNDED_INTERVALS
#include <boost/icl/interval_map.hpp>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/separate_interval_set.hpp>
#include <boost/icl/split_interval_set.hpp>

#include <iostream>

using namespace std;
using namespace boost;
using namespace unit_test;
using namespace boost::icl;

BOOST_AUTO_TEST_CASE(ticket_5482)
{
    typedef interval_map<int,int,partial_absorber,std::less>    m1_t;
    typedef interval_map<int,int,partial_absorber,std::greater> m2_t;
    m1_t m1;
    m2_t m2;
    m1.insert(make_pair(m1_t::interval_type(1), 20));
    m1.insert(make_pair(m1_t::interval_type(2), 20));
    m1.insert(make_pair(m1_t::interval_type(3), 20));

    m2.insert(make_pair(m2_t::interval_type(1), 20));
    m2.insert(make_pair(m2_t::interval_type(2), 20));
    m2.insert(make_pair(m2_t::interval_type(3), 20));

    BOOST_CHECK_EQUAL(m1.iterative_size(), m2.iterative_size());
    BOOST_CHECK_EQUAL(m1.iterative_size(), 1);
    BOOST_CHECK_EQUAL(m2.iterative_size(), 1);
}

#include <boost/cstdint.hpp>
BOOST_AUTO_TEST_CASE(ticket_5559_Denis)
{
    //Submitted by Denis
    typedef boost::icl::interval_set<boost::uint32_t, std::greater> Set;
    const uint32_t ui32_max = (std::numeric_limits<uint32_t>::max)();

    Set q1( Set::interval_type::closed(ui32_max, 0) );
    Set q5( Set::interval_type::closed(0, 0) );

    BOOST_CHECK_EQUAL(q1, q1+q5);
}

//------------------------------------------------------------------------------
// Ticket #6095 Marvin Sielenkemper
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ticket_6095_Marvin_Sielenkemper_isEmptyTest)
{
    typedef int                         Value;
    typedef boost::icl::interval<Value> Interval;
    typedef std::numeric_limits<Value>  Limits;

    Value const max((Limits::max)());

    //Interval::interval_type piff = Interval::open(max,     max);

    BOOST_CHECK(!icl::is_empty(Interval::open(max - 2, max)));
    BOOST_CHECK( icl::is_empty(Interval::open(max - 1, max)));
    BOOST_CHECK( icl::is_empty(Interval::open(max,     max)));
}

BOOST_AUTO_TEST_CASE(ticket_6095_Marvin_Sielenkemper_totalRangeTest)
{
    typedef int                                                              Value;
    typedef boost::icl::interval<Value>                                      Interval;
    typedef std::numeric_limits<Value>                                       Limits;

    Value const min((Limits::min)());
    Value const max((Limits::max)());

    boost::icl::interval_map<Value, int, boost::icl::total_enricher> intervals;

    intervals += std::make_pair(Interval::closed(min, max), 0);
    intervals += std::make_pair(Interval::right_open(0, 10),  3);

    BOOST_CHECK_EQUAL(intervals.iterative_size(), 3);
}

BOOST_AUTO_TEST_CASE(test_ticket_7871)
{
    boost::icl::split_interval_map <int, int> a, b;
    a = b;
    BOOST_CHECK_EQUAL(a, b);
}

//------------------------------------------------------------------------------
// Ticket #11308 Stephen Hewitt
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ticket_11308_Stephen_Hewitt_extreme_valued_closed_intervals)
{
    typedef boost::icl::closed_interval<boost::uint16_t, std::less> interval_t;
    typedef boost::icl::split_interval_set<boost::uint16_t, std::less, interval_t> set_t;

    set_t s1, s2;

    s1 += interval_t(0x0000, 0x9fff);
    s1 += interval_t(0xa000, 0xbfff);
    s1 += interval_t(0xc000, 0xffff);

    s2 += interval_t(0x0000, 0xffff);
    s2 += interval_t(0xa000, 0xbfff);

    BOOST_CHECK_EQUAL(s1, s2);
}

BOOST_AUTO_TEST_CASE(extreme_valued_right_open_intervals)
{
    typedef boost::icl::right_open_interval<boost::uint16_t, std::less> interval_t;
    typedef boost::icl::split_interval_set<boost::uint16_t, std::less, interval_t> set_t;

    set_t s1, s2;

    s1 += interval_t(0x0000, 0xa000);
    s1 += interval_t(0xa000, 0xc000);
    s1 += interval_t(0xc000, 0xffff); //Contains 0xfffe as largest element

    s2 += interval_t(0x0000, 0xffff);
    s2 += interval_t(0xa000, 0xc000);

    BOOST_CHECK_EQUAL(s1, s2);
}

BOOST_AUTO_TEST_CASE(extreme_valued_left_open_intervals)
{
    typedef boost::icl::left_open_interval<boost::uint16_t, std::less> interval_t;
    typedef boost::icl::split_interval_set<boost::uint16_t, std::less, interval_t> set_t;

    set_t s1, s2;

    s1 += interval_t(0x0000, 0x9fff); //Contains 1 as smallest element
    s1 += interval_t(0x9fff, 0xbfff);
    s1 += interval_t(0xbfff, 0xffff);

    s2 += interval_t(0x0000, 0xffff);
    s2 += interval_t(0x9fff, 0xbfff);

    BOOST_CHECK_EQUAL(s1, s2);
}

BOOST_AUTO_TEST_CASE(extreme_valued_open_intervals)
{
    typedef boost::icl::open_interval<boost::uint16_t, std::less> interval_t;
    typedef boost::icl::split_interval_set<boost::uint16_t, std::less, interval_t> set_t;

    set_t s1, s2;

    s1 += interval_t(0x0000, 0xa000); //Contains 1 as smallest element
    s1 += interval_t(0x9fff, 0xc000);
    s1 += interval_t(0xbfff, 0xffff); //Contains 0xfffe as largest element

    s2 += interval_t(0x0000, 0xffff);
    s2 += interval_t(0x9fff, 0xc000);

    BOOST_CHECK_EQUAL(s1, s2);
}

BOOST_AUTO_TEST_CASE(test_span_and_hull)
{
    typedef closed_interval<int> cl_I_int;
    cl_I_int span_1_2 = span< cl_I_int >(1,2);
    cl_I_int span_2_1 = span< cl_I_int >(2,1);
    BOOST_CHECK_EQUAL(span_1_2, span_2_1);
    BOOST_CHECK_EQUAL(hull< cl_I_int >(1,2), hull< cl_I_int >(2,1));

    BOOST_CHECK_EQUAL( span< right_open_interval<int> >(2,1)
                     , construct< right_open_interval<int> >(1,2) );

    BOOST_CHECK_EQUAL( hull< right_open_interval<int> >(2,1)
                     , construct< right_open_interval<int> >(1,3) );
}

BOOST_AUTO_TEST_CASE(test_ticket_11942)
{
    boost::icl::split_interval_set<int> intervals;
    intervals.insert(boost::icl::discrete_interval<int>(1, 2));
    intervals.insert(boost::icl::discrete_interval<int>(2, 3));
    intervals.insert(boost::icl::discrete_interval<int>(0, 3));

    BOOST_CHECK_EQUAL(intervals.size(), 3);
}

BOOST_AUTO_TEST_CASE(test_ticket_12872)
{
    boost::icl::split_interval_set<unsigned> ss
        = boost::icl::split_interval_set<unsigned>();
    std::vector<std::pair<unsigned, unsigned> > rs;
    rs.push_back(make_pair(88, 96));
    rs.push_back(make_pair(72, 96));
    rs.push_back(make_pair(80, 96));
    rs.push_back(make_pair(24, 64));
    rs.push_back(make_pair(80, 96));
    rs.push_back(make_pair(32, 72));
    rs.push_back(make_pair(88, 96));
    rs.push_back(make_pair(16, 56));
	//    {{88, 96}, {72, 96}, {80, 96}, {24, 64}, {80, 96}, {32, 72},
	//     {88, 96}, {16, 56}};

    for(int i=0; i < rs.size(); i++) {
        ss.add( boost::icl::interval<unsigned>::right_open(rs[i].first, rs[i].second) );
    }

    boost::icl::split_interval_map<unsigned, unsigned> im
        = boost::icl::split_interval_map<unsigned, unsigned>();
    for(int i=0; i < rs.size(); i++) {
        im.add( make_pair( boost::icl::interval<unsigned>::right_open(rs[i].first, rs[i].second), 1 ) );
    }

    BOOST_CHECK_EQUAL(interval_count(ss), interval_count(im));
    BOOST_CHECK_EQUAL(icl::size(ss), icl::size(im));
    BOOST_CHECK_EQUAL(hull(ss), hull(im));
}

BOOST_AUTO_TEST_CASE(test_ticket_12926)
{
    boost::icl::interval_set<int> a;
    a.insert(boost::icl::interval<int>::right_open(-100, 100));
    a.insert(-50);

    BOOST_CHECK_EQUAL(a.size(), 200);
}


/*
Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.

NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.
*/
#pragma once

#include <Eigen/Eigen>

#include "engine/core/math/types.hpp"
#include "engine/core/tensor/tensor.hpp"
#include "engine/gems/math/float16.hpp"

namespace isaac {

// Represents a storage container for collection of data samples.  Sample clouds are
// not presumed to have spatial representation by default. They only represent
// some arbitrary data defined on a per sample basis.
//
// For instance a sample cloud could be a list of colors that can be applied to
// another sample cloud representing samples in space or velocity vectors.
//
// Sample clouds have a fixed number of data channels, but have a dynamic number of
// samples specified at runtime.
//
// Channels will be interleaved in memory
template <typename K, size_t Channels, typename BufferType>
class SampleCloudBase {
 public:
  using buffer_t = BufferType;
  static constexpr bool kIsMutable = BufferTraits<buffer_t>::kIsMutable;
  static constexpr bool kIsOwning = BufferTraits<buffer_t>::kIsOwning;

  using data_t = TensorBase<K, 2, buffer_t>;

  using element_t = typename data_t::element_t;
  using element_ptr_t = typename data_t::element_ptr_t;
  using element_const_ptr_t = typename data_t::element_const_ptr_t;

  using buffer_view_t = typename BufferTraits<buffer_t>::buffer_view_t;
  using buffer_const_view_t = typename BufferTraits<buffer_t>::buffer_const_view_t;

  using sample_cloud_view_t = SampleCloudBase<K, Channels, buffer_view_t>;
  using sample_cloud_const_view_t = SampleCloudBase<K, Channels, buffer_const_view_t>;

  using index_t = size_t;

  using eigen_sample_t = Vector<K, Channels>;
  using eigen_const_sample_t = const Vector<K, Channels>;
  using eigen_sample_view_t = Eigen::Map<eigen_sample_t>;
  using eigen_sample_const_view_t = Eigen::Map<eigen_const_sample_t>;
  using eigen_matrix_view_t = Eigen::Map<Matrix<K, Channels, Eigen::Dynamic>>;
  using eigen_matrix_const_view_t = Eigen::Map<const Matrix<K, Channels, Eigen::Dynamic>>;

  SampleCloudBase() {}
  SampleCloudBase(size_t number_samples) : sample_count_(number_samples) { resize(number_samples); }
  SampleCloudBase(buffer_t data, size_t number_samples) : sample_count_(number_samples) {
    data_ = data_t(data, number_samples, Channels);
  }

  // Copy construction uses the default behavior
  SampleCloudBase(const SampleCloudBase& other) = default;
  // Copy assignment uses the default behavior1
  SampleCloudBase& operator=(const SampleCloudBase& other) = default;
  // Move construction uses the default behavior
  SampleCloudBase(SampleCloudBase&& other) = default;
  // Move assignment uses the default behavior
  SampleCloudBase& operator=(SampleCloudBase&& other) = default;

  // Create a view if the data is mutable
  template <bool X = kIsMutable>
  std::enable_if_t<X, sample_cloud_view_t> view() {
    return sample_cloud_view_t({this->data_.data().pointer().get(), this->data_.byte_size()},
                               this->size());
  }

  // create a const view
  sample_cloud_const_view_t const_view() const {
    return sample_cloud_const_view_t({this->data_.data().pointer().get(), this->data_.byte_size()},
                                     this->size());
  }

  // Allow conversion from owning to mutable view
  template <bool X = kIsMutable>
  operator std::enable_if_t<X, sample_cloud_view_t>() { return view();}
  // Allow conversion to const view
  operator sample_cloud_const_view_t() const { return const_view(); }

  // Resize the sample cloud. This action is destructive
  void resize(size_t number_samples) {
    data_.resize(number_samples, Channels);
    sample_count_ = number_samples;
  }

  // Returns the number of samples in the sample cloud
  size_t size() const { return sample_count_; }
  // Returns the number of data channels in the sample cloud
  constexpr size_t channels() const { return Channels; }

  // Return a mutable eigen map as a matrix to the underlying data
  template <bool X = kIsMutable>
  std::enable_if_t<X, eigen_matrix_view_t> eigen_view() {
    return eigen_matrix_view_t(data_.element_wise_begin(), Channels, sample_count_);
  }

  // Return a const eigen map as a matrix to the underlying data
  eigen_matrix_const_view_t eigen_const_view() const {
    return eigen_matrix_const_view_t(data_.element_wise_begin(), Channels, sample_count_);
  }

  // Get an eigen map view of a vector of data.
  template <bool X = kIsMutable>
  std::enable_if_t<X, eigen_sample_view_t> operator[](size_t index) {
    return eigen_sample_view_t(data_.element_wise_begin() + index * Channels, Channels);
  }

  // Get an constant eigen map view of a vector of data.
  eigen_sample_const_view_t operator[](size_t index) const {
    return eigen_sample_const_view_t(data_.element_wise_begin() + index * Channels, Channels);
  }

  // const access to the underlying buffer object
  const data_t& tensor() const { return data_; }

  // const access to the underlying buffer object
  const buffer_t& data() const { return data_.data(); }
  // access to the underlying buffer object
  template <bool X = kIsMutable>
  std::enable_if_t<X, buffer_t&> data() {
    return data_.data();
  }

 private:
  data_t data_;
  size_t sample_count_ = 0;
};

template <typename K, size_t Channels>
using SampleCloud = SampleCloudBase<K, Channels, CpuBuffer>;

template <typename K, size_t Channels>
using SampleCloudView = SampleCloudBase<K, Channels, CpuBufferView>;

template <typename K, size_t Channels>
using SampleCloudConstView = SampleCloudBase<K, Channels, CpuBufferConstView>;

#define ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, T, S) \
  using SampleCloud##N##S = SampleCloud<T, N>;         \
  using SampleCloudView##N##S = SampleCloudView<T, N>; \
  using SampleCloudConstView##N##S = SampleCloudConstView<T, N>;

#define ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(N)                   \
  template <class K>                                          \
  using SampleCloud##N = SampleCloud<K, N>;                   \
  template <class K>                                          \
  using SampleCloudView##N = SampleCloudView<K, N>;           \
  template <class K>                                          \
  using SampleCloudConstView##N = SampleCloudConstView<K, N>; \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, uint8_t, ub)       \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, uint16_t, ui16)    \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, int, i)            \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, double, d)         \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, float, f)          \
  ISAAC_DECLARE_SAMPLE_CLOUD_TYPES_IMPL(N, float16, f16)

ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(1)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(2)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(3)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(4)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(5)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(6)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(7)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(8)
ISAAC_DECLARE_SAMPLE_CLOUD_TYPES(9)

template <typename K, size_t Channels>
using CudaSampleCloud = SampleCloudBase<K, Channels, CudaBuffer>;

template <typename K, size_t Channels>
using CudaSampleCloudView = SampleCloudBase<K, Channels, CudaBufferView>;

template <typename K, size_t Channels>
using CudaSampleCloudConstView = SampleCloudBase<K, Channels, CudaBufferConstView>;

#define ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, T, S)    \
  using CudaSampleCloud##N##S = CudaSampleCloud<T, N>;         \
  using CudaSampleCloudView##N##S = CudaSampleCloudView<T, N>; \
  using CudaSampleCloudConstView##N##S = CudaSampleCloudConstView<T, N>;

#define ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(N)                      \
  template <class K>                                                  \
  using CudaSampleCloud##N = CudaSampleCloud<K, N>;                   \
  template <class K>                                                  \
  using CudaSampleCloudView##N = CudaSampleCloudView<K, N>;           \
  template <class K>                                                  \
  using CudaSampleCloudConstView##N = CudaSampleCloudConstView<K, N>; \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, uint8_t, ub)          \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, uint16_t, ui16)       \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, int, i)               \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, double, d)            \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, float, f)             \
  ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES_IMPL(N, float16, f16)

ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(1)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(2)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(3)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(4)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(5)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(6)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(7)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(8)
ISAAC_DECLARE_CUDA_SAMPLE_CLOUD_TYPES(9)

}  // namespace isaac


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE ScatterTest
#include <poplibs_support/TestDevice.hpp>

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <poplar/Engine.hpp>
#include <popops/Scatter.hpp>
#include <popops/codelets.hpp>
#include <poputil/TileMapping.hpp>
#include <poputil/exceptions.hpp>

using namespace poplar;
using namespace poplar::program;
using namespace poputil;
using namespace popops;
using namespace poplibs_support;

template <typename T, std::size_t N1, std::size_t N2, std::size_t N3>
std::array<T, N1> deviceScatter(
    std::array<T, N1> in, std::vector<std::size_t> in_shape,
    std::array<int, N2> indices, std::vector<std::size_t> indices_shape,
    std::array<T, N3> updates, std::vector<std::size_t> updates_shape,
    std::size_t index_vector_dim, std::vector<unsigned> update_window_dims,
    std::vector<std::size_t> insert_window_dims,
    std::vector<unsigned> scatter_dims_to_operand_dims) {
  auto device = createTestDevice(TEST_TARGET, 1, 4);
  Graph graph(device.getTarget());
  auto seq = Sequence();
  popops::addCodelets(graph);

  Tensor tIn = graph.addVariable(equivalent_device_type<T>().value, in_shape);
  Tensor tIndices =
      graph.addVariable(equivalent_device_type<int>().value, indices_shape);
  Tensor tUpdates =
      graph.addVariable(equivalent_device_type<T>().value, updates_shape);

  poputil::mapTensorLinearly(graph, tIn);
  poputil::mapTensorLinearly(graph, tIndices);
  poputil::mapTensorLinearly(graph, tUpdates);

  BOOST_REQUIRE_EQUAL(tIn.numElements(), N1);
  BOOST_REQUIRE_EQUAL(tIndices.numElements(), N2);
  BOOST_REQUIRE_EQUAL(tUpdates.numElements(), N3);

  scatter(graph, tIn, tIndices, tUpdates, index_vector_dim, update_window_dims,
          insert_window_dims, scatter_dims_to_operand_dims, seq);

  graph.createHostWrite("in", tIn);
  graph.createHostWrite("indices", tIndices);
  graph.createHostWrite("update", tUpdates);
  graph.createHostRead("out", tIn);

  std::array<T, N1> out;
  Engine eng(graph, seq);
  device.bind([&](const Device &d) {
    eng.load(d);
    eng.writeTensor("in", in.data(), in.data() + in.size());
    eng.writeTensor("indices", indices.data(), indices.data() + indices.size());
    eng.writeTensor("update", updates.data(), updates.data() + updates.size());
    eng.run();

    eng.readTensor("out", out.data(), out.data() + out.size());
  });

  return out;
}

BOOST_AUTO_TEST_CASE(ScatterTestCase0) {
  std::array<int, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 2> indices = {0, 2};
  std::array<int, 6> updates = {10, 20, 30, 70, 80, 90};
  std::array<int, 9> result = {10, 20, 30, 4, 5, 6, 70, 80, 90};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2}, updates, {2, 3}, 1,
                           {1}, {0}, {0}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase1) {
  std::array<int, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 2> indices = {0, 1};
  std::array<int, 6> updates = {10, 20, 30, 40, 50, 60};
  std::array<int, 9> result = {10, 20, 30, 40, 50, 60, 7, 8, 9};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2}, updates, {2, 3}, 1,
                           {1}, {0}, {0}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase2) {
  std::array<int, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 2> indices = {0, 2};
  std::array<int, 6> updates = {{10, 30, 40, 60, 70, 90}};
  std::array<int, 9> result = {10, 2, 30, 40, 5, 60, 70, 8, 90};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2}, updates, {3, 2}, 1,
                           {0}, {1}, {1}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase3) {
  std::array<float, 4> operand = {0, 0, 0, 0};
  std::array<int, 3> indices = {0, 0, 0};
  std::array<float, 4> updates = {0.12, 0.28, 0.018, 0.42};
  std::array<float, 4> result = {0.12, 0.28, 0.018, 0.42};

  BOOST_CHECK(deviceScatter(operand, {1, 2, 2, 1}, indices, {1, 3}, updates,
                            {1, 2, 2, 1}, 1, {1, 2, 3}, {0},
                            {0, 2, 1}) == result);
}

BOOST_AUTO_TEST_CASE(ScatterTestCase4) {
  std::array<int, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 2> indices = {0, 2};
  std::array<int, 6> updates = {10, 20, 30, 70, 80, 90};
  std::array<int, 9> result = {10, 20, 30, 4, 5, 6, 70, 80, 90};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2}, updates, {2, 3}, 1,
                           {1}, {0}, {0}) == result);
}

BOOST_AUTO_TEST_CASE(ScatterTestCase5) {
  std::array<float, 18> operand = {-1, 1,  -2, 2,  -3, 3,  -4, 4,  -5,
                                   5,  -6, 6,  -7, 7,  -8, 8,  -9, 9};
  std::array<int, 4> indices = {0, 0, 1, 0};
  std::array<float, 4> updates = {-10, 10, -40, 40};
  std::array<float, 18> result = {-10, 10, -2, 2,  -3, 3,  -40, 40, -5,
                                  5,   -6, 6,  -7, 7,  -8, 8,   -9, 9};

  BOOST_TEST(deviceScatter(operand, {3, 3, 2}, indices, {2, 2}, updates, {2, 2},
                           1, {1}, {0, 1}, {0, 1}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase6) {
  std::array<float, 18> operand = {-1, 1,  -2, 2,  -3, 3,  -4, 4,  -5,
                                   5,  -6, 6,  -7, 7,  -8, 8,  -9, 9};
  std::array<int, 4> indices = {0, 0, 0, 1};
  std::array<float, 4> updates = {-10, 10, -20, 20};
  std::array<float, 18> result = {-10, 10, -20, 20, -3, 3,  -4, 4,  -5,
                                  5,   -6, 6,   -7, 7,  -8, 8,  -9, 9};

  BOOST_TEST(deviceScatter(operand, {3, 3, 2}, indices, {2, 2}, updates, {2, 2},
                           0, {1}, {0, 1}, {0, 1}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase7) {
  std::array<float, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 2> indices = {1, 1};
  std::array<float, 1> updates = {10};
  std::array<float, 9> result = {1, 2, 3, 4, 10, 6, 7, 8, 9};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2}, updates, {1, 1}, 0,
                           {0, 1}, {}, {0, 1}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase8) {
  std::array<float, 9> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9};
  std::array<int, 4> indices = {2, 1, 1, 1};
  std::array<float, 2> updates = {10, 20};
  std::array<float, 9> result = {1, 2, 3, 4, 20, 6, 7, 10, 9};

  BOOST_TEST(deviceScatter(operand, {3, 3}, indices, {2, 2}, updates, {2, 1, 1},
                           0, {1, 2}, {}, {0, 1}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase9) {
  std::array<float, 0> operand = {};
  std::array<int, 2> indices = {0, 2};
  std::array<float, 0> updates = {};
  std::array<float, 0> result = {};

  BOOST_TEST(deviceScatter(operand, {3, 0}, indices, {2}, updates, {2, 0}, 1,
                           {1}, {0}, {0}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase10) {
  std::array<float, 12> operand = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12};
  std::array<int, 1> indices = {1};
  std::array<float, 6> updates = {10, 20, 30, 40, 50, 60};
  std::array<float, 12> result = {1, 2, 3, 4, 5, 6, 10, 20, 30, 40, 50, 60};

  BOOST_TEST(deviceScatter(operand, {2, 3, 2}, indices, {}, updates, {1, 3, 2},
                           0, {0, 1, 2}, {}, {0}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase11) {
  std::array<float, 4> operand = {1, 2, 3, 4};
  std::array<int, 1> indices = {1};
  std::array<float, 1> updates = {25};
  std::array<float, 4> result = {1, 25, 3, 4};

  BOOST_TEST(deviceScatter(operand, {4}, indices, {}, updates, {}, 0, {}, {0},
                           {0}) == result,
             boost::test_tools::per_element());
}

BOOST_AUTO_TEST_CASE(ScatterTestCase12) {
  std::array<float, 3> operand = {1, 2, 3};
  std::array<int, 0> indices = {};
  std::array<float, 0> updates = {};
  std::array<float, 3> result = {1, 2, 3};

  BOOST_TEST(deviceScatter(operand, {3}, indices, {0}, updates, {0}, 1, {}, {0},
                           {0}) == result,
             boost::test_tools::per_element());
}


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseCentimeterUnit.cpp
//! \author Alex Robinson
//! \brief  The inverse centimeter unit
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/length.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_InverseCentimeterUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse centimeter unit can be initialized
TEUCHOS_UNIT_TEST( InverseCentimeterUnit, initialize )
{
  quantity<InverseCentimeter> inverse_cm( 1.0*inverse_centimeter );

  TEST_EQUALITY_CONST( inverse_cm.value(), 1.0 );

  inverse_cm = 2.0*inverse_centimeter;

  TEST_EQUALITY_CONST( inverse_cm.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inverse centimeter unit can be initialize from cgs, si units
TEUCHOS_UNIT_TEST( InverseCentimeterUnit, initialize_from_cgs_si )
{
  quantity<InverseCentimeter> inverse_cm_cgs( 1.0/cgs::centimeter );

  quantity<InverseCentimeter> inverse_cm_si( 1.0/si::meter );

  TEST_EQUALITY_CONST( inverse_cm_cgs.value(), 1.0 );
  TEST_EQUALITY_CONST( inverse_cm_si.value(), 0.01 );
}

//---------------------------------------------------------------------------//
// end tstInverseCentimeterUnit.cpp
//---------------------------------------------------------------------------//


#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

#include "HCStream.cpp"

/*
 * NOT HANDLED
 * cluster 1 splits into cluster 2,3 in the middle of the window. At window end, all points should have the same label;
 *
 * test number of active/passive clusters
 * test on real datasets
 */

BOOST_AUTO_TEST_CASE( HCStream_ParseEntry ) {
	HCStream* hcStream = new HCStream(10, 0.2, 2, 0.01, 0.01);

	std::vector<std::vector<double>> points = { { 0.1, 0.1 }, { 0.1, 0.3 }, {
			0.1, 0.1 }, { 0.7, 0.7 }, { 0.7, 0.8 }, { 0.7, 0.7 } };
	for (size_t pos = 0; pos < points.size(); pos++) {
		hcStream->parseEntry(pos, points[pos]);
	}

	std::vector<int> actualLabels;
	for (auto &point : points) {
		actualLabels.push_back(hcStream->getLabel(point));
	}

	std::vector<int> trueLabels = { 0, 0, 0, 1, 1, 1 };

	BOOST_CHECK_EQUAL_COLLECTIONS(trueLabels.begin(), trueLabels.end(),
			actualLabels.begin(), actualLabels.end());
}

BOOST_AUTO_TEST_CASE( HCStream_MergeActiveClusters ) {
	int wSize = 8;
	float gridSize = 0.2;
	int densityThld = 2;
	HCStream* hcStream = new HCStream(wSize, gridSize, densityThld, 0.01, 0.01);

	std::vector<std::vector<double>> winPoints;
	std::vector<int> actualLabels;
	std::vector<int> trueLabels;

	// 3 clusters centers: { 0.6, 0.6 }, { 0.2, 0.6 }, { 0.6, 0.8 },
	winPoints = {
			{ 0.6, 0.6 },
			{ 0.2, 0.6 },
			{ 0.6, 0.8 },
			{ 0.6, 0.6 },
			{ 0.2, 0.6 },
			{ 0.6, 0.8 },
			{ 0.6, 0.6 },
			{ 0.6, 0.6 }
	};
	for (size_t pos = 0; pos < winPoints.size(); pos++) {
		hcStream->parseEntry(0 * wSize + pos, winPoints[pos]);
	}

	actualLabels.clear();
	for (auto &point : winPoints) {
		actualLabels.push_back(hcStream->getLabel(point));
	}
	trueLabels.clear();
	trueLabels = { 0, 0, 0, 0, 0, 0, 0, 0 };
	BOOST_CHECK_EQUAL_COLLECTIONS(trueLabels.begin(), trueLabels.end(),	actualLabels.begin(), actualLabels.end());
}

BOOST_AUTO_TEST_CASE( HCStream_SplitPassiveCluster ) {
	int wSize = 8;
	float gridSize = 0.2;
	int densityThld = 2;
	HCStream* hcStream = new HCStream(wSize, gridSize, densityThld, 0.01, 0.01);

	std::vector<std::vector<double>> winPoints;
	std::vector<int> actualLabels;
	std::vector<int> trueLabels;

	std::cout << "split cluster: " << std::endl;
	std::cout << std::endl;
	std::cout << std::endl;

	/*
	1st window: 1 parent cluster { 0.6, 0.6 } is split in two child clusters: { 0.2, 0.6 }, { 0.6, 0.8 }
	{ 0.2, 0.6 } is viewed as evolving from { 0.6, 0.6 }
	there are 3 overlapping clusters: 1 passive, 2 active => one label
	*/
	std::cout << "1st window: " << std::endl;
	winPoints = {
			{ 0.6, 0.6 },
			{ 0.6, 0.6 },
			{ 0.6, 0.6 },
			{ 0.6, 0.6 },
			{ 0.2, 0.6 },
			{ 0.2, 0.6 },
			{ 0.6, 0.8 },
			{ 0.6, 0.8 }
	};
	for (size_t pos = 0; pos < winPoints.size(); pos++) {
		hcStream->parseEntry(0 * wSize + pos, winPoints[pos]);
	}

	actualLabels.clear();
	for (auto &point : winPoints) {
		actualLabels.push_back(hcStream->getLabel(point));
	}
	trueLabels.clear();
	trueLabels = { 0, 0, 0, 0, 0, 0, 0, 0 };
	BOOST_CHECK_EQUAL_COLLECTIONS(trueLabels.begin(), trueLabels.end(),	actualLabels.begin(), actualLabels.end());

	/*
	2nd window: only the child clusters remaing, there are 2 labels
	*/
	std::cout << "2nd window: " << std::endl;
	winPoints = {
			{ 0.2, 0.6 },
			{ 0.2, 0.6 },
			{ 0.6, 0.8 },
			{ 0.6, 0.8 },
			{ 0.2, 0.6 },
			{ 0.2, 0.6 },
			{ 0.6, 0.8 },
			{ 0.6, 0.8 }
	};
	for (size_t pos = 0; pos < winPoints.size(); pos++) {
		hcStream->parseEntry(1 * wSize + pos, winPoints[pos]);
	}

	actualLabels.clear();
	for (auto &point : winPoints) {
		actualLabels.push_back(hcStream->getLabel(point));
	}
	trueLabels = { 0, 0, 1, 1, 0, 0, 1, 1 };
	BOOST_CHECK_EQUAL_COLLECTIONS(trueLabels.begin(), trueLabels.end(),	actualLabels.begin(), actualLabels.end());
}

BOOST_AUTO_TEST_CASE( HCStream_MergePassiveCluster ) {
	int wSize = 8;
	float gridSize = 0.2;
	int densityThld = 2;
	HCStream* hcStream = new HCStream(wSize, gridSize, densityThld, 0.01, 0.01);

	std::vector<std::vector<double>> winPoints;
	std::vector<int> actualLabels;
	std::vector<int> trueLabels;

	std::cout << "split cluster: " << std::endl;
	std::cout << std::endl;
	std::cout << std::endl;

	/*
	1st window: 2 child clusters: { 0.2, 0.6 }, { 0.6, 0.8 } evolve into a single cluster { 0.6, 0.6 },
	there are 3 overlapping clusters: 2 passive, 1 active => one label
	*/
	std::cout << "1st window: " << std::endl;
	winPoints = {
				{ 0.2, 0.6 },
				{ 0.2, 0.6 },
				{ 0.6, 0.8 },
				{ 0.6, 0.8 },
				{ 0.6, 0.6 },
				{ 0.6, 0.6 },
				{ 0.6, 0.6 },
				{ 0.6, 0.6 },
		};
	for (size_t pos = 0; pos < winPoints.size(); pos++) {
		hcStream->parseEntry(0 * wSize + pos, winPoints[pos]);
	}

	actualLabels.clear();
	for (auto &point : winPoints) {
		actualLabels.push_back(hcStream->getLabel(point));
	}
	trueLabels.clear();
	trueLabels = { 0, 0, 0, 0, 0, 0, 0, 0 };
	BOOST_CHECK_EQUAL_COLLECTIONS(trueLabels.begin(), trueLabels.end(),	actualLabels.begin(), actualLabels.end());
}


#pragma once

#include <boost/dynamic_bitset.hpp>

#include "../Variable.hpp"

#include "Counter.hpp"

namespace mist {
namespace it {

using Bitset = boost::dynamic_bitset<unsigned long long>;
using BitsetVariable = std::vector<Bitset>;
using BitsetTable = std::vector<BitsetVariable>;

/** Generates a ProbabilityDistribution from a Variable tuple.
 *
 * Recasts each Variable as an array of bitsets, one for each bin value.
 * Computes the ProbabilityDistribution using bitwise AND operation and bit
 * counting algorithm.
 */
class BitsetCounter : public Counter
{
public:
  BitsetCounter(Variable::tuple const& all_vars);
  ~BitsetCounter(){};
  void count(Variable const&, Distribution&);
  void count(Variable::tuple const&, Distribution&);
  void count(Variable::tuple const&, Variable::indexes const&, Distribution&);

private:
  BitsetTable bits;
};

class BitsetCounterOutOfRange : public std::out_of_range
{
public:
  BitsetCounterOutOfRange(std::string const& method, int index, int max)
    : out_of_range("BitsetCounter::" + method + " : Variable index " +
                   std::to_string(index) +
                   " out of bitset table range, valid range [0," +
                   std::to_string(max) + "]"){};
};

} // it
} // mist


/**
    @author J. Reher (jreher@caltech.edu)
*/

#ifndef WALKING_QP_HPP
#define WALKING_QP_HPP

#include <cassie_description/cassie_model.hpp>
#include <ros_utilities/ros_utilities.hpp>
#include <cassie_common_toolbox/PhaseVariable.hpp>
#include <cassie_common_toolbox/smoothing.hpp>
#include <std_srvs/Empty.h>
#include <ros_utilities/timing.hpp>
#include <cassie_controllers/feedforward_controller.hpp>
#include <qpOASES.hpp>
#include <control_utilities/filters.hpp>
#include <control_utilities/limits.hpp>
#include <cassie_estimation/rigidtarsus_solver.hpp>
#include <Eigen/Sparse>

USING_NAMESPACE_QPOASES

class Walking1DControl {
public:

    Walking1DControl(ros::NodeHandle &nh, cassie_model::Cassie &robot);
    void update(VectorXd &radio, VectorXd &u);
    void reset();
    bool reconfigure();
    bool isReadyToTransition() {return this->memory.readyToStop;}
    void getDebug(VectorXf &dbg);

private:

    /**
    * @brief Computations that can be reproduced given a Config and Memory object
    * Meant to only be valid during one computation frame. Anything meant to persist across time should be stored in Memory.
    */
    struct Cache {
        VectorXd ya;
        VectorXd dya;
        MatrixXd Dya;
        MatrixXd DLfya;
        VectorXd yd;
        VectorXd dyd;
        VectorXd d2yd;
        VectorXd ddqd;
        VectorXd pd;
        VectorXd vd;
        VectorXd Fd;
        VectorXd raibert_offset;
        VectorXd uff;
        VectorXd ddqtar;

        VectorXd qmd;
        VectorXd dqmd;

        // QP cache allocation
        MatrixXd Jc;
        MatrixXd dJc;
        MatrixXd Js;
        MatrixXd dJs;
        MatrixXd Jcs;
        MatrixXd dJcs;
        MatrixXd A_holonomics;
        VectorXd b_holonomics;
        MatrixXd Aeq_dynamics;
        VectorXd beq_dynamics;
        MatrixXd A_friction;
        VectorXd b_lb_friction, b_ub_friction;
        MatrixXd A_y;
        VectorXd b_y;
        MatrixXd A_clf;
        MatrixXd A_reg;
        VectorXd b_reg;
        MatrixXd Proj;
        VectorXd qpsol;
        double V;
        VectorXd Fdes;
        double delta;

        Eigen::Matrix<double,Dynamic,Dynamic,RowMajor> Aconstr;
        VectorXd lbA;
        VectorXd ubA;

        Eigen::Matrix<double,Dynamic,Dynamic,RowMajor> G;
        VectorXd g;
        Eigen::SparseMatrix<double> Gsparse;

        Eigen::EulerAnglesZYXd euler;
        Eigen::Transform<double, 3, Affine> T_des_pelvis;

        VectorXd u;

        void init();
        void reset();
    } cache;

    /**
    * @brief Data meant to persist across data frames
    */
    struct Memory {
        // Timer
        ros_utilities::Timer timer = ros_utilities::Timer(true);
        int iDomain;
        bool isInitialized;
        bool readyToStop;
        bool queueStop;
        Vector2d lastContact;
        VectorXd yd_last;
        VectorXd dyd_last;
        VectorXd raibert_offset_last;
        double   leg_angle_offset;
        double   leg_roll_offset;
        VectorXd u_prev;
        VectorXd vd_integral_error;

        MatrixXd paramCurrent;

        Vector2d raibert_deltaX;
        Vector2d raibert_deltaY;
        Vector2d lastStepVelocity;
        Vector2d velocityAllocator;
        int nVelocitySamplesThisStep;
        double lastTau;
        double s_feet; // (left) -1 <--> 1 (right)

        bool qp_initialized;

        void init();
        void reset();
    } memory;

    /**
    * @brief Persistent controller configurations
    */
    struct Config {
        bool isSim;
        bool use_qp;
        int nDomain;
        double time_scale;
        double vx_offset;
        double vy_offset;
        double leg_angle_offset_center;
        double u_stancetoe_offset;
        double swing_foot_offset;
        double stoppable_velocity_threshold;
        bool use_contact_switching;
        VectorXd Kp;
        VectorXd Kd;
        VectorXd Kpy;
        VectorXd Kdy;
        bool swing_angle_absolute;
        double raibert_KpX;
        double raibert_KpY;
        double raibert_KdX;
        double raibert_KdY;
        double raibert_KiX;
        double raibert_KiY;
        double ddq_KpX;
        double ddq_KdX;
        double ddq_KiX;
        double ddq_KpY;
        double ddq_KdY;
        double ddq_KiY;
        VectorXd torque_bounds;
        double left_stance_gravity_scale;
        double right_stance_gravity_scale;
        VectorXd Pdiag;
        VectorXd Poffdiag;
        double res_clf_ep;
        double clf_gam;
        double velocity_integrator_bleed_constant;

        double vdx_ub;
        double vdx_lb;
        double vdy_ub;
        double vdy_lb;

        double Kp_grf_leglength;
        double Kd_grf_leglength;
        double k1_grf;
        double k2_grf;
        double k3_grf;
        double k4_grf;
        double k_grf_offset;

        double force_z_scale;
        double force_sp_scale;

        bool clf_use_task_pd;
        bool clf_use_inequality;
        bool clf_use_Vdot_cost;

        // QP Terms
        int nQPIter;
        int ny;
        double ep;
        double gam;
        MatrixXd P;
        MatrixXd F;
        MatrixXd G;
        MatrixXd LFV_mat;
        MatrixXd Be;
        MatrixXd contact_pyramid;
        VectorXd lb;
        VectorXd ub;

        // Large matrix static configurations
        MatrixXd Su;
        MatrixXd Sc;

        // QP Tuning
        double reg_ddq;
        VectorXd reg_base_ddq;
        VectorXd reg_stance_ddq;
        VectorXd reg_swing_ddq;
        VectorXd reg_stance_u;
        VectorXd reg_swing_u;
        double reg_rigid;
        double reg_fx;
        double reg_fy;
        double reg_fz;
        double reg_muy;
        double reg_muz;
        double reg_clf_delta;

        double w_outputs;
        double w_hol_fixed;
        double w_hol_fx;
        double w_hol_fy;
        double w_hol_fz;
        double w_hol_my;
        double w_hol_mz;
        double w_Vdot;

        struct GaitLibrary {
            struct StanceParameters{
                std::vector< std::vector<VectorXd> > amat_array;
                std::vector< std::vector<VectorXd> > apos_base_array;
                std::vector< std::vector<VectorXd> > avelocity_array;
                std::vector< std::vector<VectorXd> > addq_array;
                std::vector< std::vector<VectorXd> > af_array;
                std::vector< std::vector<VectorXd> > au_array;

                void resize_vectors(unsigned long sz) {
                    amat_array.resize(sz);
                    apos_base_array.resize(sz);
                    avelocity_array.resize(sz);
                    addq_array.resize(sz);
                    af_array.resize(sz);
                    au_array.resize(sz);
                }
            };
            StanceParameters left;
            StanceParameters right;
            int nDomain;
            Vector2d phaseRange;
            VectorXd vd_x;
            VectorXd vd_y;

        } gaitlib;

        // Parameter checker
        ros_utilities::ParamChecker paramChecker;

        // Methods
        void init();
        void reconfigure();

    } config;

    // Pointer to the controlling nodehandle and related ROS things
    ros::NodeHandle *nh;
    ros::ServiceServer reconfigureService;

    control_utilities::LowPassFilter lpVdX    = control_utilities::LowPassFilter(0.001, 0.75);
    control_utilities::LowPassFilter lpVdY    = control_utilities::LowPassFilter(0.001, 0.25);
    control_utilities::LowPassFilter lpVdTurn = control_utilities::LowPassFilter(0.001, 0.25);

    control_utilities::LowPassFilter lpVaX = control_utilities::LowPassFilter(0.001, 0.0001);
    control_utilities::LowPassFilter lpVaY = control_utilities::LowPassFilter(0.001, 0.0001);

    control_utilities::LowPassFilter lpVaXlastStep = control_utilities::LowPassFilter(0.001, 0.0001);
    control_utilities::LowPassFilter lpVaYlastStep = control_utilities::LowPassFilter(0.001, 0.0001);

    control_utilities::RateLimiter rateRaibertX    = control_utilities::RateLimiter(-75, 75); // (-0.2, 0.2);
    control_utilities::RateLimiter rateRaibertY    = control_utilities::RateLimiter(-75, 75); // (-0.2, 0.2);

    control_utilities::RateLimiter rateStepX    = control_utilities::RateLimiter(-0.15, 0.15);
    control_utilities::RateLimiter rateStepY    = control_utilities::RateLimiter(-0.10, 0.10);

    control_utilities::RateLimiter rateJoyX    = control_utilities::RateLimiter(-0.30, 0.30);
    control_utilities::RateLimiter rateJoyY    = control_utilities::RateLimiter(-0.30, 0.30);
    control_utilities::RateLimiter rateJoyTurn = control_utilities::RateLimiter(-0.15, 0.15);

    cassie_common_toolbox::MovingAverage stepPeriodAverager = cassie_common_toolbox::MovingAverage(400, 2); // Moving average for one step period (T=0.4s)

    // QP Solver for varying matrices
    SQProblem *qpsolver;

    // Reconfigure callback for sub-node
    bool reconfigure(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

    // Pointer to robot model
    cassie_model::Cassie *robot;

    // Feedforward controller
    feedforward_underactuated ff;

    // Gait phasing variable
    PhaseVariable phase;

    // Swing leg tarsus ik solver
    RigidTarsusSolver tarsusSolver;

    // Private methods
    void nextDomain();
    void computeGuard(bool requestTransition);
    void computeActual(VectorXd &ya, VectorXd &dya, MatrixXd &Dya, MatrixXd &DLfya);
    void computeDesired(VectorXd &yd, VectorXd &dyd, VectorXd &d2yd);
    VectorXd getTorqueQP();
    VectorXd getTorqueID();
    void updateTurning(double yawUpdate);
    void updateRaibert(double xVd, double yVd);
    void updateAccelerations(double xVd, double yVd);
    void updateForces(double xVd, double yVd);
    void computeLibrary();

    int SwingLegInverseKinematics(VectorXd &qmd, VectorXd &dqmd, VectorXd &ddqmd);
};

#endif // WALKING_QP_HPP