/*
@copyright Louis Dionne 2014
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/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(
        sequence<Maybe>(list(just(1), just('2'), just(3.3))) == just(list(1, '2', 3.3))
    );

    BOOST_HANA_CONSTANT_ASSERT(
        sequence<Maybe>(list(just(1), nothing, just(3.3))) == nothing
    );

    // This is a generalized Cartesian product.
    BOOST_HANA_CONSTEXPR_ASSERT(
        sequence<List>(list(list(1, 2, 3), list(4), list(5, 6)))
        ==
        list(
            list(1, 4, 5), list(1, 4, 6),
            list(2, 4, 5), list(2, 4, 6),
            list(3, 4, 5), list(3, 4, 6)
        )
    );
    //! [main]
}


#include <boost/random/negative_binomial_distribution.hpp>


#include <Eigen/Dense>

#include <ros/ros.h>
#include <vector>
#include <ros/node_handle.h>
#include <ros/package.h>
#include "sensor_msgs/JointState.h"

#include "panda_simulation/NE_matrix.hpp"

using namespace std;
using namespace Eigen;

typedef Matrix<double, 7, 1> V7d;
typedef Matrix<double, 7, 7> M7d;

class Impedance{
  private:
    int argc;
    char **argv;

  public:
    Impedance(int argc, char ** argv);
    void startControl();
    Model *model;
    void impedanceControl();
    vector<ros::Publisher> publisher_vec;
    ros::Subscriber robot_state_sub;
    V7d q, dq;
    void JointStateCallback(const sensor_msgs::JointState& msg);
};


#include "SimilarityImageGrouper.h"

#include <boost/filesystem.hpp>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cgcolle::encoder;

static cv::Mat calcHist(const cv::Mat &image)
{
    cv::Mat hsvImage;
    cvtColor(image, hsvImage, cv::COLOR_BGR2HSV);

    int hBins = 50;
    int sBins = 60;
    int histSize[] = { hBins, sBins };
    float hRanges[] = { 0, 180 };
    float sRanges[] = { 0, 256 };
    const float *ranges[] = { hRanges, sRanges };
    int channels[] = { 0, 1 };

    cv::Mat hist;
    cv::calcHist(&hsvImage, 1, channels, cv::Mat(), hist, 2, histSize,
                 ranges, true, false);
    cv::normalize(hist, hist, 0, 1, cv::NORM_MINMAX, -1, cv::Mat());

    return hist;
}

std::vector<ImageGroup*> SimilarityImageGrouper::group(const std::vector<std::string>& imagePaths)
{
    typedef std::pair<cv::Mat, cv::Size> HistPair;

    // Calc histograms
    std::vector<HistPair> histograms(imagePaths.size());
    std::transform(imagePaths.begin(), imagePaths.end(), histograms.begin(), [](const std::string &imagePath) -> HistPair {
        cv::Mat image = cv::imread(imagePath);
        cv::Mat hist = calcHist(image);

        return HistPair(hist, image.size());
    });

    // Partition sizes
    std::vector<int> sizeLabels(histograms.size());
    int sizeLabelsCnt = cv::partition(histograms, sizeLabels, [](const HistPair &left, const HistPair &right) {
        const cv::Size &lsize = left.second;
        const cv::Size &rsize = right.second;

        return lsize.width == rsize.width && lsize.height == rsize.height;
    });

    // Paritition histograms
    std::vector<int> histLabels(imagePaths.size());
    int histLabelsCnt = cv::partition(histograms, histLabels, [this](const HistPair &left, const HistPair &right) -> bool {
        return cv::compareHist(left.first, right.first, 0) >= 0.95;
    });

    typedef std::tuple<int, int, int> ImageInfo; // idx, size, hist
    std::vector<std::tuple<int, int, int> > imageInfos;
    for (int i = 0; i < imagePaths.size(); ++i) {
        imageInfos.push_back(std::make_tuple(i, sizeLabels[i], histLabels[i]));
    }

    // Sort by labels
    std::sort(imageInfos.begin(), imageInfos.end(), [](const ImageInfo &l, const ImageInfo &r) -> bool {
        // Compare size
        if (std::get<1>(l) != std::get<1>(r)) {
            return std::get<1>(l) > std::get<1>(r);
        }

        // Compare hist
        return std::get<2>(l) > std::get<2>(r);
    });

    // Build groups
    std::vector<ImageGroup*> groups;
    int curSizeLabel = 0;
    int curHistLabel = 0;
    ImageGroup *group = nullptr;
    for (const ImageInfo &imageInfo : imageInfos) {
        int index, sizeLabel, histLabel;
        std::tie(index, sizeLabel, histLabel) = imageInfo;

        const boost::filesystem::path imagePath(imagePaths[index]);
        const std::string fileName = imagePath.filename().string();
        cgcolle::ImageEntry *entry = new cgcolle::ImageEntry(fileName, imagePath.string());

        bool isGroupChanged = sizeLabel != curSizeLabel || histLabel != curHistLabel;
        if (group == nullptr || isGroupChanged) {
            // Main frame
            group = new ImageGroup();
            groups.push_back(group);

            group->mainFrame = entry;
            curSizeLabel = sizeLabel;
            curHistLabel = histLabel;
        } else {
            // Sub frame
            group->subFrames.push_back(entry);
        }
    }

    return groups;
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 *
 *    Notes
 *      This unit test does not include testing with the function
 *      testFunctionWithLargeRootDifferences, because this test function does not provide a second
 *      derivative.
 *
 */

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/RootFinders/halleyRootFinder.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction1.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction2.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunction3.h"
#include "Tudat/Mathematics/RootFinders/UnitTests/testFunctionWithZeroRoot.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( testsuite_rootfinders )

using namespace tudat;
using namespace root_finders;
using namespace root_finders::termination_conditions;

//! Check if Halley method converges on test function #1 (TestFunction1).
BOOST_AUTO_TEST_CASE( test_halleyRootFinder_testFunction1 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction1 > testFunction = std::make_shared< TestFunction1 >( 2 );

    // The termination condition.
    HalleyRootFinder::TerminationFunction terminationConditionFunction =
            std::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Halley object.
    HalleyRootFinder halleyRootFinder( terminationConditionFunction );

    // Let the Halley method search for the root.
    const double root = halleyRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Halley method converges on test function #2 (TestFunction2).
BOOST_AUTO_TEST_CASE( test_halleyRootFinder_testFunction2 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction2 > testFunction = std::make_shared< TestFunction2 >( 2 );

    // The termination condition.
    HalleyRootFinder::TerminationFunction terminationConditionFunction =
            std::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Halley object.
    HalleyRootFinder halleyRootFinder( terminationConditionFunction );

    // Let the Halley method search for the root.
    const double root = halleyRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Halley method converges on test function #3 (TestFunction3).
BOOST_AUTO_TEST_CASE( test_halleyRootFinder_testFunction3 )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunction3 > testFunction = std::make_shared< TestFunction3 >( 2 );

    // The termination condition.
    HalleyRootFinder::TerminationFunction terminationConditionFunction =
            std::bind( &RootAbsoluteToleranceTerminationCondition< double >::checkTerminationCondition,
                         std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                             testFunction->getTrueRootAccuracy( ) ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Halley object.
    HalleyRootFinder halleyRootFinder( terminationConditionFunction );

    // Let the Halley method search for the root.
    const double root = halleyRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_CLOSE_FRACTION( root, testFunction->getTrueRootLocation( ), 1.0e-15 );
    BOOST_CHECK_LT( testFunction->evaluate( root ), testFunction->getTrueRootAccuracy( ) );
}

//! Check if Halley method converges on function with zero root (testFunctionWithZeroRoot).
// Not the best test case. Inheritance from old code. Not really relevant anymore. The basic
// idea is that Halley's method should work for both a function that becomes zero, as well as for a
// function that does not become zero. A better case should be written.
BOOST_AUTO_TEST_CASE( test_halleyRootFinder_testFunctionWithZeroRoot )
{
    // Create object containing the test functions.
    std::shared_ptr< TestFunctionWithZeroRoot > testFunction =
            std::make_shared< TestFunctionWithZeroRoot >( 2 );

    // The termination condition.
    HalleyRootFinder::TerminationFunction terminationConditionFunction
            = std::bind( &RootAbsoluteToleranceTerminationCondition< double >::
                           checkTerminationCondition,
                           std::make_shared< RootAbsoluteToleranceTerminationCondition< double > >(
                               1.0e-150 ), std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5 );

    // Test Halley object.
    HalleyRootFinder halleyRootFinder( terminationConditionFunction );

    // Let the Halley method search for the root.
    const double root = halleyRootFinder.execute( testFunction, testFunction->getInitialGuess( ) );

    // Check if the result is within the requested accuracy.
    BOOST_CHECK_SMALL( root, 1.0e-100 );
    BOOST_CHECK_SMALL( testFunction->evaluate( root ), 1.0e-200 );
}

BOOST_AUTO_TEST_SUITE_END( ) // testsuite_rootfinders

} // namespace unit_tests
} // namespace tudat


#pragma once

#include <stdio.h>
#include <Eigen/Dense>
#include <dart/dart.hpp>
#include <dart/utils/urdf/urdf.hpp>
#include <dart/utils/utils.hpp>

#include <Utils/IO/IOUtilities.hpp>

class RobotSystem {
 protected:
  dart::dynamics::SkeletonPtr skel_ptr_;
  int num_dof_;
  int num_virtual_dof_;
  int num_actuated_dof_;
  Eigen::MatrixXd I_cent_;
  Eigen::MatrixXd J_cent_;
  Eigen::MatrixXd A_cent_;

  /*
   * Update I_cent_, A_cent_, J_cent_
   * , where
   * centroid_momentum = I_cent_ * centroid_velocity = A_cent_ * qdot
   *           J_cent_ = inv(I_cent_) * A_cent_
   * centroid_velocity = J_cent_ * qdot
   */
  void _updateCentroidFrame(const Eigen::VectorXd& q_,
                            const Eigen::VectorXd& qdot_);

 public:
  RobotSystem(int numVirtual_, std::string file);
  virtual ~RobotSystem(void);

  dart::dynamics::SkeletonPtr getSkeleton() { return skel_ptr_; };
  dart::dynamics::BodyNodePtr getBodyNode(const std::string& _link_name) {
    return skel_ptr_->getBodyNode(_link_name);
  }
  dart::dynamics::BodyNodePtr getBodyNode(const int& _bn_idx) {
    return skel_ptr_->getBodyNode(_bn_idx);
  }

  Eigen::VectorXd getQ() { return skel_ptr_->getPositions(); };
  Eigen::VectorXd getQdot() { return skel_ptr_->getVelocities(); };
  void printRobotInfo();
  double getRobotMass() { return skel_ptr_->getMass(); }
  int getNumDofs() { return num_dof_; };
  int getNumVirtualDofs() { return num_virtual_dof_; };
  int getNumActuatedDofs() { return num_actuated_dof_; };

  int getJointIdx(const std::string& jointName_);
  int getDofIdx(const std::string& dofName_);

  // Position Limits
  Eigen::VectorXd getPositionLowerLimits() {
    return skel_ptr_->getPositionLowerLimits();
  }
  Eigen::VectorXd getPositionUpperLimits() {
    return skel_ptr_->getPositionUpperLimits();
  }
  // Velocity Limits
  Eigen::VectorXd getVelocityLowerLimits() {
    return skel_ptr_->getVelocityLowerLimits();
  }
  Eigen::VectorXd getVelocityUpperLimits() {
    return skel_ptr_->getVelocityUpperLimits();
  }
  // Force Torque Limits
  Eigen::VectorXd GetTorqueLowerLimits() {
    return skel_ptr_->getForceLowerLimits();
  }
  Eigen::VectorXd GetTorqueUpperLimits() {
    return skel_ptr_->getForceUpperLimits();
  }

  Eigen::MatrixXd getMassMatrix();
  Eigen::MatrixXd getInvMassMatrix();
  Eigen::VectorXd getGravity();
  Eigen::VectorXd getCoriolis();
  Eigen::VectorXd getCoriolisGravity();

  Eigen::MatrixXd getCentroidJacobian();
  Eigen::MatrixXd getCentroidInertiaTimesJacobian();
  Eigen::MatrixXd getCentroidInertia();
  Eigen::VectorXd getCentroidVelocity();
  Eigen::VectorXd getCentroidMomentum();
  Eigen::Vector3d getCoMPosition(
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Vector3d getCoMVelocity(
      dart::dynamics::Frame* rl_ = dart::dynamics::Frame::World(),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getCoMJacobian(
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getCoMJacobianDot();
  void updateSystem(const Eigen::VectorXd& q_, const Eigen::VectorXd& qdot_,
                    bool isUpdatingCentroid_ = true);
  void updateCentroidFrame();

  Eigen::Isometry3d getBodyNodeIsometry(
      const std::string& name_,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Isometry3d getBodyNodeCoMIsometry(
      const std::string& name_,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Vector6d getBodyNodeSpatialVelocity(
      const std::string& name_,
      dart::dynamics::Frame* rl_ = dart::dynamics::Frame::World(),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Vector6d getBodyNodeCoMSpatialVelocity(
      const std::string& name_,
      dart::dynamics::Frame* rl_ = dart::dynamics::Frame::World(),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeJacobian(
      const std::string& name_,
      Eigen::Vector3d localOffset_ = Eigen::Vector3d::Zero(3),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeJacobianDot(
      const std::string& name_,
      Eigen::Vector3d localOffset_ = Eigen::Vector3d::Zero(3),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeCoMJacobian(
      const std::string& name_,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeCoMJacobianDot(
      const std::string& name_,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());

  Eigen::Isometry3d getBodyNodeIsometry(
      const int& _bn_idx,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Isometry3d getBodyNodeCoMIsometry(
      const int& _bn_idx,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Vector6d getBodyNodeSpatialVelocity(
      const int& _bn_idx,
      dart::dynamics::Frame* rl_ = dart::dynamics::Frame::World(),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::Vector6d getBodyNodeCoMSpatialVelocity(
      const int& _bn_idx,
      dart::dynamics::Frame* rl_ = dart::dynamics::Frame::World(),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeJacobian(
      const int& _bn_idx,
      Eigen::Vector3d localOffset_ = Eigen::Vector3d::Zero(3),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeJacobianDot(
      const int& _bn_idx,
      Eigen::Vector3d localOffset_ = Eigen::Vector3d::Zero(3),
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeCoMJacobian(
      const int& _bn_idx,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
  Eigen::MatrixXd getBodyNodeCoMJacobianDot(
      const int& _bn_idx,
      dart::dynamics::Frame* wrt_ = dart::dynamics::Frame::World());
};


#include <boost/math/tools/polynomial_gcd.hpp>


//  Copyright Nick Thompson 2018.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/math/concepts/std_real_concept.hpp>
#include <boost/math/interpolators/catmull_rom.hpp>

void compile_and_link_test()
{
    std::vector<boost::math::concepts::std_real_concept> p0{0.1, 0.2, 0.3};
    std::vector<boost::math::concepts::std_real_concept> p1{0.2, 0.3, 0.4};
    std::vector<boost::math::concepts::std_real_concept> p2{0.3, 0.4, 0.5};
    std::vector<boost::math::concepts::std_real_concept> p3{0.4, 0.5, 0.6};
    std::vector<boost::math::concepts::std_real_concept> p4{0.5, 0.6, 0.7};
    std::vector<boost::math::concepts::std_real_concept> p5{0.6, 0.7, 0.8};
    std::vector<std::vector<boost::math::concepts::std_real_concept>> v{p0, p1, p2, p3, p4, p5};
    boost::math::catmull_rom<std::vector<boost::math::concepts::std_real_concept>> cat(v.data(), v.size());
    cat(0.0);
    cat.prime(0.0);
}


#include <CGAL/algorithm.h>
#include <CGAL/function_objects.h>
#include <vector>
#include <iostream>
#include <boost/functional.hpp>

int main()
{
  std::vector< int > v;
  v.push_back(3);
  v.push_back(5);
  v.push_back(2);
  std::cout << "min_odd = "
            << *CGAL::min_element_if(v.begin(),
                                     v.end(),
                                     CGAL::compose1_1(boost::bind2nd(std::greater< int >(), 0),
                                                      boost::bind2nd(std::modulus< int >(), 2)))
            << std::endl;
  return 0;
}


/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2015 by Horatiu George Todoran <todorangrg@gmail.com>   *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder 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.                                           *
 ***************************************************************************/

#include "utils/base_classes.h"
#include "utils/math.h"
#include <boost/random.hpp>
#include <boost/random/uniform_01.hpp>

using namespace cv;

boost::minstd_rand gIntGenStd;
boost::mt19937 gIntGen19937;
boost::variate_generator<boost::mt19937, boost::normal_distribution<> > gNormal(gIntGen19937, boost::normal_distribution<>(0,1));
boost::uniform_01<boost::minstd_rand> gUniform(gIntGenStd);

double  Distributions::normalDist() {
    return gNormal();
}
double  Distributions::normalDist(double sigma) {
    return sigma * gNormal();
}
double  Distributions::normalDist(double mean, double sigma) {
    return mean + sigma * gNormal();
}
void Distributions::normalDist(cv::Vec<double,3> &mean, cv::Vec<double,3> sigma) {
    mean[0] = normalDist(mean[0], sigma[0]);
    mean[1] = normalDist(mean[1], sigma[1]);
    mean[2] = normalDist(mean[2], sigma[2]);
}
double Distributions::uniformDist(double min, double max){
    double s = max - min;
    return min + s * gUniform();
}
void Distributions::uniformDist(double min, double max, cv::Vec<double,3> &des){
    des[0] = uniformDist(min, max);
    des[1] = uniformDist(min, max);
    des[2] = uniformDist(min, max);
}

void Gauss::add_w_sample(double val, double w){
    entry_no++;
    double temp  = w + sumweight;
    double delta = val - mean;
    double r = delta * w /temp;
    mean += r;
    m2 += sumweight * delta * r;
    sumweight = temp;
}

double Gauss::getMean(){
    return mean;
}
double Gauss::getVariance(){
    if(entry_no<=1){
        //std::cout<<"WARNING: requesting variance of less than 2 entries"<<std::endl;
        return 0;
    }
    return (m2/sumweight)* (double)entry_no/((double)entry_no - 1.0);
}
double Gauss::getSampleVariance(){
    return m2/sumweight;
}

void NormalFunc::set(double _mean, double _deviation, double _no_sigma_dev){
    mean = _mean;
    sigma = _deviation / _no_sigma_dev;
}

double NormalFunc::f(double x){
    return std::exp( - sqr( x - mean ) / ( 2.0 * sqr( sigma ) ) );
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

Line get_line_param(xy p1,xy p2){
    double a=p2.y-p1.y;
    double b=p1.x-p2.x;
    double c=-(a*p2.x+b*p2.y);

    return Line(a,b,c);
}
xy get_line_inters(Line l,xy p){
    return xy( (l.b*(l.b*p.x-l.a*p.y)-l.a*l.c)/(sqr(l.a)+sqr(l.b)), (l.a*(-l.b*p.x+l.a*p.y)-l.b*l.c)/(sqr(l.a)+sqr(l.b)));
}

double get_dist_p(Line l,xy p0, xy* p_d){
    *p_d = xy((l.b * (   l.b * p0.x - l.a * p0.y) - l.a * l.c) / (sqr(l.a) + sqr(l.b)),
              (l.a * ( - l.b * p0.x + l.a * p0.y) - l.b * l.c) / (sqr(l.a) + sqr(l.b)));
    return fabs(l.a * p0.x + l.b * p0.y + l.c) / sqrt(sqr(l.a) + sqr(l.b));
}

xy get_line_inters(Line l1,Line l2){
    double x=10000,y=10000;
    if(l1.a*l2.b!=l1.b*l2.a){
        x=(l2.b*l1.c-l2.c*l1.b)/(l2.a*l1.b-l1.a*l2.b);
        y=(l2.c*l1.a-l1.c*l2.a)/(l2.a*l1.b-l1.a*l2.b);
        return xy(x,y);
    }
    else if((l1.a==l2.a)&&(l1.b==l2.b)&&(l1.c==l2.c)){
        std::cout<<"paralel overlapping lines when computing get_line_intersection"<<std::endl;
    }
    else{
        std::cout<<"paralel NONoverlapping lines when computing get_line_intersection"<<std::endl;
    }
    return xy(0,0);
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

polar::polar() {
    r=0;angle=0;
}
polar::polar(double _r, double _angle) {
    r=_r;angle=_angle;
}
polar::polar(const polar &_p){
    r=_p.r;angle=_p.angle;
}
polar::polar(PointData &_p){
    r=_p.r;angle=_p.angle;
}

polar polar_diff(polar p1,polar p2){
    return polar(sqrt(sqr(p2.r*cos(p2.angle)-p1.r*cos(p1.angle))+sqr(p2.r*sin(p2.angle)-p1.r*sin(p1.angle))),
                 atan2(p2.r*sin(p2.angle)-p1.r*sin(p1.angle),p2.r*cos(p2.angle)-p1.r*cos(p1.angle)));
}

polar polar_diff(polar p1,PointData &p2){
    return polar_diff(p1,polar(p2.r,p2.angle));
}
polar polar_diff(PointData &p1,polar p2){
    return polar_diff(polar(p1.r,p1.angle),p2);
}

polar polar_diff(PointData p1,PointData &p2){
    return polar_diff(polar(p1.r,p1.angle),polar(p2.r,p2.angle));
}

polar radial_diff(polar p1,polar p2){
    double d_angle=fabs(p1.angle-p2.angle);
    normalizeAngle(d_angle);
    return polar(d_angle*p1.r,d_angle);
}
polar radial_diff(polar p1,PointData &p2){
    return radial_diff(p1,polar(p2.r,p2.angle));
}

xy to_xy(polar p){
    return xy(p.r*cos(p.angle),p.r*sin(p.angle));
}
xy to_xy(PointData &p){
    return to_xy(polar(p.r,p.angle));
}

polar to_polar(xy c){
    return polar(sqrt(c.x*c.x+c.y*c.y),atan2(c.y,c.x));
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

double normalizeAngle(double &angle) {
    while(angle > M_PI){
      angle = angle - (2*M_PI);
    }
    while(angle <= -M_PI){
      angle = angle + (2*M_PI);
    }
    return angle;
}

double deg_to_rad(double angle_deg){
    return (angle_deg*M_PI)/180.0;
}

double rad_to_deg(double angle_rad){
    return (angle_rad*180.0)/(double)M_PI;
}

double sqr(double x){
    return x*x;
}

double sgn(double x){
    if(x>=0){
        return 1;
    }
    else{
        return -1;
    }
}

void angular_bounds(polar p, double circle_rad, double* search_angle){
    search_angle[0] = p.angle;
    search_angle[1] = search_angle[0];
    if( fabs( circle_rad / p.r ) > 1.0  ){
        search_angle[0] -= 2 * M_PI;
        search_angle[1] += 2 * M_PI;
    }
    else{
        search_angle[0] -= asin( circle_rad / p.r );
        search_angle[1] += asin( circle_rad / p.r );
    }
}
void angular_bounds(PointData &p,double circle_rad, double* search_angle){
    return angular_bounds(polar(p.r,p.angle),circle_rad,search_angle);
}

void angular_bounds(PointDataCpy &p,double circle_rad, double* search_angle){
    return angular_bounds(polar(p.r,p.angle),circle_rad,search_angle);
}

///------------------------------------------------------------------------------------------------------------------------------------------------///

void set_tf_mat(cv::Matx<double,3,3>& tf, xy trans, double rot){
    cv::Matx<double,3,3> R ( cos(rot), -sin(rot),   0,
                             sin(rot),  cos(rot),   0,
                                      0,           0,   1);    // Rotate
    cv::Matx<double,3,3> T (          1,           0, trans.x,
                                      0,           1, trans.y,
                                      0,           0,   1);    // Translate
    // Calculate final transformation matrix
    tf = T * R;
    //Mptf = Mptf.inv();
    //std::cout <<  "Ms2r = " << std::endl << Ms2r << std::endl;
    //std::cout <<  "Mr2s = " << std::endl << Mr2s << std::endl;
}

xy   mat_mult(cv::Matx<double,3,3>& tf,xy p){
    cv::Matx<double,3,1> pw (p.x, p.y, 1.0);
    cv::Matx<double,3,1> pi = tf * pw;
    return  xy (pi(0,0), pi(1,0));
}

cv::RotatedRect cov2rect(cv::Matx<double, 2, 2> _C,xy _center){
    cv::RotatedRect ellipse;
    cv::Mat_<double> eigval, eigvec;
    cv::eigen(_C, eigval, eigvec);

    /// Exercise4
    bool index_x;
    ellipse.center = _center;
    if(_C(0,0)>_C(1,1)){
        index_x=0;
    }
    else{
        index_x=1;
    }
    ellipse.size.height=sqrt(fabs(eigval(0,!index_x)))*2.4477;//y
    ellipse.size.width=sqrt(fabs(eigval(0,index_x)))*2.4477;//x
    if((eigval(0,index_x)!=0)&&(eigval(0,!index_x)!=0))
        ellipse.angle=atan2(eigvec(index_x,1),eigvec(index_x,0))*(180/M_PI);

    return ellipse;
}


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

#include <nt2/euler/functions/stirling.hpp>
#include <nt2/euler/functions/details/stirling_kernel.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/sqrt_2pi.hpp>
#include <nt2/include/constants/stirlingsplitlim.hpp>
#include <nt2/include/functions/simd/exp.hpp>
#include <nt2/include/functions/simd/fma.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/is_less.hpp>
#include <nt2/include/functions/simd/minus.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/functions/simd/pow.hpp>
#include <nt2/include/functions/simd/rec.hpp>
#include <nt2/include/functions/simd/unary_minus.hpp>
#include <nt2/sdk/meta/as_logical.hpp>

#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INFINITIES
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#endif

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( stirling_, tag::cpu_
                            , (A0)(X)
                            , ((simd_<floating_<A0>,X>))
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      A0 w = nt2::rec(a0);
      w = fma(w,details::stirling_kernel<A0>::stirling1(w), nt2::One<A0>());
      A0 y = nt2::exp(-a0);
      bA0 test = is_less(a0, nt2::Stirlingsplitlim<A0>());
      A0 z =  a0 - nt2::Half<A0>();
      z =  if_else(test, z, Half<A0>()*z);
      A0 v =  nt2::pow(a0,z);
      y *= v;
      y = if_else(test,y, y*v); /* Avoid overflow in pow() */
      y *= nt2::Sqrt_2pi<A0>()*w;
      #ifndef BOOST_SIMD_NO_INFINITIES
      y = if_else(eq(a0, Inf<A0>()), a0, y);
      #endif
      return y;
    }
  };
} }

#endif


#ifndef TEST_UNIT_TORSTEN_PK_CPT_MODEL_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_PK_CPT_MODEL_TEST_FIXTURE

#include <stan/math/rev/mat.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <stan/math/torsten/pmx_onecpt_model.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/pmx_linode_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <test/unit/math/torsten/pmx_ode_test_fixture.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

struct CoupledOneCptODE {
  /*
   * Coupled model functor
   */
  template <typename T0, typename T1, typename T2, typename T3>
  inline
  std::vector<typename boost::math::tools::promote_args<T0, T1, T2, T3>::type>
  operator()(const T0& t,
             const std::vector<T1>& x,
             const std::vector<T2>& x_pk,
             const std::vector<T3>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i,
             std::ostream* pstream_) const {
    typedef typename boost::math::tools::promote_args<T0, T1, T2, T3>::type
      scalar;

    scalar
      VC = theta[1],
      Mtt = theta[3],
      circ0 = theta[4],
      alpha = theta[5],
      gamma = theta[6],
      ktr = 4 / Mtt,
      prol = x[0] + circ0,
      transit = x[1] + circ0,
      circ = x[2] + circ0,
      conc = x_pk[1] / VC,
      Edrug = alpha * conc;

    std::vector<scalar> dxdt(3);
    dxdt[0] = ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1);
    dxdt[1] = ktr * (prol - transit);
    dxdt[2] = ktr * (transit - circ);

    return dxdt;
  }

  /*
   * full ODE model functor
   */
  template <typename T0, typename T1, typename T2>
  inline
  std::vector<typename boost::math::tools::promote_args<T0, T1, T2>::type>
  operator()(const T0& t,
             const std::vector<T1>& x,
             const std::vector<T2>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i,
             std::ostream* pstream_) const {
    typedef typename boost::math::tools::promote_args<T0, T1, T2>::type
      scalar;

    scalar
      CL = theta[0],
      VC = theta[1],
      ka = theta[2],
      Mtt = theta[3],
      circ0 = theta[4],
      alpha = theta[5],
      gamma = theta[6],
      ktr = 4 / Mtt,
      prol = x[2] + circ0,
      transit = x[3] + circ0,
      circ = x[4] + circ0,
      Edrug;

    std::vector<scalar> dxdt(5);
    dxdt[0] = - ka * x[0];
    dxdt[1] = ka * x[0] - CL / VC * x[1];

    Edrug = alpha * x[1] / VC;

    dxdt[2] = ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1);
    dxdt[3] = ktr * (prol - transit);
    dxdt[4] = ktr * (transit - circ);

    return dxdt;
  }
};

struct CoupledTwoCptODE {
  /*
   * Coupled model functor
   */
  template <typename T0, typename T1, typename T2, typename T3>
  inline
  std::vector<typename boost::math::tools::promote_args<T0, T1, T2, T3>::type>
  operator()(const T0& t,
             const std::vector<T1>& y,
             const std::vector<T2>& y_pk,
             const std::vector<T3>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i,
             std::ostream* pstream_) const {
    typedef typename boost::math::tools::promote_args<T0, T1, T2, T3>::type
      scalar;

    scalar VC = theta[2],
      Mtt = theta[5],
      circ0 = theta[6],
      alpha = theta[7],
      gamma = theta[8],
      ktr = 4 / Mtt,
      prol = y[0] + circ0,
      transit = y[1] + circ0,
      circ = y[2] + circ0,
      conc = y_pk[1] / VC,
      Edrug = alpha * conc;

    std::vector<scalar> dxdt(3);
    dxdt[0] = ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1);
    dxdt[1] = ktr * (prol - transit);
    dxdt[2] = ktr * (transit - circ);

    return dxdt;
  }

  /*
   * full ODE model functor
   */
  template <typename T0, typename T1, typename T2>
  inline
    std::vector<typename boost::math::tools::promote_args<T0, T1, T2>::type>
  operator()(const T0& t,
           const std::vector<T1>& x,
           const std::vector<T2>& theta,
           const std::vector<double>& x_r,
           const std::vector<int>& x_i,
           std::ostream* pstream_) const {
    typedef typename boost::math::tools::promote_args<T0, T1, T2>::type
    scalar;

    scalar
      CL = theta[0],
      Q = theta[1],
      VC = theta[2],
      VP = theta[3],
      ka = theta[4],
      k10 = CL / VC,
      k12 = Q / VC,
      k21 = Q / VP,
      Mtt = theta[5],
      circ0 = theta[6],
      alpha = theta[7],
      gamma = theta[8],
      ktr = 4 / Mtt,
      prol = x[3] + circ0,
      transit = x[4] + circ0,
      circ = x[5] + circ0,
      Edrug;

    std::vector<scalar> dxdt(6);
    dxdt[0] = -ka * x[0];
    dxdt[1] = ka * x[0] - (k10 + k12) * x[1] + k21 * x[2];
    dxdt[2] = k12 * x[1] - k21 * x[2];
    Edrug = alpha * x[1] / VC;
    dxdt[3] = ktr * prol * ((1 - Edrug) * pow(circ0 / circ, gamma) - 1);
    dxdt[4] = ktr * (prol - transit);
    dxdt[5] = ktr * (transit - circ);

    return dxdt;
  }
};

struct TorstenCoupledOneCptTest : public testing::Test {
  // for events generation
  const int nt;
  const int nOde;
  const int nPD;
  std::vector<double> time;
  std::vector<double> amt;
  std::vector<double> rate;
  std::vector<int> cmt;
  std::vector<int> evid;
  std::vector<double> ii;
  std::vector<int> addl;
  std::vector<int> ss;
  std::vector<std::vector<double> > parameters;
  std::vector<std::vector<double> > biovar;
  std::vector<std::vector<double> > tlag;

  // for ODE integrator
  double t0;
  std::vector<double> x_r;
  std::vector<int> x_i;
  double rtol;
  double atol;
  int max_num_steps;
  std::ostream* msgs;

  void SetUp() {
    // make sure memory's clean before starting each test
    stan::math::recover_memory();
  }
  TorstenCoupledOneCptTest() :
    nt(10),
    nOde(5),
    nPD(3),
    time(nt),
    amt(nt, 0),
    rate(nt, 0),
    cmt(nt, 2),
    evid(nt, 0),
    ii(nt, 0),
    addl(nt, 0),
    ss(nt, 0),
    // params: // CL // VC // ka // Mtt // Circ0 // alpha // gamma
    parameters{ {10, 35, 2.0, 125, 5, 3e-4, 0.17} },
    biovar{ { 1, 1, 1, 1, 1 } },
    tlag{ { 0, 0, 0, 0, 0 } },
    t0(0.0),
    rtol             {1.E-10},
    atol             {1.E-10},
    max_num_steps    {100000},
    msgs             {nullptr} {
      for (int i = 0; i < nt; ++i) {
        time[i] = i * 0.25;
      }
      time.back() = 4.0;
      amt[0]  = 10000;
      cmt[0]  = 1;
      evid[0] = 1;
      SetUp();
  }
};

struct TorstenCoupledTwoCptTest : public testing::Test {
  // for events generation
  const int nt;
  const int nOde;
  const int nPD;
  std::vector<double> time;
  std::vector<double> amt;
  std::vector<double> rate;
  std::vector<int> cmt;
  std::vector<int> evid;
  std::vector<double> ii;
  std::vector<int> addl;
  std::vector<int> ss;
  std::vector<std::vector<double> > parameters;
  std::vector<std::vector<double> > biovar;
  std::vector<std::vector<double> > tlag;

  // for ODE integrator
  double t0;
  std::vector<double> x_r;
  std::vector<int> x_i;
  double rtol;
  double atol;
  int max_num_steps;
  std::ostream* msgs;

  void SetUp() {
    // make sure memory's clean before starting each test
    stan::math::recover_memory();
  }
  TorstenCoupledTwoCptTest() :
    nt(10),
    nOde(6),
    nPD(3),
    time(nt),
    amt(nt, 0),
    rate(nt, 0),
    cmt(nt, 2),
    evid(nt, 0),
    ii(nt, 0),
    addl(nt, 0),
    ss(nt, 0),
    // CL // Q // VC // VP // ka // Mtt // Circ0 // alpha // gamma
    parameters{ {10, 15, 35, 105, 2.0, 125, 5, 3e-4, 0.17} },
    biovar{ { 1, 1, 1, 1, 1, 1 } },
    tlag{ { 0, 0, 0, 0, 0, 0 } },
    t0(0.0),
    rtol             {1.E-10},
    atol             {1.E-10},
    max_num_steps    {100000},
    msgs             {nullptr} {
      for (int i = 0; i < nt; ++i) {
        time[i] = i * 0.25;
      }
      time.back() = 4.0;
      amt[0]  = 10000;
      cmt[0]  = 1;
      evid[0] = 1;
      SetUp();
  }
};

#endif


#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <array>
#include <unordered_set>
#include <unordered_map>
#include <boost/format.hpp>
#include "decision_tree.h"
#include "utils.h"

using namespace std;
using namespace Eigen;

DecisionTree::DecisionTree(int min_samples_split, double min_impurity, int max_depth) : min_samples_split(min_samples_split), min_impurity(min_impurity), max_depth(max_depth) {}

DecisionTree::~DecisionTree() {}

void DecisionTree::fit(const MatrixXd& X, const VectorXd& y){
	root = build_tree(X, y, 0);
}

DecisionNode* DecisionTree::build_tree(const MatrixXd& X, const VectorXd& y, int current_depth){
	double largest_impurity = 0.0;
	//cout << "start..." << endl;
	MatrixXd Xy = MatrixXd(X.rows(), X.cols()+1);
	Xy << X, y;
	//cout << "its ok?"<<endl;
	//cout << "..............." <<endl;
	//cout << "X.rows: " << X.rows() << "  X.cols:" << X.cols() << " y.size:" << y.size() << "  ..Xy.rows:" << Xy.rows() << "..Xy.cols:" << Xy.cols() << endl;
	int n_samples = X.rows();
	int n_features = X.cols();

	array<double, 2> best_criteria;
	array<MatrixXd, 4> best_sets;

	if(n_samples >= min_samples_split && current_depth < max_depth){
		// calculate the impurity for each feature
		for(int i = 0; i < n_features; i++){
			// all values of feature index i
			VectorXd feature_values = X.col(i);
			unordered_set<double> unique_values;
			for(int j = 0; j < feature_values.size(); j++){
				unique_values.insert(feature_values[j]);
			}

			// iterate through all unique values of feature column i and
			// calculate the impurity
			for(auto p = unique_values.begin(); p != unique_values.end(); p++){
				//cout << "start div..." << endl;
				array<MatrixXd,2> X_div = Utils::divide_on_feature(Xy, i, *p);
				MatrixXd Xy1 = X_div[0];
				MatrixXd Xy2 = X_div[1];
				//cout << "Xy1: " << Xy1.rows() << ".." << Xy1.cols() << endl;
				//cout << "Xy2: " << Xy2.rows() << ".." << Xy2.cols() << endl;

				if(Xy1.rows() > 0 && Xy2.rows() > 0){
					// select the y_values of the two sets
					VectorXd y1 = Xy1.col(Xy1.cols()-1);
					VectorXd y2 = Xy2.col(Xy2.cols()-1);
					//cout << "y1:" << y1.size() << "y2: " << y2.size() << endl;
					// calculate impurity
					double impurity = impurity_calculation(y, y1, y2);
					//cout << "impurity done " << impurity << endl;
					// if this threshold resulted in a higher information gain than previously
					// recorded save the threshod value and the feature
					if(impurity > largest_impurity){
						largest_impurity = impurity;
						best_criteria[0] = i;
						best_criteria[1] = *p;
						//cout << "best_criteria " << best_criteria[0] << " " << best_criteria[1] << endl;

						//MatrixXd Xy1_feature = Xy1.leftCols(Xy1.cols()-1);
						//MatrixXd Xy2_feature = Xy2.leftCols(Xy2.cols()-1);

						MatrixXd Xy1_feature(Xy1.rows(), Xy1.cols()-1), Xy2_feature(Xy2.rows(), Xy2.cols()-1);
						for(int k = 0; k < Xy1.cols()-1; k++){
							Xy1_feature.col(k) = Xy1.col(k);
						}
						for(int k = 0; k < Xy2.cols()-1; k++){
							Xy2_feature.col(k) = Xy2.col(k);
						}
						best_sets[0] = Xy1_feature;
						best_sets[1] = y1;
						best_sets[2] = Xy2_feature;
						best_sets[3] = y2;
					}
				}
			}
		}
	}
	//cout << "bulid tree next" << endl;
	if(largest_impurity > min_impurity){
		DecisionNode* true_branch = build_tree(best_sets[0], best_sets[1], current_depth+1);
		DecisionNode* false_branch = build_tree(best_sets[2], best_sets[3], current_depth+1);
		return new DecisionNode(static_cast<int>(best_criteria[0]), best_criteria[1], -1, true_branch, false_branch);
	}
	// we're at leaf ==> determind value
	double leaf_value = leaf_value_calculation(y);
	cout << "leaf value: " << leaf_value << endl;
	return new DecisionNode(leaf_value);
}

int DecisionTree::predict(const VectorXd& X, DecisionNode* r){
	if(r == nullptr){
		r = root;
	}
	if(r->value != -1){
		return r->value;
	}
	//cout << "r->feature:" << r->feature << endl;
	double feature_value = X(r->feature);
	//cout << "nani ..." << endl;
	DecisionNode* branch = r->false_branch;
	if(feature_value >= r->threshold){
		branch = r->true_branch;
	}
	return predict(X, branch);
}

VectorXi DecisionTree::predict(const MatrixXd& X){
	VectorXi ret(X.rows());
	for(int i = 0; i < X.rows(); i++){
		ret(i) = predict(X.row(i), root);
	}
	return ret;
}

double DecisionTree::impurity_calculation(const VectorXd& y, const VectorXd& y1, const VectorXd& y2){
	// calculate information gain
	double prob = y1.size()/double(y2.size());
	double entropy = Utils::calculate_entropy(y);
	double info_gain = entropy - prob*Utils::calculate_entropy(y1) - (1-prob)*Utils::calculate_entropy(y2);
	return info_gain;
}

int DecisionTree::leaf_value_calculation(const VectorXd& y){
	int most_common;
	int max_count = 0;
	unordered_map<int,int> m;
	for(int i = 0; i < y.size(); i++){
		m[y(i)]++;
	}
	for(auto p = m.begin(); p != m.end(); p++){
		if(p->second > max_count){
			most_common = p->first;
			max_count = p->second;
		}
	}
	return most_common;
}


#include <cmath>
#include <cstring>
#include <cassert>
#include <complex>
#include <filesystem>

#include <signal.h>
#include <boost/fiber/buffered_channel.hpp>

#include <scluk/language_extension.hpp>
#include <scluk/math.hpp>
#include <scluk/array.hpp>
#include <scluk/functional.hpp>
#include <scluk/sliding_queue.hpp>

#include "dft/sliding_dft.hpp"
#include "sdl_gui_thread.hpp"
#include "audio_params.hpp"

int main() {
    using namespace scluk::language_extension;

    std::filesystem::current_path(EXECUTABLE_DIR);

    sdl_gui_thread gui_thread;

	//interrupt signal handling
    signal(SIGINT, scluk::lambda_to_fnptr<void(int)>([&gui_thread](int) {
        out("caught sigint!");
        gui_thread.data.do_exit = true;
    }));

    audio::sliding_dft dft;
    audio::duplex_chan cb_chan;
    portaudio::async_stream stream({ .frames_per_buffer=audio::ft_dist, .rate=audio::rate, .log=EXECUTABLE_DIR"/log.txt" }, audio::cb, cb_chan);

    scluk::sliding_queue<audio::ift_chunk, audio::ift_overlap> ift_queue(audio::ift_chunk(0.f));
    //first iteration, just to populate the arrays
    dft.push_frames(cb_chan.cb_to_main.value_pop());
    audio::dft_array phase_adjusted_dft(dft), old_dft(dft);

    //main loop
    while(!gui_thread.data.do_exit) {
        //push the frames received from portaudio
        dft.push_frames_fft(cb_chan.cb_to_main.value_pop());

        const f32 pitch_mul = gui_thread.data.do_apply_effect ? std::pow(2.f, f32(gui_thread.data.pitch)/12.f) : 1.f;
        //phase adjustment to avoid artifacts (this is what makes this a phase vocoder)
        for(u64 i : index(dft)) {
            using std::abs, std::arg;
            using scluk::math::pi;

            //p means phase, A means amplitude
            const f32 p_new = arg(dft[i]), p_old = arg(old_dft[i]),
                      p_old_adj = arg(phase_adjusted_dft[i]), A_new = abs(dft[i]);

            //integer division allows me to automatically floor without additional cost
            const f32 unwrap_addend = 2.f*pi * f32(i / audio::ift_overlap);

            const f32 raw_p_delta = p_new - p_old;
            const f32 mod_p_delta = raw_p_delta + std::signbit(raw_p_delta) * 2.f*pi;

            const f32 adj_p_delta = (unwrap_addend + mod_p_delta) * pitch_mul;
            const f32 p_new_adj = p_old_adj + adj_p_delta;

            phase_adjusted_dft[i] = std::polar(A_new, p_new_adj);
        }

        //calculate the latest ift, apply the hann window and enqueue it
        ift_queue << scluk::math::hann_window(phase_adjusted_dft.ifft<audio::ft_win>(pitch_mul));

        audio::frame_chunk frames(0.f);

        for(u64 n : index(ift_queue)) {
            u64 starting_index = (audio::ift_overlap-1 - n) * audio::ft_dist;

            for(u64 i : range(audio::ft_dist))
                frames[i] += ift_queue[n][starting_index + i];
        }

        //send the frames to the callback
        if(gui_thread.data.do_output_audio)
            cb_chan.main_to_cb.push(std::move(frames));
        else cb_chan.main_to_cb.push(audio::frame_chunk(0.f));

        //send the old dft to the gui and repopulate it
        gui_thread.channel.try_push(std::move(old_dft));
        old_dft = dft;
    }
}


#ifndef __SWIG_CPP
#define __SWIG_CPP

#include "swig_mod.hpp"
#include <Eigen/Cholesky>

Eigen::MatrixXd cholesky_swig_eigen(const Eigen::MatrixXd &M) {
    Eigen::LLT<Eigen::MatrixXd> lltOfM(M);
  return lltOfM.matrixL();
}
#endif


// This file is a part of the OpenSurgSim project.
// Copyright 2016, SimQuest Solutions Inc.
//
// 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 <cmath>
#include <fstream>
#include <iostream>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <string>
#include <vector>
#include <yaml-cpp/yaml.h>

#include "SurgSim/DataStructures/Vertices.h"
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/CardinalSplines.h"
#include "SurgSim/Math/Vector.h"

/// Function to generate points on a circle with radius 'radius'.
/// \note The points always start from the origin and go on the circle to 'angle'.
/// \param subdivisions Number of interpolated points on the circle.
/// \param radius Radius of the circle on which points are generated.
/// \param angle How much (of the circle) should points occupy, in radian.
/// \return A list of points on the circle.
std::vector<SurgSim::Math::Vector3d> generateNeedle(size_t subdivisions, double radius, double angle)
{
	SURGSIM_ASSERT(0 < angle && angle < 2.0 * M_PI) << __FUNCTION__ << " 'angle' must be in the range (0, 2PI)";
	SURGSIM_ASSERT(2 <= subdivisions) << __FUNCTION__ << " 'subdivisions' must be at least 2";

	double increment = angle / static_cast<double>(subdivisions);
	double controlAngle = 0.0;

	std::vector<SurgSim::Math::Vector3d> vertices(subdivisions+1);
	for (size_t i = 0; i < subdivisions; ++i)
	{
		vertices[i] = radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);
		controlAngle += increment;
	}
	vertices[subdivisions] =
		radius * SurgSim::Math::Vector3d(std::cos(controlAngle) - 1.0, std::sin(controlAngle), 0.0);

	return vertices;
}

/// Function to generate a suture, starting from the origin and wraps around 'axis' in circles.
/// A circle (formed by the suture when wraps 'axis') is defined by four markers.
/// Each marker is rotated 45 degrees along 'axis' from previous one.
/// 'subdivisions' controls how many points to interpolate between each pair of markers.
/// In the returned list, the origin will always be the first point.
/// \param subdivisions Number of points between makers.
/// \param radius Radius of the circle the suture forms.
/// \param length Length of the suture.
/// \param circles Number of circles the suture forms wrapping around 'axis'.
/// \param axis The axis around which the suture wraps.
/// \return A list of points representing the suture.
std::vector<SurgSim::Math::Vector3d> generateSuture(size_t subdivisions, double radius, double length, size_t circles,
										const SurgSim::Math::Vector3d& axis = SurgSim::Math::Vector3d(1.0, 0.0, 0.0))
{
	// Increment on X-axis.
	double increment = length / circles / 4.0;

	SurgSim::Math::Vector3d direction = axis;
	direction.normalize();
	auto rotateXToAxis = SurgSim::Math::makeRotationQuaternion(
											 -std::acos(direction.dot(SurgSim::Math::Vector3d(1.0, 0.0, 0.0))),
													  direction.cross(SurgSim::Math::Vector3d(1.0, 0.0, 0.0)));

	std::vector<SurgSim::Math::Vector3d> controlPoints;
	controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0) - increment * direction);
	controlPoints.push_back(SurgSim::Math::Vector3d(0.0, 0.0, 0.0));

	// Used to control the rotation between markers.
	SurgSim::Math::Quaterniond rotation =
								SurgSim::Math::makeRotationQuaternion(M_PI_2, SurgSim::Math::Vector3d(1.0, 0.0, 0.0));
	auto point = SurgSim::Math::Vector3d(increment, radius, 0.0);
	for (size_t i = 0; i < circles * 4; ++i)
	{
		controlPoints.push_back(rotateXToAxis * point);
		point = rotation * point;
		point.x() += increment;
	}

	std::vector<SurgSim::Math::Vector3d> result;
	SurgSim::Math::CardinalSplines::interpolate(subdivisions, controlPoints, &result);

	return result;
}

/// Save a list of needle and a list of suture as one ply file.
/// Points in 'needle' will be output first, as the order they appear in 'needle' and
/// points in 'suture' will be output second, as the order they appear in 'suture'.
/// The caller of this function needs to make sure
/// the needle and the suture connects (smoothly) at needle[last] and suture[begin].
/// \param fileName Name of the ply file.
/// \param needle List of points representing the needle.
/// \param suture List of points representing the suture.
/// \param asPhysics Boolean to decide whether or not to save physics properties.
/// \param needleMassDensity Mass density of needle.
/// \param needlepoissonRatio Poisson ratio of needle.
/// \param needleYoungModulus Young modulus of needle.
/// \param sutureMassDensity Mass density of suture.
/// \param suturepoissonRatio Poisson ratio of suture.
/// \param sutureYoungModulus Young modulus of suture.
/// \param radius Radius used for fem1d beam.
void saveNeedleSuturePly(const std::string& fileName,
						 const std::vector<SurgSim::Math::Vector3d>& needle,
						 const std::vector<SurgSim::Math::Vector3d>& suture,
						 bool asPhysics,
						 double needleMassDensity, double needlePoissonRatio, double needleYoungModulus,
						 double sutureMassDensity, double suturePoissonRatio, double sutureYoungModulus,
						 double radius = 0.0001)
{
	std::ofstream out(fileName);

	size_t numOfPoints = needle.size() + suture.size();
	if (out.is_open())
	{
		out << "ply" << std::endl;
		out << "format ascii 1.0" << std::endl;
		out << "comment Created by OpenSurgSim, www.opensurgsim.org" << std::endl;
		out << "element vertex " << numOfPoints << std::endl;
		out << "property float x\nproperty float y\nproperty float z" << std::endl;
		if (asPhysics)
		{
			out << "element 1d_element " << numOfPoints - 1 << std::endl;
			out << "property list uint uint vertex_indices" << std::endl;
			out << "property double mass_density" << std::endl;
			out << "property double poisson_ratio" << std::endl;
			out << "property double young_modulus" << std::endl;
		}
		out << "element radius 1" << std::endl;
		out << "property double value" << std::endl;
		out << "element boundary_condition 0" << std::endl;
		out << "property uint vertex_index" << std::endl;
		out << "end_header" << std::endl;
		for (const auto& vertex : needle)
		{
			out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
		}
		for (const auto& vertex : suture)
		{
			out << vertex[0] << " " << vertex[1] << " " << vertex[2] << std::endl;
		}

		if (asPhysics)
		{
			int index = 0;
			for (const auto& vertex : needle)
			{
				out << "2 " << index << " " << index + 1 << " " <<
					needleMassDensity << " " << needlePoissonRatio << " " << needleYoungModulus << std::endl;
				++index;
			}
			for (const auto& vertex : suture)
			{
				out << "2 " << index << " " << index + 1 << " " <<
					sutureMassDensity << " " << suturePoissonRatio << " " << sutureYoungModulus << std::endl;
				++index;
			}
		}

		out << radius << std::endl;

		if (out.bad())
		{
			SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
				<< "There was a problem writing " << fileName;
		}

		out.close();
	}
	else
	{
		SURGSIM_LOG_WARNING(SurgSim::Framework::Logger::getDefaultLogger()) << __FUNCTION__
			<< "Could not open " << fileName << " for writing.";
	}
};

// Utility to generate a ply file for needle suture.
// Command line input are supported.
int main(int argc, char* argv[])
{
	namespace po = boost::program_options;

	po::options_description commandLine("Allowed options");
	commandLine.add_options()("help", "produce help message")
	("filename", po::value<std::string>()->default_value("needlesuture.ply"),
									"File name to save the generated needle suture.")
	("needleSubdivisions", po::value<int>()->default_value(10),
									"Number of interpolated points for the needle (default 10)")
	("needleRadius", po::value<double>()->default_value(0.019), "Radius of the needle (in m) (default 0.019)")
	("needleAngle", po::value<double>()->default_value(135),
									"How big is the arc (common values: 90, 135, 180, 225) in degrees (default 135)")
	("sutureSubdivisions", po::value<int>()->default_value(20),
									"Number of interpolated points for each pair of markers on the suture (default 20)")
	("sutureRadius", po::value<double>()->default_value(0.01), "Radius of the suture (default 0.01")
	("sutureLength", po::value<double>()->default_value(0.45), "Length of th suture (in m) (default 0.45)")
	("sutureCircles", po::value<int>()->default_value(2), "Number of circles formed by suture going around the axis")
	("savePhysicsProperty", po::value<bool>()->default_value(true),
									"Output physics properties for the needle and suture in ply file")
	("physicsProperty", po::value<std::string>(),
									"File name for physics properties. Note that command line values have priority: "
									"program will use value(s) from command line if specified.")
	("needleMassDensity", po::value<double>(), "Mass density for the needle (default 7500 Kg.m-3)")
	("needlePoissonRatio", po::value<double>(), "Poisson ratio for the needle (default 0.305)")
	("needleYoungModulus", po::value<double>(), "Young Modulus for the needle (default (1.8e11 Pa)")
	("sutureMassDensity", po::value<double>(), "Mass density for the suture (default 900 Kg.m-3)")
	("suturePoissonRatio", po::value<double>(), "Poisson ratio for the suture (default 0.45)")
	("sutureYoungModulus", po::value<double>(), "Young Modulus for the suture (default 1.75e9 Pa)");

	po::variables_map variables;
	try
	{
		po::store(po::parse_command_line(argc, argv, commandLine), variables);
	}
	catch (po::error& e)
	{
		std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
		std::cerr << commandLine << std::endl;
		return 1;
	}

	if (variables.count("help"))
	{
		std::cout << commandLine << "\n";
		return 1;
	}

	double needleMassDensity = 7500, needlePoissonRatio = 0.305, needleYoungModulus = 180 * 1e9;
	double sutureMassDensity = 900,  suturePoissonRatio = 0.45,  sutureYoungModulus = 1.75 * 1e9;
	if (variables.count("physicsProperty"))
	{
		boost::filesystem::path fileName("Data/" + variables["physicsProperty"].as<std::string>());
		boost::filesystem::path filePath = boost::filesystem::complete(fileName);
		std::cout << "File path: " << filePath << std::endl;
		if (!boost::filesystem::exists(filePath))
		{
			std::cout << "Can't find my file!" << std::endl;
			return -1;
		}

		boost::property_tree::ptree parameters;
		boost::property_tree::ini_parser::read_ini(filePath.string(), parameters);
		needleMassDensity  = parameters.get<double>("PhysicsProperty.needleMassDensity");
		needlePoissonRatio = parameters.get<double>("PhysicsProperty.needlePoissonRatio");
		needleYoungModulus = parameters.get<double>("PhysicsProperty.needleYoungModulus");
		sutureMassDensity  = parameters.get<double>("PhysicsProperty.sutureMassDensity");
		suturePoissonRatio = parameters.get<double>("PhysicsProperty.suturePoissonRatio");
		sutureYoungModulus = parameters.get<double>("PhysicsProperty.sutureYoungModulus");
	}

	// Command line values for physics properties will take precedence if specified.
	if (variables.count("needleMassDensity"))  needleMassDensity  = variables["needleMassDensity"].as<double>();
	if (variables.count("needlePoissonRatio")) needlePoissonRatio = variables["needlePoissonRatio"].as<double>();
	if (variables.count("needleYoungModulus")) needleYoungModulus = variables["needleYoungModulus"].as<double>();
	if (variables.count("sutureMassDensity"))  sutureMassDensity  = variables["sutureMassDensity"].as<double>();
	if (variables.count("suturePoissonRatio")) suturePoissonRatio = variables["suturePoissonRatio"].as<double>();
	if (variables.count("sutureYoungModulus")) sutureYoungModulus = variables["sutureYoungModulus"].as<double>();

	auto needlePoints = generateNeedle(variables["needleSubdivisions"].as<int>(),
									   variables["needleRadius"].as<double>(),
									   variables["needleAngle"].as<double>() / 180 * M_PI);

	auto suturePoints = generateSuture(variables["sutureSubdivisions"].as<int>(),
									   variables["sutureRadius"].as<double>(),
									   variables["sutureLength"].as<double>(),
									   variables["sutureCircles"].as<int>(),
									   needlePoints[0] - needlePoints[1]);

	// Points generated are starting from the origin, when constructing the needle suture,
	// the list of needle points need to be reversed and the origin is removed to avoid duplication.
	std::vector<SurgSim::Math::Vector3d> needlePointsReversed(needlePoints.rbegin(), needlePoints.rend()-1);

	saveNeedleSuturePly(variables["filename"].as<std::string>(), needlePointsReversed, suturePoints,
						variables["savePhysicsProperty"].as<bool>(),
						needleMassDensity, needlePoissonRatio, needleYoungModulus,
						sutureMassDensity, suturePoissonRatio, sutureYoungModulus);
	return 0;
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
#include <NTL/ZZ.h>
#include <algorithm>
#include <complex>

#include <helib/norms.h>
#include <helib/helib.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {
struct Parameters
{
  Parameters(long R, long m, long r, long L, double epsilon) :
      R(R), m(m), r(r), L(L), epsilon(epsilon){};

  const long R; // number of rounds
  const long m;
  const long r;
  const long L;
  const double epsilon;

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "R=" << params.R << ","
              << "m=" << params.m << ","
              << "r=" << params.r << ","
              << "L=" << params.L << ","
              << "epsilon=" << params.epsilon << "}";
  }
};

// Utility functions for the tests

// Compute the L-infinity distance between two vectors
double calcMaxDiff(const std::vector<std::complex<double>>& v1,
                   const std::vector<std::complex<double>>& v2)
{
  if (helib::lsize(v1) != helib::lsize(v2)) {
    throw std::runtime_error("Vector sizes differ.");
  }

  double maxDiff = 0.0;
  for (long i = 0; i < helib::lsize(v1); i++) {
    double diffAbs = std::abs(v1[i] - v2[i]);
    if (diffAbs > maxDiff)
      maxDiff = diffAbs;
  }
  return maxDiff;
}
// Compute the max relative difference between two vectors
double calcMaxRelDiff(const std::vector<std::complex<double>>& v1,
                      const std::vector<std::complex<double>>& v2)
{
  if (helib::lsize(v1) != helib::lsize(v2)) {
    throw std::runtime_error("Vector sizes differ.");
  }

  // Compute the largest-magnitude value in the vector
  double maxAbs = 0.0;
  for (const auto& x : v1) {
    if (std::abs(x) > maxAbs)
      maxAbs = std::abs(x);
  }
  if (maxAbs < 1e-10)
    maxAbs = 1e-10;

  double maxDiff = 0.0;
  for (long i = 0; i < helib::lsize(v1); i++) {
    double relDiff = std::abs(v1[i] - v2[i]) / maxAbs;
    if (relDiff > maxDiff)
      maxDiff = relDiff;
  }

  return maxDiff;
}

inline bool cx_equals(const std::vector<std::complex<double>>& v1,
                      const std::vector<std::complex<double>>& v2,
                      double epsilon)
{
  return (calcMaxRelDiff(v1, v2) < epsilon);
}

::testing::AssertionResult ciphertextMatches(
    const helib::EncryptedArrayCx& ea,
    const helib::SecKey& sk,
    const std::vector<std::complex<double>>& p,
    const helib::Ctxt& c,
    double epsilon)
{
  std::vector<std::complex<double>> pp;
  ea.decrypt(c, sk, pp);
  if (helib_test::verbose) {
    std::cout << "    relative-error=" << calcMaxRelDiff(p, pp)
              << ", absolute-error=" << calcMaxRelDiff(p, pp) << std::endl;
  }

  if (cx_equals(pp, p, epsilon)) {
    return ::testing::AssertionSuccess();
  } else {
    return ::testing::AssertionFailure()
           << "Ciphertext does not match plaintext:" << std::endl
           << "p = " << helib::vecToStr(p) << std::endl
           << "pp = " << helib::vecToStr(pp) << std::endl;
  }
}

void negateVec(std::vector<std::complex<double>>& p1)
{
  for (auto& x : p1)
    x = -x;
}
void add(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] += from[i];
}
void sub(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] -= from[i];
}
void mul(std::vector<std::complex<double>>& to,
         const std::vector<std::complex<double>>& from)
{
  if (to.size() < from.size())
    to.resize(from.size(), 0);
  for (std::size_t i = 0; i < from.size(); i++)
    to[i] *= from[i];
}
void rotate(std::vector<std::complex<double>>& p, long amt)
{
  long sz = p.size();
  std::vector<std::complex<double>> tmp(sz);
  for (long i = 0; i < sz; i++)
    tmp[((i + amt) % sz + sz) % sz] = p[i];
  p = tmp;
}

class GTestApproxNums : public ::testing::TestWithParam<Parameters>
{
protected:
  const long R;
  const long m;
  const long r;
  const long L;
  const double epsilon;

  helib::Context context;
  helib::SecKey secretKey;
  const helib::PubKey publicKey;
  const helib::EncryptedArrayCx& ea;

  GTestApproxNums() :
      R(GetParam().R),
      m(GetParam().m),
      r(GetParam().r),
      L(GetParam().L),
      epsilon(GetParam().epsilon),
      context(m, /*p=*/-1, r),
      secretKey(
          (context.scale = 4, buildModChain(context, L, /*c=*/2), context)),
      publicKey(
          (secretKey.GenSecKey(), addSome1DMatrices(secretKey), secretKey)),
      ea(context.ea->getCx())
  {}

  virtual void SetUp() override
  {
    if (helib_test::verbose) {
      ea.getPAlgebra().printout();
      std::cout << "r = " << context.alMod.getR() << std::endl;
      std::cout << "ctxtPrimes=" << context.ctxtPrimes
                << ", specialPrimes=" << context.specialPrimes << std::endl
                << std::endl;
    }

    helib::setupDebugGlobals(&secretKey, context.ea);
  }

  virtual void TearDown() override { helib::cleanupDebugGlobals(); }
};

TEST_P(GTestApproxNums, basicArithmeticWorks)
{
  if (helib_test::verbose)
    std::cout << "Test Arithmetic ";
  // Test objects

  helib::Ctxt c1(publicKey), c2(publicKey), c3(publicKey);

  std::vector<std::complex<double>> vd;
  std::vector<std::complex<double>> vd1, vd2, vd3;
  ea.random(vd1);
  ea.random(vd2);

  // test encoding of shorter vectors
  vd1.resize(vd1.size() - 2);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  vd1.resize(vd1.size() + 2, 0.0);

  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  // Test - Multiplication
  c1 *= c2;
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] *= vd2[i];

  NTL::ZZX poly;
  ea.random(vd3);
  ea.encode(poly, vd3, /*size=*/1.0);
  c1.addConstant(poly); // vd1*vd2 + vd3
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] += vd3[i];

  // Test encoding, encryption of a single number
  double xx = NTL::RandomLen_long(16) / double(1L << 16); // random in [0,1]
  ea.encryptOneNum(c2, publicKey, xx);
  c1 += c2;
  for (auto& x : vd1)
    x += xx;

  // Test - Multiply by a mask
  std::vector<long> mask(helib::lsize(vd1), 1);
  for (long i = 0; i * (i + 1) < helib::lsize(mask); i++) {
    mask[i * i] = 0;
    mask[i * (i + 1)] = -1;
  }

  ea.encode(poly, mask, /*size=*/1.0);
  c1.multByConstant(poly); // mask*(vd1*vd2 + vd3)
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] *= mask[i];

  // Test - Addition
  ea.random(vd3);
  ea.encrypt(c3, publicKey, vd3, /*size=*/1.0);
  c1 += c3;
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] += vd3[i];

  c1.negate();
  c1.addConstant(NTL::to_ZZ(1));
  for (long i = 0; i < helib::lsize(vd1); i++)
    vd1[i] = 1.0 - vd1[i];

  // Diff between approxNums HE scheme and plaintext floating
  ea.decrypt(c1, secretKey, vd);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "res=", vd, 10) << std::endl;
  helib::printVec(std::cout << "vec=", vd1, 10) << std::endl;
#endif
  if (helib_test::verbose)
    std::cout << "(max |res-vec|_{infty}=" << calcMaxDiff(vd, vd1) << "): ";

  EXPECT_TRUE(cx_equals(vd, vd1, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd, vd1) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, complexArithmeticWorks)
{
  // Test complex conjugate
  helib::Ctxt c1(publicKey), c2(publicKey);

  std::vector<std::complex<double>> vd;
  std::vector<std::complex<double>> vd1, vd2;
  ea.random(vd1);
  ea.random(vd2);

  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, vd2, /*size=*/1.0);

  if (helib_test::verbose)
    std::cout << "Test Conjugate: ";
  for_each(vd1.begin(), vd1.end(), [](std::complex<double>& d) {
    d = std::conj(d);
  });
  c1.complexConj();
  ea.decrypt(c1, secretKey, vd);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1=", vd1, 10) << std::endl;
  helib::printVec(std::cout << "res=", vd, 10) << std::endl;
#endif
  EXPECT_TRUE(cx_equals(vd, vd1, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd, vd1) << std::endl
      << std::endl;
  ;

  // Test that real and imaginary parts are actually extracted.
  helib::Ctxt realCtxt(c2), imCtxt(c2);
  std::vector<std::complex<double>> realParts(vd2), real_dec;
  std::vector<std::complex<double>> imParts(vd2), im_dec;

  if (helib_test::verbose)
    std::cout << "Test Real and Im parts: ";
  for_each(realParts.begin(), realParts.end(), [](std::complex<double>& d) {
    d = std::real(d);
  });
  for_each(imParts.begin(), imParts.end(), [](std::complex<double>& d) {
    d = std::imag(d);
  });

  ea.extractRealPart(realCtxt);
  ea.decrypt(realCtxt, secretKey, real_dec);

  ea.extractImPart(imCtxt);
  ea.decrypt(imCtxt, secretKey, im_dec);

#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd2=", vd2, 10) << std::endl;
  helib::printVec(std::cout << "real=", realParts, 10) << std::endl;
  helib::printVec(std::cout << "res=", real_dec, 10) << std::endl;
  helib::printVec(std::cout << "im=", imParts, 10) << std::endl;
  helib::printVec(std::cout << "res=", im_dec, 10) << std::endl;
#endif
  EXPECT_TRUE(cx_equals(realParts,
                        real_dec,
                        NTL::conv<double>(epsilon * realCtxt.getPtxtMag())))
      << "  max(re)=" << helib::largestCoeff(realParts)
      << ", max(re1)=" << helib::largestCoeff(real_dec)
      << ", maxDiff=" << calcMaxDiff(realParts, real_dec) << std::endl;
  EXPECT_TRUE(cx_equals(imParts,
                        im_dec,
                        NTL::conv<double>(epsilon * imCtxt.getPtxtMag())))
      << "  max(im)=" << helib::largestCoeff(imParts)
      << ", max(im1)=" << helib::largestCoeff(im_dec)
      << ", maxDiff=" << calcMaxDiff(imParts, im_dec) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, rotatesAndShiftsWork)
{
  std::srand(std::time(0)); // set seed, current time.
  int nplaces = rand() % static_cast<int>(ea.size() / 2.0) + 1;

  if (helib_test::verbose)
    std::cout << "Test Rotation of " << nplaces << ": ";

  helib::Ctxt c1(publicKey);
  std::vector<std::complex<double>> vd1;
  std::vector<std::complex<double>> vd_dec;
  ea.random(vd1);
  ea.encrypt(c1, publicKey, vd1, /*size=*/1.0);

#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1=", vd1, 10) << std::endl;
#endif
  std::rotate(vd1.begin(), vd1.end() - nplaces, vd1.end());
  ea.rotate(c1, nplaces);
  c1.reLinearize();
  ea.decrypt(c1, secretKey, vd_dec);
#ifdef HELIB_DEBUG
  helib::printVec(std::cout << "vd1(rot)=", vd1, 10) << std::endl;
  helib::printVec(std::cout << "res: ", vd_dec, 10) << std::endl;
#endif

  EXPECT_TRUE(
      cx_equals(vd1, vd_dec, NTL::conv<double>(epsilon * c1.getPtxtMag())))
      << "  max(vd)=" << helib::largestCoeff(vd_dec)
      << ", max(vd1)=" << helib::largestCoeff(vd1)
      << ", maxDiff=" << calcMaxDiff(vd_dec, vd1) << std::endl
      << std::endl;
}

TEST_P(GTestApproxNums, generalOpsWorks)
{
  /************** Each round consists of the following:
   1. c1.multiplyBy(c0)
   2. c0 += random constant
   3. c2 *= random constant
   4. tmp = c1
   5. ea.rotate(tmp, random amount in [-nSlots/2, nSlots/2])
   6. c2 += tmp
   7. ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
   8. c1.negate()
   9. c3.multiplyBy(c2)
   10. c0 -= c3
   **************/
  long nslots = ea.size();
  char buffer[32];

  std::vector<std::complex<double>> p0, p1, p2, p3;
  ea.random(p0);
  ea.random(p1);
  ea.random(p2);
  ea.random(p3);

  helib::Ctxt c0(publicKey), c1(publicKey), c2(publicKey), c3(publicKey);
  ea.encrypt(c0, publicKey, p0, /*size=*/1.0);
  ea.encrypt(c1, publicKey, p1, /*size=*/1.0);
  ea.encrypt(c2, publicKey, p2, /*size=*/1.0);
  ea.encrypt(c3, publicKey, p3, /*size=*/1.0);

  helib::resetAllTimers();
  HELIB_NTIMER_START(Circuit);

  for (long i = 0; i < R; i++) {

    if (helib_test::verbose)
      std::cout << "*** round " << i << "..." << std::endl;

    long shamt = NTL::RandomBnd(2 * (nslots / 2) + 1) - (nslots / 2);
    // random number in [-nslots/2..nslots/2]
    long rotamt = NTL::RandomBnd(2 * nslots - 1) - (nslots - 1);
    // random number in [-(nslots-1)..nslots-1]

    // two random constants
    std::vector<std::complex<double>> const1, const2;
    ea.random(const1);
    ea.random(const2);

    NTL::ZZX const1_poly, const2_poly;
    ea.encode(const1_poly, const1, /*size=*/1.0);
    ea.encode(const2_poly, const2, /*size=*/1.0);

    mul(p1, p0); // c1.multiplyBy(c0)
    c1.multiplyBy(c0);
    if (helib_test::verbose) {
      CheckCtxt(c1, "c1*=c0");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1, epsilon));

    add(p0, const1); // c0 += random constant
    c0.addConstant(const1_poly);
    if (helib_test::verbose) {
      CheckCtxt(c0, "c0+=k1");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0, epsilon));

    mul(p2, const2); // c2 *= random constant
    c2.multByConstant(const2_poly);
    if (helib_test::verbose) {
      CheckCtxt(c2, "c2*=k2");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    std::vector<std::complex<double>> tmp_p(p1); // tmp = c1
    helib::Ctxt tmp(c1);
    sprintf(buffer, "tmp=c1>>=%d", (int)shamt);
    rotate(tmp_p,
           shamt); // ea.shift(tmp, random amount in [-nSlots/2,nSlots/2])
    ea.rotate(tmp, shamt);
    if (helib_test::verbose) {
      CheckCtxt(tmp, buffer);
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, tmp_p, tmp, epsilon));

    add(p2, tmp_p); // c2 += tmp
    c2 += tmp;
    if (helib_test::verbose) {
      CheckCtxt(c2, "c2+=tmp");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    sprintf(buffer, "c2>>>=%d", (int)rotamt);
    rotate(p2, rotamt); // ea.rotate(c2, random amount in [1-nSlots, nSlots-1])
    ea.rotate(c2, rotamt);
    if (helib_test::verbose) {
      CheckCtxt(c2, buffer);
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p2, c2, epsilon));

    negateVec(p1); // c1.negate()
    c1.negate();
    if (helib_test::verbose) {
      CheckCtxt(c1, "c1=-c1");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p1, c1, epsilon));

    mul(p3, p2); // c3.multiplyBy(c2)
    c3.multiplyBy(c2);
    if (helib_test::verbose) {
      CheckCtxt(c3, "c3*=c2");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p3, c3, epsilon));

    sub(p0, p3); // c0 -= c3
    c0 -= c3;
    if (helib_test::verbose) {
      CheckCtxt(c0, "c0=-c3");
    }
    EXPECT_TRUE(ciphertextMatches(ea, secretKey, p0, c0, epsilon));
  }

  c0.cleanUp();
  c1.cleanUp();
  c2.cleanUp();
  c3.cleanUp();

  HELIB_NTIMER_STOP(Circuit);

  std::vector<std::complex<double>> pp0, pp1, pp2, pp3;

  ea.decrypt(c0, secretKey, pp0);
  ea.decrypt(c1, secretKey, pp1);
  ea.decrypt(c2, secretKey, pp2);
  ea.decrypt(c3, secretKey, pp3);

  if (helib_test::verbose) {
    std::cout << "Test " << R << " rounds of mixed operations, ";
  }
  EXPECT_TRUE(
      cx_equals(pp0, p0, NTL::conv<double>(epsilon * c0.getPtxtMag())) &&
      cx_equals(pp1, p1, NTL::conv<double>(epsilon * c1.getPtxtMag())) &&
      cx_equals(pp2, p2, NTL::conv<double>(epsilon * c2.getPtxtMag())) &&
      cx_equals(pp3, p3, NTL::conv<double>(epsilon * c3.getPtxtMag())))
      << "  max(p0)=" << helib::largestCoeff(p0)
      << ", max(pp0)=" << helib::largestCoeff(pp0)
      << ", maxDiff=" << calcMaxDiff(p0, pp0) << std::endl
      << "  max(p1)=" << helib::largestCoeff(p1)
      << ", max(pp1)=" << helib::largestCoeff(pp1)
      << ", maxDiff=" << calcMaxDiff(p1, pp1) << std::endl
      << "  max(p2)=" << helib::largestCoeff(p2)
      << ", max(pp2)=" << helib::largestCoeff(pp2)
      << ", maxDiff=" << calcMaxDiff(p2, pp2) << std::endl
      << "  max(p3)=" << helib::largestCoeff(p3)
      << ", max(pp3)=" << helib::largestCoeff(pp3)
      << ", maxDiff=" << calcMaxDiff(p3, pp3) << std::endl
      << std::endl;

  if (helib_test::verbose) {
    std::cout << std::endl;
    helib::printAllTimers();
    std::cout << std::endl;
  }
  helib::resetAllTimers();
}

INSTANTIATE_TEST_SUITE_P(typicalParameters,
                         GTestApproxNums,
                         ::testing::Values(
                             // SLOW
                             Parameters(1, 1024, 8, 150, 0.01)
                             // FAST
                             // Parameters(1, 128, 8, 150, 0.01)
                             ));
// if (R<=0) R=1;
// if (R<=2)
//  L = 100*R;
// else
//  L = 220*(R-1);

} // namespace


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

class StdNormalTestRig : public VectorRealRNGTestRig {
 public:
  /*
   * The default StdNormalTestRig constructor initializes the TestRig with
   * valid and invalid parameters for a random number generator with no
   * arguments.
   */
  StdNormalTestRig() : VectorRealRNGTestRig(10000, 10) {}

  /*
   * This function wraps up the random number generator for testing.
   *
   * The tested rng can have up to three parameters. Any unused parameters can
   * be ignored.
   */
  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1&, const T2&, const T3&, T_rng& rng) const {
    return stan::math::std_normal_rng(rng);
  }

  /*
   * This function builds the quantiles that we will supply to
   * assert_matches_quantiles to test the std_normal_rng
   */
  std::vector<double> generate_quantiles(double, double, double) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::normal_distribution<> dist(0, 1);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsStdNormal, errorCheck) {
  /*
   * This test verifies that std_normal_rng throws errors in the right places.
   *
   * It does so by calling test_rig::generate_samples for all possible
   * combinations of calling arguments.
   */
  check_dist_throws_all_types(StdNormalTestRig());
}

TEST(ProbDistributionsStdNormal, distributionTest) {
  /*
   * This test checks that the std_normal_rng is actually generating numbers
   * from the correct distributions. Quantiles are computed from
   * test_rig::generate_quantiles
   *
   * It does so for all possible combinations of calling arguments.
   */
  check_quantiles_no_params(StdNormalTestRig());
}

TEST(ProbDistributionsStdNormal, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::std_normal_rng(rng));
}

TEST(ProbDistributionsStdNormal, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::std_normal_rng(rng));
  }

  // Generate quantiles from boost's normal distribution
  boost::math::normal_distribution<> dist(0.0, 1.0);
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(quantile(dist, frac));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}


/*

 [begin_description]
 Test case for issue 147
 [end_description]

 Copyright 2011-2015 Karsten Ahnert
 Copyright 2011-2015 Mario Mulansky

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

// disable checked iterator warning for msvc

#include <boost/config.hpp>
#ifdef BOOST_MSVC
    #pragma warning(disable:4996)
#endif

#define BOOST_TEST_MODULE odeint_regression_147

#include <utility>

#include <boost/array.hpp>

#include <boost/test/unit_test.hpp>

#include <boost/mpl/vector.hpp>

#include <boost/numeric/odeint.hpp>

using namespace boost::unit_test;
using namespace boost::numeric::odeint;
namespace mpl = boost::mpl;

typedef double state_type;

void rhs( const state_type &x , state_type &dxdt , const double t )
{
    dxdt = 1;
}

template<class Stepper, class InitStepper>
struct perform_init_test
{
    void operator()( void )
    {
        double t = 0;
        const double dt = 0.1;

        state_type x = 0;

        Stepper stepper;
        InitStepper init_stepper;
        stepper.initialize( init_stepper, rhs, x, t, dt );

        // ab-stepper needs order-1 init steps: t and x should be (order-1)*dt
        BOOST_CHECK_CLOSE( t , (stepper.order()-1)*dt , 1E-16 );
        BOOST_CHECK_CLOSE( x, ( stepper.order() - 1 ) * dt, 2E-14 );
    }
};

typedef mpl::vector<
    euler< state_type > ,
    modified_midpoint< state_type > ,
    runge_kutta4< state_type > ,
    runge_kutta4_classic< state_type > ,
    runge_kutta_cash_karp54_classic< state_type > ,
    runge_kutta_cash_karp54< state_type > ,
    runge_kutta_dopri5< state_type > ,
    runge_kutta_fehlberg78< state_type >
    > runge_kutta_steppers;

BOOST_AUTO_TEST_SUITE( regression_147_test )

BOOST_AUTO_TEST_CASE_TEMPLATE( init_test , InitStepper,
                               runge_kutta_steppers )
{
    perform_init_test< adams_bashforth<4, state_type>, InitStepper > tester;
    tester();
}

BOOST_AUTO_TEST_SUITE_END()