//
// Copyright Jason Rice 2017
// 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)
//
#ifndef NBDL_WEBSOCKET_DETAIL_SEND_HANDSHAKE_RESPONSE_HPP
#define NBDL_WEBSOCKET_DETAIL_SEND_HANDSHAKE_RESPONSE_HPP

#include <nbdl/detail/beast_sha1.hpp>
#include <nbdl/promise.hpp>
#include <nbdl/util/base64_encode.hpp>
#include <nbdl/websocket/detail/get_auth_token.hpp>
#include <nbdl/websocket/detail/parse_handshake_request.hpp>

#include <array>
#include <boost/asio.hpp>
#include <string_view>

namespace nbdl::websocket::detail
{
  namespace asio = boost::asio;
  using asio::ip::tcp;
  using namespace std::string_view_literals;
  using std::string_view;

  inline std::string generate_accept_token(std::string const& key)
  {
    namespace sha1 = nbdl::detail::beast_sha1;
    using nbdl::util::base64_encode;

    constexpr string_view guid = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"sv;
    std::array<unsigned char, 20> digest{};
    sha1::sha1_context ctx{};

    sha1::init(ctx);
    sha1::update(ctx, key.data(), key.length());
    sha1::update(ctx, guid.data(), guid.size());
    sha1::finish(ctx, digest.data());

    return base64_encode(digest);
  }

  struct send_handshake_response_fn
  {
    template <typename Resolver>
    auto operator()(Resolver& resolver, tcp::socket& socket, handshake_info_t const& handshake_info)
    {
      auto const& [websocket_key, cookies] = handshake_info;
      accept_token = generate_accept_token(websocket_key);

      const Buffers buffers{{
        asio::buffer(response_start)
      , asio::buffer(accept_token)
      , asio::buffer(response_end)
      }};

      asio::async_write(socket, buffers, [&, cookies = cookies](std::error_code error_code, std::size_t)
      {
        if (error_code)
        {
          resolver.reject(error_code);
        }
        else
        {
          resolver.resolve(socket, detail::get_auth_token(cookies));
        }
      });
    }

  private:

    using Buffers = std::array<asio::const_buffer, 3>;

    static constexpr string_view response_start = string_view(
      "HTTP/1.1 101 Switching Protocols"
      "\r\n"
      "Upgrade: websocket"
      "\r\n"
      "Connection: Upgrade"
      "\r\n"
      "Sec-WebSocket-Accept: "
    );
    static constexpr string_view response_end = string_view("\r\n\r\n");

    std::string accept_token{};
  };

  constexpr auto send_handshake_response = [] { return nbdl::promise(send_handshake_response_fn{}); };
}

#endif


//
//  statistics.hpp
//  ndnrtc
//
//  Copyright 2013 Regents of the University of California
//  For licensing details see the LICENSE file.
//
//  Author:  Peter Gusev
//  Created: 8/21/13
//

#ifndef ndnrtc_statistics_h
#define ndnrtc_statistics_h

#include <string>
#include <map>
#include <stdexcept>
#include <iostream>
#include <iomanip>

#include <boost/shared_ptr.hpp>

namespace ndnrtc {
    namespace statistics {
        enum class Indicator {
                // general
                Timestamp,  // NDN-RTC timestamp when statistics were captured

                // consumer
                // buffer
                AcquiredNum,                    // PlaybackQueue
                AcquiredKeyNum,                 // PlaybackQueue
                DroppedNum,                     // Buffer
                DroppedKeyNum,                  // Buffer
                AssembledNum,                   // Buffer
                AssembledKeyNum,                // Buffer
                RecoveredNum,                   // VideoPlayout
                RecoveredKeyNum,                // VideoPlayout
                RescuedNum,                     // VideoPlayout
                RescuedKeyNum,                  // VideoPlayout
                IncompleteNum,                  // Buffer
                IncompleteKeyNum,               // Buffer
                BufferTargetSize,               // RemoteStreamImpl
                BufferPlayableSize,             // PlaybackQueue
                BufferReservedSize,             // PlaybackQueue
                CurrentProducerFramerate,       // BufferControl
                VerifySuccess,                  // SampleValidator
                VerifyFailure,                  // SampleValidator
                LatencyControlStable,           // LatencyControl
                LatencyControlCommand,          // LatencyControl
                FrameFetchAvgDelta,             // Buffer
                FrameFetchAvgKey,               // Buffer

                // playout
                LastPlayedNo,                   // VideoPlayout
                LastPlayedDeltaNo,              // VideoPlayout
                LastPlayedKeyNo,                // VideoPlayout
                PlayedNum,                      // VideoPlayout
                PlayedKeyNum,                   // VideoPlayout
                SkippedNum,                     // VideoPlayout
                LatencyEstimated,

                // pipeliner
                SegmentsDeltaAvgNum,            // SampleEstimator
                SegmentsKeyAvgNum,              // SampleEstimator
                SegmentsDeltaParityAvgNum,      // SampleEstimator
                SegmentsKeyParityAvgNum,        // SampleEstimator
                RtxNum,
                RebufferingsNum,                // PipelineControlStateMachine
                RequestedNum,                   // Pipeliner
                RequestedKeyNum,                // Pipeliner
                DW,                             // InterestControl
                W,                              // InterestControl
                SegmentsReceivedNum,            // SegmentController
                TimeoutsNum,                    // SegmentController
                NacksNum,                       // SegmentController
                AppNackNum,                     // SegmentController
                Darr,                           // LatencyControl
                BytesReceived,                  // SegmentController
                RawBytesReceived,               // SegmentController
                State,                          // PipelineControlStateMachine
                DoubleRtFrames,                 // Pipeliner
                DoubleRtFramesKey,              // Pipeliner

                // DRD estimator
                DrdOriginalEstimation,          // BufferControl
                DrdCachedEstimation,            // BufferControl

                // interest queue
                QueueSize,                      // InterestQueue
                InterestsSentNum,               // InterestQueue

                // producer
                //media thread
                BytesPublished,
                RawBytesPublished,
                PublishedSegmentsNum,
                ProcessedNum,
                PublishedNum,
                PublishedKeyNum,
                InterestsReceivedNum,
                SignNum,

                // encoder
                // DroppedNum, // borrowed from buffer (above)
                EncodedNum,

                // capturer
                CapturedNum
        };

        class StatisticsStorage {
        public:
                typedef std::map<Indicator, double> StatRepo;
                static const std::map<Indicator, std::string> IndicatorNames;
                static const std::map<Indicator, std::string> IndicatorKeywords;

                static StatisticsStorage*
                createConsumerStatistics()
                { return new StatisticsStorage(StatisticsStorage::ConsumerStatRepo); }

                static StatisticsStorage*
                createProducerStatistics()
                { return new StatisticsStorage(StatisticsStorage::ProducerStatRepo); }

                StatisticsStorage(const StatisticsStorage& statisticsStorage):
                inidicatorNames_(StatisticsStorage::IndicatorNames),
                indicators_(statisticsStorage.getIndicators()){}
                ~StatisticsStorage(){}

                // may throw an exception if indicator is not present in the repo
                void
                updateIndicator(const statistics::Indicator& indicator,
                                const double& value) throw(std::out_of_range);

                StatRepo
                getIndicators() const;

                StatisticsStorage&
                operator=(const StatisticsStorage& other)
                {
                    indicators_ = other.getIndicators();
                    return *this;
                }

                double&
                operator[](const statistics::Indicator& indicator)
                { return indicators_.at(indicator); }

                friend std::ostream& operator<<(std::ostream& os,
                                                const StatisticsStorage& storage)
                {
                    for (auto& it:storage.indicators_)
                    {
                        try {
                            os << std::fixed
                            << storage.inidicatorNames_.at(it.first) << "\t"
                            << std::setprecision(2) << it.second << std::endl;
                        }
                        catch (...) {
                        }
                    }

                    return os;
                }
        private:
                StatisticsStorage(const StatRepo& indicators):indicators_(indicators){}

                const std::map<Indicator, std::string> inidicatorNames_;
                static const StatRepo ConsumerStatRepo;
                static const StatRepo ProducerStatRepo;
                StatRepo indicators_;
        };

        class StatObject {
        public:
            StatObject(const boost::shared_ptr<StatisticsStorage>& statStorage):statStorage_(statStorage){}

            virtual ~StatObject(){}

        protected:
            boost::shared_ptr<StatisticsStorage> statStorage_;
        };
    };
}

#endif


#pragma once

#include <memory>
#include <vector>
#include <Eigen\Dense>

namespace dynaman {
	class arfModelLinearBase
	{
	public:
		~arfModelLinearBase();
		virtual Eigen::MatrixXf arf(const Eigen::MatrixXf& posRel, const Eigen::MatrixXf& eulerAnglesAUTD) = 0;
		virtual Eigen::MatrixXf arf(const Eigen::MatrixXf& posRel, const std::vector<Eigen::Matrix3f>& rots) = 0;
	};

	class arfModelTabular : public arfModelLinearBase
	{
	private:
		Eigen::VectorXf m_tableDistance;
		Eigen::VectorXf m_tableAngle;
		Eigen::MatrixXf m_tableForce;

		Eigen::MatrixXf arfFromDirections(const Eigen::MatrixXf& posRel, const Eigen::MatrixXf& directionsAutd);

	public:
		arfModelTabular(const Eigen::VectorXf& tableDistance, const Eigen::VectorXf& tableAngle, const Eigen::MatrixXf& tableForce);
		Eigen::MatrixXf arf(const Eigen::MatrixXf& posRel, const Eigen::MatrixXf& eulerAnglesAutd) override;
		Eigen::MatrixXf arf(const Eigen::MatrixXf& posRel, const std::vector<Eigen::Matrix3f>& rots) override;
	};

	class arfModelFocusSphereR100 : public arfModelTabular
	{
	public:
		arfModelFocusSphereR100();

	private:
		Eigen::VectorXf tableDistances() const;
		Eigen::VectorXf tableAngles() const;
		Eigen::MatrixXf tableForces() const;
	};

	class arfModelFocusSphereR50 : public arfModelTabular {
	public:
		arfModelFocusSphereR50();

	private:
		Eigen::VectorXf tableDistances() const;
		Eigen::VectorXf tableAngles() const;
		Eigen::MatrixXf tableForces() const;
	};
}


#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>

#include <iostream>
#include <thread>
#include <vector>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/filters/voxel_grid.h>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
// PCL specific includes
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

ros::Publisher pub;

void
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
  pcl::fromROSMsg (*input, *cloud);

  std::cout << "test: " << cloud->points[4]  << std::endl;
  std::cout << "test2: " << cloud->points[4].rgb << cloud->points[4].r << cloud->points[4].g << cloud->points[4].b << std::endl;

  pcl::search::Search <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud <pcl::PointXYZRGB>);
  // Perform the actual filtering
  pcl::VoxelGrid<pcl::PointXYZRGB> sor;
  sor.setInputCloud (cloud);
  float voxel_size = 0.04;
  sor.setMinimumPointsNumberPerVoxel(3);
  sor.setLeafSize (voxel_size, voxel_size, voxel_size);
  sor.filter (*cloud_filtered);

  pcl::IndicesPtr indices (new std::vector <int>);
  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud (cloud_filtered);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 5.0);
  pass.filter (*indices);

  pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
  reg.setInputCloud (cloud_filtered);
  reg.setIndices (indices);
  reg.setSearchMethod (tree);
  reg.setDistanceThreshold (0.06);
  reg.setPointColorThreshold (6);
  reg.setRegionColorThreshold (2);
  reg.setMinClusterSize (60);
  reg.setMaxClusterSize (5000);

  std::vector <pcl::PointIndices> clusters;
  reg.extract (clusters);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();

  sensor_msgs::PointCloud2 output;

  pcl::toROSMsg(*colored_cloud,output);
  output.header.frame_id = input->header.frame_id;

  // Publish the data
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "segment_rgb");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/camera/depth/color/points", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("output_rgb", 1);

  // Spin
  ros::spin ();
}

/*
typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

void callback(const PointCloud::ConstPtr& msg)
{
  printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
//  BOOST_FOREACH (cloud, msg->points);

//  pcl::visualization::CloudViewer viewer ("Cluster viewer");
//  viewer.showCloud (cloud);

//  pcl::search::Search <pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);

//  pcl::IndicesPtr indices (new std::vector <int>);
//  pcl::PassThrough<pcl::PointXYZRGB> pass;
//  pass.setInputCloud (cloud);
//  pass.setFilterFieldName ("z");
//  pass.setFilterLimits (0.0, 5.0);
//  pass.filter (*indices);

//  pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
//  reg.setInputCloud (cloud);
//  reg.setIndices (indices);
//  reg.setSearchMethod (tree);
//  reg.setDistanceThreshold (10);
//  reg.setPointColorThreshold (6);
//  reg.setRegionColorThreshold (5);
//  reg.setMinClusterSize (600);

//  std::vector <pcl::PointIndices> clusters;
//  reg.extract (clusters);

//  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
//  pcl::visualization::CloudViewer viewer ("Cluster viewer");
//  viewer.showCloud (colored_cloud);

}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
  ros::spin();
}
*/


#include <iostream>
#include <openssl/sha.h>
#include "patternsearch.h"
#include <openssl/aes.h>
#include "main.h"
#include <openssl/evp.h>
#include <boost/thread.hpp>
#include "util.h"

namespace patternsearch
{
	#define PSUEDORANDOM_DATA_SIZE 30 //2^30 = 1GB
    #define PSUEDORANDOM_DATA_CHUNK_SIZE 6 //2^6 = 64 bytes //must be same as SHA512_DIGEST_LENGTH 64
    #define L2CACHE_TARGET 12 // 2^12 = 4096 bytes
    #define AES_ITERATIONS 15

	// useful constants
    uint32_t psuedoRandomDataSize=(1<<PSUEDORANDOM_DATA_SIZE); //2^30 = 1GB
    uint32_t cacheMemorySize = (1<<L2CACHE_TARGET); //2^12 = 4096 bytes
    uint32_t chunks=(1<<(PSUEDORANDOM_DATA_SIZE-PSUEDORANDOM_DATA_CHUNK_SIZE)); //2^(30-6) = 16 mil
    uint32_t chunkSize=(1<<(PSUEDORANDOM_DATA_CHUNK_SIZE)); //2^6 = 64 bytes
    uint32_t comparisonSize=(1<<(PSUEDORANDOM_DATA_SIZE-L2CACHE_TARGET)); //2^(30-12) = 256K

	void static SHA512Filler(char *mainMemoryPsuedoRandomData, int threadNumber, int totalThreads,uint256 midHash){
		//Generate psuedo random data to store in main memory
		unsigned char hash_tmp[sizeof(midHash)];
		memcpy((char*)&hash_tmp[0], (char*)&midHash, sizeof(midHash) );
		uint32_t* index = (uint32_t*)hash_tmp;
		uint32_t chunksToProcess=chunks/totalThreads;
		uint32_t startChunk=threadNumber*chunksToProcess;
		for( uint32_t i = startChunk; i < startChunk+chunksToProcess;  i++){
            //This changes the first character of hash_tmp
			*index = i;
            SHA512((unsigned char*)hash_tmp, sizeof(hash_tmp), (unsigned char*)&(mainMemoryPsuedoRandomData[i*chunkSize]));
		}
	}

	void static aesSearch(char *mainMemoryPsuedoRandomData, int threadNumber, int totalThreads, std::vector< std::pair<uint32_t,uint32_t> > *results, boost::mutex *mtx){
		//Allocate temporary memory
		unsigned char *cacheMemoryOperatingData;
		unsigned char *cacheMemoryOperatingData2;
		cacheMemoryOperatingData=new unsigned char[cacheMemorySize+16];
		cacheMemoryOperatingData2=new unsigned char[cacheMemorySize];

		//Create references to data as 32 bit arrays
		uint32_t* cacheMemoryOperatingData32 = (uint32_t*)cacheMemoryOperatingData;
		uint32_t* cacheMemoryOperatingData322 = (uint32_t*)cacheMemoryOperatingData2;
        //uint32_t* mainMemoryPsuedoRandomData32 = (uint32_t*)mainMemoryPsuedoRandomData;

		//Search for pattern in psuedorandom data

		unsigned char key[32] = {0};
		unsigned char iv[AES_BLOCK_SIZE];
		int outlen1, outlen2;
        unsigned int useEVP = GetArg("-useevp", 1);

		//Iterate over the data
		int searchNumber=comparisonSize/totalThreads;
		int startLoc=threadNumber*searchNumber;
		for(uint32_t k=startLoc;k<startLoc+searchNumber;k++){

            //copy data to first l2 cache
			memcpy((char*)&cacheMemoryOperatingData[0], (char*)&mainMemoryPsuedoRandomData[k*cacheMemorySize], cacheMemorySize);

			for(int j=0;j<AES_ITERATIONS;j++){

                //use last 4 bytes of first cache as next location
				uint32_t nextLocation = cacheMemoryOperatingData32[(cacheMemorySize/4)-1]%comparisonSize;

				//Copy data from indicated location to second l2 cache -
				memcpy((char*)&cacheMemoryOperatingData2[0], (char*)&mainMemoryPsuedoRandomData[nextLocation*cacheMemorySize], cacheMemorySize);

				//XOR location data into second cache
				for(uint32_t i = 0; i < cacheMemorySize/4; i++){
					cacheMemoryOperatingData322[i] = cacheMemoryOperatingData32[i] ^ cacheMemoryOperatingData322[i];
				}

				//AES Encrypt using last 256bits of Xorred value as key
				//AES_set_encrypt_key((unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-32], 256, &AESkey);

				//Use last X bits as initial vector

				//AES CBC encrypt data in cache 2, place it into cache 1, ready for the next round
				//AES_cbc_encrypt((unsigned char*)&cacheMemoryOperatingData2[0], (unsigned char*)&cacheMemoryOperatingData[0], cacheMemorySize, &AESkey, iv, AES_ENCRYPT);
				if(useEVP){
			EVP_CIPHER_CTX          *ctx = EVP_CIPHER_CTX_new();
					memcpy(key,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-32],32);
					memcpy(iv,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-AES_BLOCK_SIZE],AES_BLOCK_SIZE);
					EVP_EncryptInit(ctx, EVP_aes_256_cbc(), key, iv);
					EVP_EncryptUpdate(ctx, cacheMemoryOperatingData, &outlen1, cacheMemoryOperatingData2, cacheMemorySize);
					EVP_EncryptFinal(ctx, cacheMemoryOperatingData + outlen1, &outlen2);
					EVP_CIPHER_CTX_cleanup(ctx);
				}else{
					AES_KEY AESkey;
					AES_set_encrypt_key((unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-32], 256, &AESkey);
					memcpy(iv,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-AES_BLOCK_SIZE],AES_BLOCK_SIZE);
					AES_cbc_encrypt((unsigned char*)&cacheMemoryOperatingData2[0], (unsigned char*)&cacheMemoryOperatingData[0], cacheMemorySize, &AESkey, iv, AES_ENCRYPT);
				}
			}

			//use last X bits as solution
			uint32_t solution=cacheMemoryOperatingData32[(cacheMemorySize/4)-1]%comparisonSize;
            if(solution<1000){
				uint32_t proofOfCalculation=cacheMemoryOperatingData32[(cacheMemorySize/4)-2];
                //LogPrintf("found solution - %d / %u / %u\n",k,solution,proofOfCalculation);
				boost::mutex::scoped_lock lck(*mtx);
				(*results).push_back( std::make_pair( k, proofOfCalculation ) );
			}
		}

		//free memory
		delete [] cacheMemoryOperatingData;
		delete [] cacheMemoryOperatingData2;
	}

    std::vector< std::pair<uint32_t,uint32_t> > pattern_search( uint256 midHash, char *mainMemoryPsuedoRandomData, int totalThreads){

            boost::this_thread::disable_interruption di;

		std::vector< std::pair<uint32_t,uint32_t> > results;

        //clock_t t1 = clock();
		boost::thread_group* sha512Threads = new boost::thread_group();
		char *threadsComplete;
		threadsComplete=new char[totalThreads];
		for (int i = 0; i < totalThreads; i++){
			sha512Threads->create_thread(boost::bind(&SHA512Filler, mainMemoryPsuedoRandomData, i,totalThreads,midHash));
		}
		//Wait for all threads to complete
		sha512Threads->join_all();

        //clock_t t2 = clock();
        //LogPrintf("create sha512 data %d\n",((double)t2-(double)t1)/CLOCKS_PER_SEC);

		boost::mutex mtx;
		boost::thread_group* aesThreads = new boost::thread_group();
		threadsComplete=new char[totalThreads];
		for (int i = 0; i < totalThreads; i++){
			aesThreads->create_thread(boost::bind(&aesSearch, mainMemoryPsuedoRandomData, i,totalThreads,&results, &mtx));
		}
		//Wait for all threads to complete
		aesThreads->join_all();

        //clock_t t3 = clock();
        //LogPrintf("aes search %d\n",((double)t3-(double)t2)/CLOCKS_PER_SEC);

		delete aesThreads;
		delete sha512Threads;
            boost::this_thread::restore_interruption ri(di);
		return results;
	}

    bool pattern_verify( uint256 midHash, uint32_t a, uint32_t b ){
		//return false;

		clock_t t1 = clock();

		//Basic check
        if( a >= comparisonSize ) return false;

		//Allocate memory required
		unsigned char *cacheMemoryOperatingData;
		unsigned char *cacheMemoryOperatingData2;
		cacheMemoryOperatingData=new unsigned char[cacheMemorySize+16];
		cacheMemoryOperatingData2=new unsigned char[cacheMemorySize];
		uint32_t* cacheMemoryOperatingData32 = (uint32_t*)cacheMemoryOperatingData;
		uint32_t* cacheMemoryOperatingData322 = (uint32_t*)cacheMemoryOperatingData2;

		unsigned char  hash_tmp[sizeof(midHash)];
		memcpy((char*)&hash_tmp[0], (char*)&midHash, sizeof(midHash) );
		uint32_t* index = (uint32_t*)hash_tmp;

		uint32_t startLocation=a*cacheMemorySize/chunkSize;
		uint32_t finishLocation=startLocation+(cacheMemorySize/chunkSize);

        //copy data to first l2 cache
		for( uint32_t i = startLocation; i <  finishLocation;  i++){
			*index = i;
			SHA512((unsigned char*)hash_tmp, sizeof(hash_tmp), (unsigned char*)&(cacheMemoryOperatingData[(i-startLocation)*chunkSize]));
		}

        unsigned int useEVP = GetArg("-useevp", 1);

        //allow override for AESNI testing
        /*if(midHash==0){
            useEVP=0;
        }else if(midHash==1){
            useEVP=1;
        }*/

        unsigned char key[32] = {0};
		unsigned char iv[AES_BLOCK_SIZE];
		int outlen1, outlen2;

		//memset(cacheMemoryOperatingData2,0,cacheMemorySize);
		for(int j=0;j<AES_ITERATIONS;j++){

			//use last 4 bits as next location
			startLocation = (cacheMemoryOperatingData32[(cacheMemorySize/4)-1]%comparisonSize)*cacheMemorySize/chunkSize;
			finishLocation=startLocation+(cacheMemorySize/chunkSize);
			for( uint32_t i = startLocation; i <  finishLocation;  i++){
				*index = i;
				SHA512((unsigned char*)hash_tmp, sizeof(hash_tmp), (unsigned char*)&(cacheMemoryOperatingData2[(i-startLocation)*chunkSize]));
			}

			//XOR location data into second cache
			for(uint32_t i = 0; i < cacheMemorySize/4; i++){
				cacheMemoryOperatingData322[i] = cacheMemoryOperatingData32[i] ^ cacheMemoryOperatingData322[i];
			}

			//AES Encrypt using last 256bits as key

			if(useEVP){
EVP_CIPHER_CTX          *ctx = EVP_CIPHER_CTX_new();
				memcpy(key,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-32],32);
				memcpy(iv,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-AES_BLOCK_SIZE],AES_BLOCK_SIZE);
				EVP_EncryptInit(ctx, EVP_aes_256_cbc(), key, iv);
				EVP_EncryptUpdate(ctx, cacheMemoryOperatingData, &outlen1, cacheMemoryOperatingData2, cacheMemorySize);
				EVP_EncryptFinal(ctx, cacheMemoryOperatingData + outlen1, &outlen2);
				EVP_CIPHER_CTX_cleanup(ctx);
			}else{
				AES_KEY AESkey;
				AES_set_encrypt_key((unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-32], 256, &AESkey);
				memcpy(iv,(unsigned char*)&cacheMemoryOperatingData2[cacheMemorySize-AES_BLOCK_SIZE],AES_BLOCK_SIZE);
				AES_cbc_encrypt((unsigned char*)&cacheMemoryOperatingData2[0], (unsigned char*)&cacheMemoryOperatingData[0], cacheMemorySize, &AESkey, iv, AES_ENCRYPT);
			}

		}

		//use last X bits as solution
		uint32_t solution=cacheMemoryOperatingData32[(cacheMemorySize/4)-1]%comparisonSize;
		uint32_t proofOfCalculation=cacheMemoryOperatingData32[(cacheMemorySize/4)-2];
        //LogPrintf("verify solution - %d / %u / %u\n",a,solution,proofOfCalculation);

		//free memory
		delete [] cacheMemoryOperatingData;
		delete [] cacheMemoryOperatingData2;

        clock_t t2 = clock();
        //LogPrintf("verify %d\n",((double)t2-(double)t1)/CLOCKS_PER_SEC);

        if(solution<1000 && proofOfCalculation==b){
			return true;
		}

		return false;

	}
}


#include <stdio.h>
#include <string>
#include <map>
#include <fstream>
#include "llvm/Support/raw_ostream.h"
#include <cstdio>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <array>
#include <regex>
#include "helper.h"
#include "klee/ExecutionState.h"
#include "Executor.h"
#include <cmath>
#include <math.h>
#include <vector>
#include <boost/lambda/lambda.hpp>
#include <boost/math/distributions/beta.hpp>
#include "TeleScopeHandler.h"

using namespace klee;
using namespace std;
using namespace llvm;

//==============================================
//============= TeleScopeHandler ===============
//==============================================

TeleScopeHandler::TeleScopeHandler(int n)
{
   numLoop = n;
}

int TeleScopeHandler::ProcessAllPaths(vector<ExecutionState *> states)
{
   LOG(LOG_MASK_TS, "Telescoping done, final result is %g",
       TeleScopeData::finalResult / numLoop);
   return 0;
}

int TeleScopeHandler::ts_init_handler(ExecutionState *state,
                                      int small_thres, int real_thres)
{
   state->tsData.ts_init(state, small_thres, real_thres);
   return 0;
}

int TeleScopeHandler::ts_pkt_handler(ExecutionState *state,
                                      int pktID, int cur_reg)
{
   assert(state == state->tsData.getState());
   state->tsData.ts_pkt(pktID, cur_reg);
   return 0;
}

int TeleScopeHandler::ts_handler(ExecutionState *state, int cur_reg)
{
   assert(state == state->tsData.getState());
   state->tsData.ts_final(cur_reg);
   return 0;
}

int TeleScopeHandler::ts_cmin_pkt_handler(ExecutionState *state,
                                          int pktID)
{
   assert(state == state->tsData.getState());
   int cmin_max = state->cmin->getMaxValue();
   state->tsData.ts_pkt(pktID, cmin_max);
   return 0;
}

int TeleScopeHandler::ts_cmin_handler(ExecutionState *state)
{
   assert(state == state->tsData.getState());
   int cmin_max = state->cmin->getMaxValue();
   state->tsData.ts_final(cmin_max);
   return 0;
}

//==============================================
//============= TeleScopeData ==================
//==============================================

double TeleScopeData::finalResult = 0.0;

TeleScopeData::TeleScopeData():
   reg(0),
   smallThres(0),
   realThres(0),
   pktProbs(0)
{}

TeleScopeData::TeleScopeData(const TeleScopeData &_tsData):
   state(_tsData.state),
   reg(_tsData.reg),
   smallThres(_tsData.smallThres),
   realThres(_tsData.realThres),
   perPktProbs(_tsData.perPktProbs),
   pktProbs(_tsData.pktProbs),
   pktRegs(_tsData.pktRegs)
{

}

void TeleScopeData::dump()
{
   uint32_t i;
   LOG(LOG_MASK_TS, "state %d has %ld pkt probs", state->id, perPktProbs.size());
   for (i = 0; i < pktProbs.size(); i ++) {
      LOG(LOG_MASK_TS, "pkt[%d] prob=%g", i, perPktProbs[i])
   }

   LOG(LOG_MASK_TS, "state %d has %ld pkt regs", state->id, pktRegs.size());
   for (i = 0; i < pktRegs.size(); i ++) {
      LOG(LOG_MASK_TS, "pkt[%d] reg=%d", i, pktRegs[i])
   }
}

int TeleScopeData::ts_init(ExecutionState *s, int small_thres, int real_thres)
{
   LOG(LOG_MASK_TS, "Initing TsData, small thres=%d, real thres=%d",
       small_thres, real_thres);
   state = s;
   smallThres = small_thres;
   realThres  = real_thres;
   return 0;
}

int TeleScopeData::ts_pkt(int pktID, int cur_reg)
{
   //LOG(LOG_MASK_TS, "Updating TsData for pkt=%d, pathProb=%g, reg: %d->%d",
   //    pktID, state->getPathProb(), reg, cur_reg);

   if (int(pktRegs.size()) != pktID) {
      WARN("pktregs size=%ld, pktID=%d\n", pktRegs.size(), pktID);
      assert(0);
   }
   assert(int(pktRegs.size()) == pktID);
   assert(int(perPktProbs.size()) == pktID);
   assert(int(pktProbs.size()) == pktID);

   reg = cur_reg;

   // TODO; Update per-packet constraints and probs
   double pathProb = state->getPathProb();
   pktProbs.push_back(pathProb);
   if (pktID == 0) {
      perPktProbs.push_back(pathProb);
   } else {
      double lastProb = pktProbs[pktID - 1];
      perPktProbs.push_back(pathProb / lastProb);
   }
   pktRegs.push_back(cur_reg);

   return 0;
}

bool AlmostEqual2sComplement(float A, float B, int maxUlps)
{
    // Make sure maxUlps is non-negative and small enough that the
    // default NAN won't compare as equal to anything.
    assert(maxUlps > 0 && maxUlps < 4 * 1024 * 1024);
    int aInt = *(int*)&A;
    // Make aInt lexicographically ordered as a twos-complement int
    if (aInt < 0)
        aInt = 0x80000000 - aInt;
    // Make bInt lexicographically ordered as a twos-complement int
    int bInt = *(int*)&B;
    if (bInt < 0)
        bInt = 0x80000000 - bInt;
    int intDiff = abs(aInt - bInt);
    if (intDiff <= maxUlps)
        return true;
    return false;
}

static int checkVectorSame(vector<double> v)
{
   int ret = 0;
   double first = v[0];
   for (auto val: v) {
      if (!AlmostEqual2sComplement(val, first, 1)) {
         ret = 1;
      }
   }
   if (ret == 0) {
      return 0;
   }

   assert(ret == 1);
   double second = v[1];
   for (uint32_t i = 2; i < v.size(); i ++) {
      if (!AlmostEqual2sComplement(v[i], second, 1)) {
         ret = 2;
      }
   }
   return ret;
}

TeleScopeData::PeriodicityType TeleScopeData::ts_periodicity()
{
   // TODO: for now, we only distinguish two types by comparing
   // TODO: Also check per-packet constraint for periodicity

   int ret = checkVectorSame(perPktProbs);
   if (ret == 0) {
      return REPEAT_ALL;
   } else if (ret == 1) {
      return REPEAT_FROM_SECOND;
   } else {
      return NO_PERIODICITY;
   }
}

int TeleScopeData::ts_final(int cur_reg)
{
   assert(reg == cur_reg);
   //LOG(LOG_MASK_TS, "Telescoping state %d with reg=%d, smallThres=%d, real=%d",
   //    state->id, reg, smallThres, realThres);

   // check if this state is telescopable
   if (reg == smallThres) {
      LOG(LOG_MASK_TS, "state %d (cur_reg = smallThres = %d) "
          "seems telescopable", state->id, reg);
      dump();

      // infer the final prob result
      double result = 0.0;
      TeleScopeData::PeriodicityType ptype = ts_periodicity();

      if (ptype == NO_PERIODICITY) {
         LOG(LOG_MASK_TS, "state %d turns out not telescopable", state->id);
      } else {
         if (ptype == REPEAT_ALL) {
            result = pow(perPktProbs[0], realThres);
         } else if (ptype == REPEAT_FROM_SECOND) {
            result = perPktProbs[0] * pow(perPktProbs[1], realThres-1);
         }
         LOG(LOG_MASK_TS, "telescoping state %d inferred prob of reg==%d is %g",
             state->id, realThres, result);
         finalResult += result;
      }
   } else {
      //LOG(LOG_MASK_TS, "state %d (cur_reg=%d smallThres=%d) "
      //    "seems not telescopable", state->id, cur_reg, smallThres);
   }

   return 0;
}


//==============================================================================
//         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 BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_SSE_SSE2_IS_EQZ_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_SIMD_SSE_SSE2_IS_EQZ_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_SSE2_SUPPORT
#include <boost/simd/predicates/functions/is_eqz.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/sdk/meta/as_logical.hpp>
#include <boost/simd/swar/functions/details/shuffle.hpp>
#include <boost/dispatch/meta/downgrade.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( is_eqz_
                                    , boost::simd::tag::sse2_
                                    , (A0)
                                    , ((simd_ < int64_<A0>
                                              , boost::simd::tag::sse_
                                              >
                                      ))
                                    )
  {
    typedef typename meta::as_logical<A0>::type result_type;

    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::downgrade<A0>::type          base;

      const base tmp1 = boost::simd::bitwise_cast<base>(is_eqz(boost::simd::bitwise_cast<base>(a0)));
      const base tmp2 = details::shuffle<1,0,3,2>(tmp1);
      return boost::simd::bitwise_cast<result_type>(b_and(tmp1, tmp2));
    }
  };
} } }

#endif
#endif


/* methods.cpp */

#include "linalg.h"
#include "methods.h"
#include "structs.h"

#include <Python.h>
#include <math.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/QR>

extern "C"
{
    void _ETS_IK(PyObject *ets, int n, double *q, double *Tep, double *ret)
    {
        // double E;
        double *Te = (double *)PyMem_RawCalloc(16, sizeof(double));
        double *e = (double *)PyMem_RawCalloc(6, sizeof(double));

        double *a = (double *)PyMem_RawCalloc(6, sizeof(double));
        // a[0] = 1.0;
        // a[1] = 4.0;
        // a[2] = 2.0;
        // a[3] = 5.0;
        // a[4] = 3.0;
        // a[5] = 6.0;
        a[0] = 1.0;
        a[1] = 2.0;
        a[2] = 3.0;
        a[3] = 4.0;
        a[4] = 5.0;
        a[5] = 6.0;
        double *b = (double *)PyMem_RawCalloc(12, sizeof(double));
        b[0] = 11.0;
        b[1] = 15.0;
        b[2] = 19.0;
        b[3] = 12.0;
        b[4] = 16.0;
        b[5] = 20.0;
        b[6] = 13.0;
        b[7] = 17.0;
        b[8] = 21.0;
        b[9] = 14.0;
        b[10] = 18.0;
        b[11] = 22.0;
        // b[0] = 11.0;
        // b[1] = 12.0;
        // b[2] = 13.0;
        // b[3] = 14.0;
        // b[4] = 15.0;
        // b[5] = 16.0;
        // b[6] = 17.0;
        // b[7] = 18.0;
        // b[8] = 19.0;
        // b[9] = 20.0;
        // b[10] = 21.0;
        // b[11] = 22.0;

        // double *U = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *invU = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *temp = (double *)PyMem_RawCalloc(16, sizeof(double));
        // double *ret = (double *)PyMem_RawCalloc(16, sizeof(double));
        // Py_ssize_t m;
        int arrived = 0, iter = 0;

        while (arrived == 0 && iter < 500)
        {
            // Current pose Te
            // _ETS_fkine(ets, q, (double *)NULL, NULL, Te);

            // Angle axis error e
            _angle_axis(Te, Tep, e);

            // Squared error E
            // E = 0.5 * e @ We @ e
            // E = 0.5 * (e[0] * e[0] + e[1] * e[1] + e[2] * e[2] + e[3] * e[3] + e[4] * e[4] + e[5] * e[5]);
        }

        // _ETS_fkine(ets, q, (double *)NULL, NULL, Te);

        // for (int i = 0; i < 2; i++)
        // {
        //     for (int j = 0; j < 4; j++)
        //     {
        //         ret[i * 4 + j] = 0.0;
        //     }
        // }

        // _mult_T(2, 3, 0, a, 3, 4, 0, b, ret);
        // _mult_T(3, 2, 1, a, 3, 4, 0, b, ret);
        // _mult_T(3, 2, 1, a, 4, 3, 1, b, ret);
        // _mult_T(2, 3, 0, a, 4, 3, 1, b, ret);

        // int j = 0;
    }

    void _ETS_hessian(int n, MapMatrixJc &J, MapMatrixHr &H)
    {
        for (int j = 0; j < n; j++)
        {
            for (int i = j; i < n; i++)
            {
                H.block<3, 1>(j * 6, i) = J.block<3, 1>(3, j).cross(J.block<3, 1>(0, i));
                H.block<3, 1>(j * 6 + 3, i) = J.block<3, 1>(3, j).cross(J.block<3, 1>(3, i));

                if (i != j)
                {
                    H.block<3, 1>(i * 6, j) = H.block<3, 1>(j * 6, i);
                    H.block<3, 1>(i * 6 + 3, j) = Eigen::Vector3d::Zero();
                }
            }
        }
    }

    void _ETS_jacob0(ETS *ets, double *q, double *tool, MapMatrixJc &eJ)
    {
        // ET *et;
        // double T[16];
        // MapMatrix4dc eT(T);
        // Matrix4dc U;
        // Matrix4dc invU;
        // Matrix4dc temp;
        // Matrix4dc ret;

        // int j = 0;

        // U = Eigen::Matrix4d::Identity();

        // // Get the forward  kinematics into T
        // _ETS_fkine(ets, q, (double *)NULL, tool, eT);

        // for (int i = 0; i < ets->m; i++)
        // {
        //     et = ets->ets[i];

        //     if (et->isjoint)
        //     {
        //         _ET_T(et, &ret(0), q[et->jindex]);
        //         temp = U * ret;
        //         U = temp;

        //         if (i == ets->m - 1 && tool != NULL)
        //         {
        //             MapMatrix4dc e_tool(tool);
        //             temp = U * e_tool;
        //             U = temp;
        //         }

        //         _inv(&U(0), &invU(0));
        //         temp = invU * eT;

        //         if (et->axis == 0)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 2) * temp(1, 3) - U(Eigen::seq(0, 2), 1) * temp(2, 3);

        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 0);
        //         }
        //         else if (et->axis == 1)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 0) * temp(2, 3) - U(Eigen::seq(0, 2), 2) * temp(0, 3);
        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 1);
        //         }
        //         else if (et->axis == 2)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 1) * temp(0, 3) - U(Eigen::seq(0, 2), 0) * temp(1, 3);
        //             eJ(Eigen::seq(3, 5), j) = U(Eigen::seq(0, 2), 2);
        //         }
        //         else if (et->axis == 3)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 0);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         else if (et->axis == 4)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 1);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         else if (et->axis == 5)
        //         {
        //             eJ(Eigen::seq(0, 2), j) = U(Eigen::seq(0, 2), 2);
        //             eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
        //         }
        //         j++;
        //     }
        //     else
        //     {
        //         _ET_T(et, &ret(0), q[et->jindex]);
        //         temp = U * ret;
        //         U = temp;
        //     }
        // }

        ET *et;
        Eigen::Matrix<double, 6, Eigen::Dynamic> tJ(6, ets->n);
        double T[16];
        MapMatrix4dc eT(T);
        Matrix4dc U = Eigen::Matrix4d::Identity();
        Matrix4dc invU;
        Matrix4dc temp;
        Matrix4dc ret;
        int j = ets->n - 1;

        if (tool != NULL)
        {
            Matrix4dc e_tool(tool);
            temp = e_tool * U;
            U = temp;
        }

        for (int i = ets->m - 1; i >= 0; i--)
        {
            et = ets->ets[i];

            if (et->isjoint)
            {
                if (et->axis == 0)
                {
                    tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3);
                    tJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2));
                }
                else if (et->axis == 1)
                {
                    tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3);
                    tJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2));
                }
                else if (et->axis == 2)
                {
                    tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3);
                    tJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2));
                }
                else if (et->axis == 3)
                {
                    tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 4)
                {
                    tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 5)
                {
                    tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2));
                    tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }

                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
                j--;
            }
            else
            {
                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
            }
        }

        Eigen::Matrix<double, 6, 6> ev;
        ev.topLeftCorner<3, 3>() = U.topLeftCorner<3, 3>();
        ev.topRightCorner<3, 3>() = Eigen::Matrix3d::Zero();
        ev.bottomLeftCorner<3, 3>() = Eigen::Matrix3d::Zero();
        ev.bottomRightCorner<3, 3>() = U.topLeftCorner<3, 3>();
        eJ = ev * tJ;
    }

    void _ETS_jacobe(ETS *ets, double *q, double *tool, MapMatrixJc &eJ)
    {
        ET *et;
        double T[16];
        MapMatrix4dc eT(T);
        Matrix4dc U = Eigen::Matrix4d::Identity();
        Matrix4dc invU;
        Matrix4dc temp;
        Matrix4dc ret;
        int j = ets->n - 1;

        if (tool != NULL)
        {
            Matrix4dc e_tool(tool);
            temp = e_tool * U;
            U = temp;
        }

        for (int i = ets->m - 1; i >= 0; i--)
        {
            et = ets->ets[i];

            if (et->isjoint)
            {
                if (et->axis == 0)
                {
                    eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3);
                    eJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2));
                }
                else if (et->axis == 1)
                {
                    eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3);
                    eJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2));
                }
                else if (et->axis == 2)
                {
                    eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3);
                    eJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2));
                }
                else if (et->axis == 3)
                {
                    eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 4)
                {
                    eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }
                else if (et->axis == 5)
                {
                    eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2));
                    eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero();
                }

                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
                j--;
            }
            else
            {
                _ET_T(et, &ret(0), q[et->jindex]);
                temp = ret * U;
                U = temp;
            }
        }
    }

    void _ETS_fkine(ETS *ets, double *q, double *base, double *tool, MapMatrix4dc &e_ret)
    {
        ET *et;
        Matrix4dc temp;
        Matrix4dc current;

        if (base != NULL)
        {
            MapMatrix4dc e_base(base);
            current = e_base;
        }
        else
        {
            current = Eigen::Matrix4d::Identity();
        }

        for (int i = 0; i < ets->m; i++)
        {
            et = ets->ets[i];

            _ET_T(et, &e_ret(0), q[et->jindex]);
            temp = current * e_ret;
            current = temp;
        }

        if (tool != NULL)
        {
            MapMatrix4dc e_tool(tool);
            e_ret = current * e_tool;
        }
        else
        {
            e_ret = current;
        }
    }

    void _ET_T(ET *et, double *ret, double eta)
    {
        // Check if static and return static transform
        if (!et->isjoint)
        {
            _copy(et->T, ret);
            return;
        }

        if (et->isflip)
        {
            eta = -eta;
        }

        // Calculate ET trasform based on eta
        et->op(ret, eta);
    }

} /* extern "C" */


#include <bits/stdc++.h>
#include <gflags/gflags.h>
#include <sol.hpp>
#include <TH/THTensor.h>
#include "objects_detection_lib.hpp"

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

#include <boost/filesystem.hpp>
#include <boost/gil/gil_all.hpp>

#include <sys/stat.h>

using namespace boost::filesystem;
using namespace cv;
using namespace std;

DEFINE_string(config, "", "configuration");
DEFINE_string(model, "", "model");
DEFINE_string(input, "", "input");
DEFINE_string(output, "", "output");
DEFINE_int32(realtime, 1, "realtime");
DEFINE_int32(batchSize, 8, "batch size");
DEFINE_int32(width, 640, "width");
DEFINE_int32(height, 480, "height");
DEFINE_double(threshold, 0.5, "threshold");
int main(int argc, char* argv[]) {
  gflags::ParseCommandLineFlags(&argc, &argv, true);

  // Initialize torch and load model
  sol::state lua;
  lua.open_libraries();

  lua.script("require 'torch'; require 'cunn'; require 'cudnn'; require 'image';");
  lua.script("torch.setdefaulttensortype('torch.FloatTensor');");
  //lua.script("cudnn.benchmark = true;");
  lua.script("cudnn.fastest = true;");

  lua.script("m = torch.load('" + FLAGS_model + "'):cuda();");
  lua.script("m:evaluate();");

  stringstream ss; ss << "input = torch.FloatTensor(" << FLAGS_batchSize << ", 3, 224, 224);";
  lua.script(ss.str());

  THFloatTensor *input = lua["input"];
  float *data = input->storage->data;

  // warm up
  auto start = chrono::high_resolution_clock::now();
  for (int i=0; i < 100; i++) {
    lua.script("result = m:forward(input:cuda()):float();");
  }

  // Initialize region proposal
  objects_detection::init_objects_detection(FLAGS_config, false /*use_ground_plane*/, false /*use_stixels*/);

  // Set up input and start
  VideoCapture cap;
  deque<path> files;
  map<double, string> timestamped_files;
  if (FLAGS_input == "") {
    cap.open(1);
    cap.set(CV_CAP_PROP_FRAME_WIDTH, FLAGS_width);
    cap.set(CV_CAP_PROP_FRAME_HEIGHT, FLAGS_height);
  } else {
    path p(FLAGS_input);
    assert(exists(p));
    if (is_regular_file(p)) {
      files.push_back(p);
    } else {
      path log = p / "log.txt";
      if (FLAGS_realtime && exists(log)) {
        ifstream in(log.string(), fstream::in);
        int frame; double time;
        while(in >> frame >> time) {
          stringstream ss; ss << setw(4) << setfill('0') << frame+1 << ".png";
          timestamped_files[time] = FLAGS_input + "/" + ss.str();
        }
      } else {
        FLAGS_realtime = 0;
        copy(directory_iterator(p), directory_iterator(), back_inserter(files));
        sort(files.begin(), files.end());
      }
    }
  }

  VideoWriter outputVideo, proposalVideo;
  if (FLAGS_output == "") {
    namedWindow("Proposals");
    namedWindow("Output");
  } else {
    mkdir(FLAGS_output.c_str(), 0777);
    outputVideo.open((path(FLAGS_output) / "output.avi").string(), CV_FOURCC('M', 'J', 'P', 'G'), 30, Size(FLAGS_width, FLAGS_height));
    proposalVideo.open((path(FLAGS_output) / "proposal.avi").string(), CV_FOURCC('M', 'J', 'P', 'G'), 30, Size(FLAGS_width, FLAGS_height));
  }

  double cur_timestamp = 0;
  double video_timestamp = 0;
  int num_proposals = 0;
  int num_frames = 0;
  while(true) {
    path filename;
    double timestamp;
    Mat image;
    if (FLAGS_input == "") {
      timestamp = 0;
      cap >> image;
    } else if (timestamped_files.size() > 0) {
      timestamp = cur_timestamp;
      auto it = --timestamped_files.upper_bound(timestamp);
      filename = it->second;
      cout << filename << endl;
      image = imread(filename.string(), CV_LOAD_IMAGE_COLOR);
      if (++it == timestamped_files.end()) timestamped_files.clear();
    } else {
      if (files.empty()) break;
      timestamp = timestamp + 1/30.;
      filename = files.front();
      image = imread(filename.string(), CV_LOAD_IMAGE_COLOR);
      files.pop_front();
    }
    if (image.rows != FLAGS_height || image.cols != FLAGS_width) {
      resize(image, image, Size(FLAGS_width, FLAGS_height));
    }

    cout << timestamp << " processing " << filename << endl;
    num_frames++;
    auto start = chrono::high_resolution_clock::now();

    objects_detection::input_image_const_view_t input_view =
      boost::gil::interleaved_view(image.cols, image.rows,
        reinterpret_cast<boost::gil::rgb8c_pixel_t*>(image.data), static_cast<size_t>(image.step));

    objects_detection::set_monocular_image(input_view);
    objects_detection::compute();

    std::vector<doppia::Detection2d> detections = objects_detection::get_detections();
    cout << detections.size() << " regions detected." << endl;
    num_proposals += detections.size();

    Mat proposals = image.clone();

    int detected = 0;
    for (int i = 0; i < detections.size(); i += FLAGS_batchSize) {
      vector<Rect> bboxes;
      for (int j = 0, e = min(FLAGS_batchSize, (int)detections.size()-i); j < e; j++) {
        auto d = detections[i+j];
        int l = max(0, (int)d.bounding_box.min_corner().x()), t = max(0, (int)d.bounding_box.min_corner().y());
        Rect r(l, t, min(FLAGS_width-1, (int)d.bounding_box.max_corner().x()) - l + 1, min(FLAGS_height-1, (int)d.bounding_box.max_corner().y()) - t + 1);
        rectangle(proposals, r, Scalar(0, 0, 255));
        bboxes.push_back(r);

        Mat patch;
        image(r).convertTo(patch, CV_32FC3, 1/255.0);
        resize(patch, patch, Size(224, 224));

        float *D = (float*)patch.data;
        int step = 3*patch.cols;
        for (int r = 0, R = patch.rows; r < R; r++) {
          for (int c = 0, C = patch.cols; c < C; c++) {
            data[j*3*224*224 +             r*224 + c] = (D[step*r + 3*c+2] - 0.485) / 0.229;
            data[j*3*224*224 +   224*224 + r*224 + c] = (D[step*r + 3*c+1] - 0.456) / 0.224;
            data[j*3*224*224 + 2*224*224 + r*224 + c] = (D[step*r + 3*c] - 0.406) / 0.225;
          }
        }
      }

      lua.script("result = m:forward(input:cuda()):float();");
      THFloatTensor *result = lua["result"];
      for (int j = 0, e = min(FLAGS_batchSize, (int)detections.size()-i); j < e; j++) {
        float neg = exp(result->storage->data[j*2]), pos = exp(result->storage->data[j*2+1]);
        float p = pos / (pos + neg);
        if (p >= FLAGS_threshold) {
          detected++;
          rectangle(image, bboxes[j], Scalar(0, 0, 255 * p));
        }
      }
    }
    cout << detected << " pedestrians detected." << endl;

    auto elapsed = chrono::duration<double>(chrono::high_resolution_clock::now() - start).count();
    cur_timestamp = cur_timestamp + elapsed;

    if (FLAGS_output == "") {
      imshow("Proposals", proposals);
      imshow("Output", image);
      waitKey(1);
    } else {
      do {
        outputVideo << image;
        proposalVideo << proposals;
        video_timestamp += 1/30.;
      } while (FLAGS_realtime && video_timestamp < cur_timestamp);
    }
  }
  cout << "Done! Time/frame: " << cur_timestamp * 1000 / num_frames << "ms. #Proposals/frame: " << num_proposals * 1.0 / num_frames << endl;
  if (FLAGS_output == "") waitKey(0);
  return 0;
}


#include <FpCoreTest.h>
#include <boost/shared_ptr.hpp>
#include <gtest/gtest.h>
#include <fruitpunch/Resources/Raw.h>
#include <fruitpunch/Physics/PhysicsBox.h>

using namespace std;
using namespace boost;
using namespace fp_core::physics;

// ---------------------------------------------------------------------------
// Test Class
// ---------------------------------------------------------------------------

class PhysicsBoxTest : public FpCoreTest {
protected:
  string mPath;

  virtual void SetUp() {
	initEnvironment();
  }
};

TEST_F(PhysicsBoxTest, Constructor) {
	PhysicsBox box(300,400);
	boost::shared_ptr<b2PolygonShape> shape = static_pointer_cast<b2PolygonShape>(box.getBox2dShape(1.0f));
	ASSERT_EQ(4, shape->GetVertexCount());
	ASSERT_EQ(300, shape->GetVertex(1).x - shape->GetVertex(0).x);
	ASSERT_EQ(400, shape->GetVertex(2).y - shape->GetVertex(0).y);
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_POW_INCLUDE
#define MTL_POW_INCLUDE

#include <boost/numeric/mtl/vector/map_view.hpp>

namespace mtl {

    namespace vec {

        /// Raise Vector \a v to power \a exp
        template <typename Vector, typename Exponent>
        pow_by_view<Vector, Exponent> pow(const Vector& v, const Exponent& exp)
        {
            return pow_by_view<Vector, Exponent>(v, exp);
        }

    } // namespace vec

} // namespace mtl

#endif // MTL_POW_INCLUDE


#ifndef KGRAPH_VALUE_TYPE
#define KGRAPH_VALUE_TYPE float
#endif

#include <cctype>
#include <type_traits>
#include <iostream>
#include <boost/timer/timer.hpp>
#include <boost/program_options.hpp>
#include <sys/time.h>
#include <cctype>
#include <random>
#include <iomanip>
#include <type_traits>
#include <boost/timer/timer.hpp>
#include <boost/tr1/random.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>

#include "kgraph.h"
#include "kgraph-data.h"

using namespace boost::timer;
using namespace std;
using namespace boost;
using namespace kgraph;
namespace po = boost::program_options;

typedef KGRAPH_VALUE_TYPE value_type;

int main(int argc, char *argv[]) {
    string data_path;
    string output_path;
    KGraph::IndexParams params;
    unsigned D;
    unsigned skip;
    unsigned gap;
    unsigned synthetic;
    float noise;

    bool lshkit = true;

    po::options_description desc_visible("General options");
    desc_visible.add_options()
    ("help,h", "produce help message.")
    ("version,v", "print version information.")
    ("data", po::value(&data_path), "input path")
    ("output", po::value(&output_path), "output path")
    (",K", po::value(&params.K)->default_value(default_K), "number of nearest neighbor")
    ("controls,C", po::value(&params.controls)->default_value(default_controls), "number of control pounsigneds")
    ;

    po::options_description desc_hidden("Expert options");
    desc_hidden.add_options()
    ("iterations,I", po::value(&params.iterations)->default_value(default_iterations), "")
    (",S", po::value(&params.S)->default_value(default_S), "")
    (",R", po::value(&params.R)->default_value(default_R), "")
    (",L", po::value(&params.L)->default_value(default_L), "")
    ("delta", po::value(&params.delta)->default_value(default_delta), "")
    ("recall", po::value(&params.recall)->default_value(default_recall), "")
    ("prune", po::value(&params.prune)->default_value(default_prune), "")
    ("noise", po::value(&noise)->default_value(0), "noise")
    ("seed", po::value(&params.seed)->default_value(default_seed), "")
    ("dim,D", po::value(&D), "dimension, see format")
    ("skip", po::value(&skip)->default_value(0), "see format")
    ("gap", po::value(&gap)->default_value(0), "see format")
    ("raw", "read raw binary file, need to specify D.")
    ("synthetic", po::value(&synthetic)->default_value(0), "generate synthetic data, for performance evaluation only, specify number of points")
    ;

    po::options_description desc("Allowed options");
    desc.add(desc_visible).add(desc_hidden);

    po::positional_options_description p;
    p.add("data", 1);
    p.add("output", 1);

    po::variables_map vm;
    po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
    po::notify(vm);

    if (vm.count("raw") == 1) {
        lshkit = false;
    }

    if (vm.count("version")) {
        cout << "KGraph version " << KGraph::version() << endl;
        return 0;
    }

    if (vm.count("help")
            || (synthetic && (vm.count("dim") == 0 || vm.count("data")))
            || (!synthetic && (vm.count("data") == 0 || (vm.count("dim") == 0 && !lshkit)))) {
        cout << "Usage: index [OTHER OPTIONS]... INPUT [OUTPUT]" << endl;
        cout << desc_visible << endl;
        cout << desc_hidden << endl;
        return 0;
    }

    if (params.S == 0) {
        params.S = params.K;
    }

    if (lshkit && (synthetic == 0)) {   // read dimension information from the data file
        static const unsigned LSHKIT_HEADER = 3;
        ifstream is(data_path.c_str(), ios::binary);
        unsigned header[LSHKIT_HEADER]; /* entry size, row, col */
        is.read((char *)header, sizeof header);
        BOOST_VERIFY(is);
        BOOST_VERIFY(header[0] == sizeof(value_type));
        is.close();
        D = header[2];
        skip = LSHKIT_HEADER * sizeof(unsigned);
        gap = 0;
    }

    Matrix<value_type> data;
    if (synthetic) {
        if (!std::is_floating_point<value_type>::value) {
            throw runtime_error("synthetic data not implemented for non-floating-point values.");
        }
        data.resize(synthetic, D);
        cerr << "Generating synthetic data..." << endl;
        default_random_engine rng(params.seed);
        uniform_real_distribution<double> distribution(-1.0, 1.0);
        data.zero(); // important to do that
        for (unsigned i = 0; i < synthetic; ++i) {
            value_type *row = data[i];
            for (unsigned j = 0; j < D; ++j) {
                row[j] = distribution(rng);
            }
        }
    }
    else {
        data.load(data_path, D, skip, gap);
    }
    if (noise != 0) {
        if (!std::is_floating_point<value_type>::value) {
            throw runtime_error("noise injection not implemented for non-floating-point value.");
        }
        tr1::ranlux64_base_01 rng;
        double sum = 0, sum2 = 0;
        for (unsigned i = 0; i < data.size(); ++i) {
            for (unsigned j = 0; j < data.dim(); ++j) {
                value_type v = data[i][j];
                sum += v;
                sum2 += v * v;
            }
        }
        double total = double(data.size()) * data.dim();
        double avg2 = sum2 / total, avg = sum / total;
        double dev = sqrt(avg2 - avg * avg);
        cerr << "Adding Gaussian noise w/ " << noise << "x sigma(" << dev << ")..." << endl;
        boost::normal_distribution<double> gaussian(0, noise * dev);
        for (unsigned i = 0; i < data.size(); ++i) {
            for (unsigned j = 0; j < data.dim(); ++j) {
                data[i][j] += gaussian(rng);
            }
        }
    }

    MatrixOracle<value_type, metric::l2sqr> oracle(data);
    KGraph::IndexInfo info;
    KGraph *kgraph = KGraph::create(); //(oracle, params, &info);
    {
        auto_cpu_timer timer;
        kgraph->build(oracle, params, output_path.c_str(), &info);
        cerr << info.stop_condition << endl;
    }

    if (output_path.size()) {

      //Note that we modify the index save procedure to reduce index size
      //kgraph->save(output_path.c_str());
    }

    delete kgraph;

    return 0;
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"

#include <boost/graph/isomorphism.hpp>

using namespace graph_tool;
using namespace boost;

struct check_iso
{

    template <class Graph1, class Graph2, class IsoMap, class InvMap,
              class VertexIndexMap>
    void operator()(Graph1& g1, Graph2* g2, InvMap cinv_map1, InvMap cinv_map2,
                    int64_t max_inv, IsoMap map, VertexIndexMap index1,
                    VertexIndexMap index2, bool& result) const
    {
        auto inv_map1 = cinv_map1.get_unchecked(num_vertices(g1));
        auto inv_map2 = cinv_map2.get_unchecked(num_vertices(*g2));

        vinv_t<decltype(inv_map1)> vinv1(inv_map1, max_inv);
        vinv_t<decltype(inv_map2)> vinv2(inv_map2, max_inv);

        result = isomorphism(g1, *g2,
                             isomorphism_map(map.get_unchecked(num_vertices(g1))).
                             vertex_invariant1(vinv1).
                             vertex_invariant2(vinv2).
                             vertex_index1_map(index1).
                             vertex_index2_map(index2));
    }

    template <class Prop>
    struct vinv_t
    {
        vinv_t(Prop& prop, int64_t max)
            : _prop(prop), _max(max) {}
        Prop& _prop;
        int64_t _max;

        template <class Vertex>
        int64_t operator()(Vertex v) const
        {
            return _prop[v];
        };

        int64_t max() const { return _max; }

        typedef int64_t result_type;
        typedef size_t argument_type;
    };
};

struct directed_graph_view_pointers:
    mpl::transform<graph_tool::detail::always_directed,
                   mpl::quote1<std::add_pointer> >::type {};

struct undirected_graph_view_pointers:
    mpl::transform<graph_tool::detail::never_directed,
                   mpl::quote1<std::add_pointer> >::type {};

typedef property_map_types::apply<integer_types,
                                  GraphInterface::vertex_index_map_t,
                                  mpl::bool_<false> >::type
    vertex_props_t;

bool check_isomorphism(GraphInterface& gi1, GraphInterface& gi2,
                       boost::any ainv_map1, boost::any ainv_map2,
                       int64_t max_inv, boost::any aiso_map)
{
    bool result;

    typedef property_map_type::apply<int32_t,
                                     GraphInterface::vertex_index_map_t>::type
        iso_map_t;
    auto iso_map = any_cast<iso_map_t>(aiso_map);

    typedef property_map_type::apply<int64_t,
                                     GraphInterface::vertex_index_map_t>::type
        inv_map_t;
    auto inv_map1 = any_cast<inv_map_t>(ainv_map1);
    auto inv_map2 = any_cast<inv_map_t>(ainv_map2);

    if (gi1.GetDirected() != gi2.GetDirected())
        return false;
    if (gi1.GetDirected())
    {
        run_action<graph_tool::detail::always_directed>()
            (gi1, std::bind(check_iso(),
                            placeholders::_1, placeholders::_2,
                            inv_map1, inv_map2, max_inv, iso_map,
                            gi1.GetVertexIndex(),
                            gi2.GetVertexIndex(), std::ref(result)),
             directed_graph_view_pointers())
            (gi2.GetGraphView());
    }
    else
    {
        run_action<graph_tool::detail::never_directed>()
            (gi1, std::bind(check_iso(),
                            placeholders::_1, placeholders::_2,
                            inv_map1, inv_map2, max_inv, iso_map,
                            gi1.GetVertexIndex(),
                            gi2.GetVertexIndex(), std::ref(result)),
             undirected_graph_view_pointers())
            (gi2.GetGraphView());
    }

    return result;
}


#include <boost/test/unit_test.hpp>
#include "svm_test_input.hpp"
#include "../src/svm_impl/classifiers_generator.h"
#include "../src/svm_impl/regressions_generator.h"
#include "../src/svm_impl/regression_params.hpp"
#include "../src/svm_impl/classifier_params.hpp"
#include "../src/svm_impl/svm_model.h"
#include <limits>
#include <stdexcept>
#include <iostream>

namespace
{

const mlmodels::class_data labels = { 0, 0, 1, 1, 0, 2, 1, 2, 1, 0 };

const mlmodels::training_data dataf = make_multi_array<mlmodels::value_type, 10, 2>(
        { { 100.f, 10.f },
          { 150.f, 10.f },
          { 600.f, 200.f },
          { 600.f, 10.f },
          { 10.f, 100.f },
          { 455.f, 10.f },
          { 345.f, 255.f },
          { 10.f, 501.f },
          { 401.f, 255.f },
          { 30.f, 150.f }
        }
);

}   // end of namespace

BOOST_AUTO_TEST_CASE(test_invalid_data_for_model)
{
    using namespace mlmodels;
    auto model = svm::classifier::create_model(svm::classifier::c_rbf_train{},
                        training_data{}, class_data{}
            );
    BOOST_TEST((model.get() == nullptr), "we are expecting to fail in model generation as we have invalid data for the model");
    // while it should be the same - test for regression type as well
    model = svm::regression::create_model(svm::regression::epsilon_linear_train{},
                            training_data{}, class_data{}
            );
    BOOST_TEST((model.get() == nullptr), "we are expecting to fail in model generation as we have invalid data for the model");
}

BOOST_AUTO_TEST_CASE(test_train_model)
{
    using namespace mlmodels;
    try {
        auto model = svm::classifier::create_model(svm::classifier::nu_poly_train{}, train_data, train_labels);
        BOOST_TEST((model.get() != nullptr), "we are expecting to successfully pass training with this - if not then we have an anexpected issue here");
    } catch (const std::exception& e) {
        BOOST_TEST(false, "failed to create model: "<<e.what());
    } catch (...) {
        BOOST_TEST(false, "unexpted failure in generating the model");
    }

    // now lets try to do the same for regression
    try {
//        std::cout<<"start training on epsilon_rbf_train"<<std::endl;
        auto model = svm::regression::create_model(svm::regression::epsilon_rbf_train{}, dataf, labels);
 //       std::cout<<"finish"<<std::endl;
        BOOST_TEST((model.get() != nullptr), "we are expecting to successfully pass training with this - if not then we have an anexpected issue here");
    } catch (const std::exception& e) {
        BOOST_TEST(false, "failed to create model: "<<e.what());
    } catch (...) {
        BOOST_TEST(false, "unexpted failure in generating the model");
    }
}

BOOST_AUTO_TEST_CASE(test_invalid_prediction)
{
    using namespace mlmodels;
    // create invalid model
    auto model = svm::classifier::create_model(svm::classifier::nu_linear_train{},
                        training_data{}, class_data{}
            );
    BOOST_TEST_REQUIRE((model.get() == nullptr), "we are expecting to fail in model generation as we have invalid data for the model");
    auto ret = svm::predict(model, labels);
    BOOST_TEST(ret == std::numeric_limits<value_type>::max());
    auto ret2 = svm::test(model, dataf);
    BOOST_TEST(ret2.empty());
    model = svm::regression::create_model(svm::regression::epsilon_sig_train{},
                            training_data{}, class_data{}
            );
    BOOST_TEST((model.get() == nullptr), "we are expecting to fail in model generation as we have invalid data for the model");
    ret = svm::predict(model, labels);
    BOOST_TEST(ret == std::numeric_limits<value_type>::max());
    ret2 = svm::test(model, dataf);
    BOOST_TEST(ret2.empty());
}

BOOST_AUTO_TEST_CASE(test_invalid_model_train_args)
{
    using namespace mlmodels;
    // by passing labels and data with the a different number of elements
    // want to see if this is not accepted
    try {
        auto model = svm::regression::create_model(svm::regression::nu_sig_train{}, dataf, train_labels);
        BOOST_TEST((model.get() == nullptr), "we should get invalid model with these values");
    } catch (const std::exception&) {
        BOOST_TEST(false, "we should get an empty model");
    } catch (...) {
        BOOST_TEST(false, "we should not get a starndard exception, got some other exception");
    }
}

BOOST_AUTO_TEST_CASE(test_invalid_prediction_data_for_model)
{
    using namespace mlmodels;
    // make sure that if we are passing wrong number of features we would fail to predict
    try {
        auto model = svm::classifier::create_model(svm::classifier::c_linear_train{}, train_data, train_labels);
        BOOST_TEST_REQUIRE((model.get() != nullptr), "failed to train the model!!");
        // now try to run prediction and test, and see that it would fail since we are passing the wrong number of features
        auto ret = svm::test(model, dataf);
        BOOST_TEST(ret.empty(), "the args to the model test is wrong - wrong number of features, should return empty");
        auto ret2 = svm::predict(model, labels);
        BOOST_TEST((ret2 == std::numeric_limits<value_type>::max()), "the args to predict are wrong, we pass the wrong number of features");
    } catch (...) {
        BOOST_TEST(false, "we should not get any exception here");
    }
}
BOOST_AUTO_TEST_CASE(test_prediction_is_valid_return_value)
{
    using namespace mlmodels;
    // do the same for regression model
    try {
        //auto model = svm::regression::create_model(svm::regression::epsilon_linear_train{}, dataf, labels);
        auto model = svm::classifier::create_model(svm::classifier::c_linear_train{}, dataf, labels);
        BOOST_TEST_REQUIRE((model.get() != nullptr), "failed to train the model!!");
        // pass a row of valid size for prediction
        const class_data row{11.f, 110.f};
        auto ret = svm::test(model, dataf);
        BOOST_TEST(not ret.empty(), "the args to the model test is  - good, should not return empty");
        auto ret2 = svm::predict(model, row);
        BOOST_TEST((ret2 != std::numeric_limits<value_type>::max()), "the args to predict are good, we pass good number of features");
    } catch (...) {
        BOOST_TEST(false, "we should not get any exception here");
    }
    return;
    // in this test we would make sure that if we are passing a valid arguments we are getting
    // valid results - by valid it means that they are not out of bound, but we are not checking
    // if this is a correct prediction mathematically
    try {
        auto model = svm::classifier::create_model(svm::classifier::c_linear_train{}, train_data, train_labels);
        BOOST_TEST_REQUIRE((model.get() != nullptr), "failed to train the model!!");
        // pass a row of valid size for prediction
        const class_data row{0.0416667, -1,-0.333333, -0.283019, -0.260274, 1, 1, 0.343511, 1, -1, -1, -0.333333, -1};
        auto ret = svm::test(model, test_data);
        BOOST_TEST(not ret.empty(), "the args to the model test is  - good, should not return empty");
        auto ret2 = svm::predict(model, row);
        BOOST_TEST((ret2 != std::numeric_limits<value_type>::max()), "the args to predict are good, we pass good number of features");
    } catch (...) {
        BOOST_TEST(false, "we should not get any exception here");
    }

}


#include "dvs_hot_pixel_filter/utils.h"

#include <boost/filesystem.hpp>
#include <gflags/gflags.h>
#include <iostream>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>

DECLARE_bool(no_stats);

namespace dvs_hot_pixel_filter {
namespace utils {

const std::string OUTPUT_FOLDER = "./stats/";

bool parse_arguments(int argc, char* argv[],
                     std::string* path_to_input_rosbag)
{
  if(argc < 2)
  {
    std::cerr << "Error: not enough input arguments.\n"
        "Usage:\n\trosrun dvs_hot_pixel_filter hot_pixel_filter path_to_bag.bag\n\n"
        "Additional (optional) command-line flags include:\n"
        "\t--n_hot_pix=<number_of_hot_pixels>\n"
        "\t--n_std=<number_of_standard_deviations>\n"
        "\t--no_stats (do not save stats on disk)"<< std::endl;
    return false;
  }

  *path_to_input_rosbag = std::string(argv[1]);
  std::cout << "Input bag: " << *path_to_input_rosbag << std::endl;
  return true;
}

void write_histogram_image(const std::string filename,
                           const cv::Mat& histogram,
                           const std::vector<cv::Point>& hot_pixels)
{
  cv::Mat display_image;

  if (!hot_pixels.empty())
  {
    cv::Vec3b colour = cv::Vec3b(255, 0, 0);
    cv::Mat local_hist;
    histogram.copyTo(local_hist);
    // create mask
    cv::Mat mask = cv::Mat::zeros(histogram.size(), CV_8UC1);
    for (auto point : hot_pixels)
    {
      mask.at<uint8_t>(point) = 1;
    }
    double max;
    cv::minMaxLoc(local_hist, nullptr, &max);
    local_hist.setTo(max, mask);
    cv::normalize(local_hist, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT);
    display_image.setTo(colour, mask);
  }
  else
  {
    cv::normalize(histogram, display_image, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::applyColorMap(display_image, display_image, cv::COLORMAP_HOT);
  }

  cv::Mat large_image;
  cv::resize(display_image, large_image, cv::Size(), 3, 3, cv::INTER_NEAREST);
  cv::imwrite(filename, large_image);
}

std::string extract_bag_name(const std::string fullname)
{
  int pos = 0;
  int len = fullname.length();
  // go from the back to the first forward- or back-slash.
  for (int i = len; i > 0; i--)
  {
    if (fullname[i] == '/' || fullname[i] == '\\')
    {
      pos = i + 1;
      break;
    }
  }
  int count = 4;
  // now go from there to the first '.'
  for (int i = 0; i < len; i++)
  {
    if (fullname[pos + i] == '.')
    {
      count = i;
      break;
    }
  }
  std::string bag_name = fullname.substr(pos, count);
  return bag_name;
}

void build_histograms(rosbag::View& view,
                      topic_mats& histograms)
{
  std::cout << "Building event count histogram(s)..." << std::endl;

  std::vector<std::string> seen_topics;
  for(const rosbag::MessageInstance& m : view)
  {
    if(m.getDataType() == "dvs_msgs/EventArray")
    {
      const std::string topic_name = m.getTopic();
      // pointer to the message
      dvs_msgs::EventArrayConstPtr s = m.instantiate<dvs_msgs::EventArray>();
      const cv::Size msg_size = cv::Size(s->width, s->height);

      cv::Mat& histogram = histograms[topic_name];

      // initialise event_count_histogram if we haven't seen the topic yet
      if ( !contains(topic_name, seen_topics) )
      {
        histogram = cv::Mat::zeros(msg_size, CV_64FC1);
        seen_topics.push_back(topic_name);
        std::cout << "added " << topic_name << " to seen_topics" << std::endl;
      }

      if (msg_size != histogram.size())
      {
        std::cerr << "Error: a new event message in " << topic_name <<
        " does not match the existing topic histogram size.\n message: " <<
        msg_size << "\t histogram: " << histogram.size() << std::endl;
        return;
      }

      for(auto e : s->events)
      {
        // accumulate events without discrimination
        histogram.at<double>(e.y, e.x)++;
      }
    }
  }

  std::cout << "...done!" << std::endl;
}

void detect_hot_pixels(const topic_mats& histograms_by_topic,
                       const double& num_std_devs,
                       const int num_hot_pixels,
                       topic_points& hot_pixels_by_topic)
{
  for(const auto& topic : histograms_by_topic)
    {
      const std::string topic_name = topic.first;
      const cv::Mat& histogram = topic.second;
      std::vector<cv::Point>& hot_pixels = hot_pixels_by_topic[topic_name];
      if (num_hot_pixels == -1)
      {
        // auto-detect hot pixels
        double threshold;
        dvs_hot_pixel_filter::utils::find_threshold(
            histogram, num_std_devs, threshold);
        dvs_hot_pixel_filter::utils::hot_pixels_by_threshold(
            histogram, threshold, hot_pixels);
      }
      else
      {
        // user-specified number of hot pixels
        dvs_hot_pixel_filter::utils::hot_pixels_by_ranking(
            histogram, num_hot_pixels, hot_pixels);
      }
    }
}

void hot_pixels_by_threshold(const cv::Mat& histogram,
                             const double& threshold,
                             std::vector<cv::Point>& hot_pixels)
{
  for (int y = 0; y < histogram.rows; y++)
  {
    for (int x = 0; x < histogram.cols; x++)
    {
      if (histogram.at<double>(y, x) > threshold)
      {
        hot_pixels.push_back(cv::Point(x, y));
      }
    }
  }
}
void hot_pixels_by_ranking(const cv::Mat& histogram,
                           const double& num_hot_pixels,
                           std::vector<cv::Point>& hot_pixels)
{
  cv::Mat local_hist;
  histogram.copyTo(local_hist);

  for (int i = 0; i < num_hot_pixels; i++)
  {
    double max;
    cv::Point maxLoc;
    cv::minMaxLoc(local_hist, nullptr, &max, nullptr, &maxLoc);

    hot_pixels.push_back(maxLoc);
    local_hist.at<double>(maxLoc) = 0;
  }
}

void find_threshold(const cv::Mat& histogram,
                    const double num_std_devs,
                    double& threshold)
{
  cv::Scalar mean_Scalar, stdDev_Scalar;
  cv::meanStdDev(histogram, mean_Scalar, stdDev_Scalar, histogram > 0);

  const double mean = mean_Scalar[0];
  const double stdDev = stdDev_Scalar[0];
  threshold = mean + num_std_devs*stdDev;
}

void write_all_msgs(rosbag::View& view,
                    topic_points& hot_pixels_by_topic,
                    rosbag::Bag& output_bag)
{
  constexpr int log_every_n_messages = 10000;
  const uint32_t num_messages = view.size();
  uint32_t message_index = 0;
  std::cout << "Writing..." << std::endl;
  // write the new rosbag without hot pixels by iterating over all messages
  for(rosbag::MessageInstance const m : view)
  {
    write_msg(
        m, hot_pixels_by_topic, output_bag);
    if(message_index++ % log_every_n_messages == 0)
    {
      std::cout << "Message: " << message_index << " / " << num_messages << std::endl;
    }
  }

  std::cout << "Message: " << num_messages << " / " << num_messages << std::endl;
  std::cout << "...done!" << std::endl;
}

void write_event_msg(const std::string topic_name,
                     const dvs_msgs::EventArrayConstPtr event_array_ptr,
                     const std::vector<cv::Point>& hot_pixels,
                     rosbag::Bag& output_bag)
{
  std::vector<dvs_msgs::Event> events;
  for(auto e : event_array_ptr->events)
  {
    if (!contains(cv::Point(e.x, e.y), hot_pixels))
    {
      events.push_back(e);
    }
  }

  if (events.size() > 0)
  {
  // Write new event array message to output rosbag

  dvs_msgs::EventArray event_array_msg;
  event_array_msg.events = events;
  event_array_msg.width = event_array_ptr->width;
  event_array_msg.height = event_array_ptr->height;
  event_array_msg.header.stamp = events.back().ts;

  output_bag.write(topic_name, event_array_msg.header.stamp, event_array_msg);
  }
}

void write_msg(const rosbag::MessageInstance& m,
               topic_points& hot_pixels_topic,
               rosbag::Bag& output_bag)
{
  if(m.getDataType() == "dvs_msgs/EventArray")
  {
    const std::string topic_name = m.getTopic();
    std::vector<cv::Point>& hot_pixels = hot_pixels_topic[topic_name];
    dvs_msgs::EventArrayConstPtr event_array_ptr = m.instantiate<dvs_msgs::EventArray>();
    write_event_msg(topic_name, event_array_ptr, hot_pixels, output_bag);

  }
  else if(m.getDataType() == "sensor_msgs/Image")
  {
    sensor_msgs::ImageConstPtr img_msg = m.instantiate<sensor_msgs::Image>();
    output_bag.write(m.getTopic(), img_msg->header.stamp, m);
  }
  else if(m.getDataType() == "sensor_msgs/Imu")
  {
    sensor_msgs::ImuConstPtr imu_msg = m.instantiate<sensor_msgs::Imu>();
    output_bag.write(m.getTopic(), imu_msg->header.stamp, m);
  }
  else
  {
    output_bag.write(m.getTopic(), m.getTime(), m);
  }
}

std::string usable_filename(const std::string filename_in)
{
  std::string filename = filename_in;
  std::replace( filename.begin(), filename.end(), '/', '_'); // replace all '/' to '_'
  std::replace( filename.begin(), filename.end(), '\\', '_'); // replace all '\' to '_'
  return filename;
}

void write_hot_pixels(const std::string filename,
                      const std::vector<cv::Point>& hot_pixels)
{
  std::ofstream hot_pixels_file;
  hot_pixels_file.open(filename);
  // the important part
  for (const auto& point : hot_pixels)
  {
    hot_pixels_file << point.x << ", " << point.y << "\n";
  }
  hot_pixels_file.close();
}

void save_stats(const std::string bag_name,
                 const std::string topic_name,
                 const cv::Mat& histogram,
                 const std::vector<cv::Point>& hot_pixels,
                 const bool one_topic)
{
  cv::Mat histogram_after;
  histogram.copyTo(histogram_after);
  for (auto point : hot_pixels)
  {
    histogram_after.at<double>(point) = 0;
  }

  const double num_events = cv::sum(histogram)[0];
  const double num_events_after = cv::sum(histogram_after)[0];
  const double percent_events_discarded = (1 - num_events_after/num_events)*100;

  std::cout << std::setprecision(4) << topic_name << "\t" << num_events <<
      "\t" << hot_pixels.size() << "\t\t0\t(before)" << std::endl;

  std::cout << std::setprecision(4) << topic_name << "\t" << num_events_after <<
      "\t0\t\t" << percent_events_discarded << "\t(after)" << std::endl;

  if (!FLAGS_no_stats)
  {
    // save images
    std::string dstDir = OUTPUT_FOLDER + bag_name + "/";
    if (!one_topic)
    {
      dstDir += usable_filename(topic_name) + "/";
    }
    boost::filesystem::create_directories(dstDir); // create if needed
    std::string fname_b = dstDir + "hist_before.png";
    std::string fname_a = dstDir + "hist_after.png";
    std::string fname_hp = dstDir + "hot_pixels.txt";

    write_histogram_image(fname_b, histogram);
    write_histogram_image(fname_a, histogram_after, hot_pixels);
    write_hot_pixels(fname_hp, hot_pixels);
  }
}

}  // namespace utils
}  // namespace dvs_hot_pixel_filter


//
// main.cpp
// ~~~~~~~~
//
// Copyright (c) 2003-2012 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// 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)
//

#include <iostream>
#include <string>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
#include "server.hpp"

using namespace http::server ;
using namespace std ;

class TileHandler: public request_handler {
public:
    void handle_request(const request& req, reply& rep) {

        boost::smatch m ;
        boost::regex_match(req.path_, m, rx_) ;

        int zoom = stoi(m.str(1)) ;
        int tx = stoi(m.str(2)) ;
        int ty = stoi(m.str(3)) ;
        string extension = m.str(4) ;

        cout << tx << ' ' << ty << ' ' << zoom << endl ;
    }

     static boost::regex rx_ ;
};

boost::regex TileHandler::rx_(R"(/map/[^/]+/tiles/[^/]+/(\d+)/(\d+)/(\d+)\.([^/]+))") ;

class Factory: public request_handler_factory {
public:
    Factory() = default ;

    std::shared_ptr<request_handler> create(const request &req) {
        boost::smatch m ;
        if ( boost::regex_match(req.path_, m, TileHandler::rx_) )
             return std::shared_ptr<request_handler>(new TileHandler()) ;
        else
            return nullptr ;
    }
};

int main(int argc, char* argv[])
{
  try
  {
    // Check command line arguments.
    if (argc != 5)
    {
      std::cerr << "Usage: http_server <address> <port> <threads> <doc_root>\n";
      std::cerr << "  For IPv4, try:\n";
      std::cerr << "    receiver 0.0.0.0 80 1 .\n";
      std::cerr << "  For IPv6, try:\n";
      std::cerr << "    receiver 0::0 80 1 .\n";
      return 1;
    }

    // Initialise the server.
    std::size_t num_threads = boost::lexical_cast<std::size_t>(argv[3]);
    http::server::server s(make_shared<Factory>(), argv[1], argv[2], num_threads);

    // Run the server until stopped.
    s.run();
  }
  catch (std::exception& e)
  {
    std::cerr << "exception: " << e.what() << "\n";
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Copyright 2019 FZI Research Center for Information Technology
//
// 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.
////////////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------------
/*!\file    DampedLeastSquaresSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/27
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/DampedLeastSquaresSolver.h>

// Pluginlib
#include <pluginlib/class_list_macros.h>

// other
#include <boost/algorithm/clamp.hpp>

/**
 * \class cartesian_controller_base::DampedLeastSquaresSolver
 *
 * Users may explicitly specify this solver with \a "damped_least_squares" as \a
 * ik_solver in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "damped_least_squares"
 *     ...
 *
 *     solver:
 *         ...
 *         damped_least_squares:
 *             alpha: 0.5
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::DampedLeastSquaresSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  DampedLeastSquaresSolver::DampedLeastSquaresSolver()
    : m_alpha(0.01)
  {
  }

  DampedLeastSquaresSolver::~DampedLeastSquaresSolver(){}

  trajectory_msgs::JointTrajectoryPoint DampedLeastSquaresSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {
    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint velocities according to:
    // \f$ \dot{q} = ( J^T J + \alpha^2 I )^{-1} J^T f \f$
    ctrl::MatrixND identity;
    identity.setIdentity(m_number_joints, m_number_joints);

    m_current_velocities.data =
      (m_jnt_jacobian.data.transpose() * m_jnt_jacobian.data
       + m_alpha * m_alpha * identity).inverse() * m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool DampedLeastSquaresSolver::init(ros::NodeHandle& nh,
                                      const KDL::Chain& chain,
                                      const KDL::JntArray& upper_pos_limits,
                                      const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_jacobian.resize(m_number_joints);

    // Connect dynamic reconfigure and overwrite the default values with values
    // on the parameter server. This is done automatically if parameters with
    // the according names exist.
    m_callback_type = boost::bind(
        &DampedLeastSquaresSolver::dynamicReconfigureCallback, this, _1, _2);

    m_dyn_conf_server.reset(
        new dynamic_reconfigure::Server<IKConfig>(
          ros::NodeHandle(nh.getNamespace() + "/solver/damped_least_squares")));
    m_dyn_conf_server->setCallback(m_callback_type);
    return true;
  }

    void DampedLeastSquaresSolver::dynamicReconfigureCallback(IKConfig& config, uint32_t level)
    {
      m_alpha = config.alpha;
    }

} // namespace