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

#include <votca/tools/linalg.h>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_eigen.h>

namespace votca { namespace tools {

using namespace std;

/**
 * ublas binding to GSL  Singular Value Decomposition
 *
 * A = U S V^T
 *
 * @param A MxN matrix do decompose. Becomes an MxN orthogonal matrix U
 * @param V NxN orthogonal square matrix
 * @param E NxN diagonal matrix of singular values
 * @return succeeded or not
 */
bool linalg_singular_value_decomposition( ub::matrix<double> &A, ub::matrix<double> &V, ub::vector<double> &S )
{
	/*
        gsl_error_handler_t *handler = gsl_set_error_handler_off();
	const size_t N = A.size1();

        // gsl does not handle conversion of a symmetric_matrix
        ub::matrix<double> _A( N,N );
        _A = A;

	E.resize(N, false);
	V.resize(N, N, false);
	gsl_matrix_view A_view = gsl_matrix_view_array(&_A(0,0), N, N);
	gsl_vector_view E_view = gsl_vector_view_array(&E(0), N);
	gsl_matrix_view V_view = gsl_matrix_view_array(&V(0,0), N, N);
	gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(N);

	int status = gsl_eigen_symmv(&A_view.matrix, &E_view.vector, &V_view.matrix, w);
	//gsl_eigen_symmv_sort(&E_view.vector, &V_view.matrix, GSL_EIGEN_SORT_ABS_ASC);
	gsl_eigen_symmv_free(w);
	gsl_set_error_handler(handler);

	return (status != 0);
         */
    throw std::runtime_error("linalg_singular_value_decomposition is not compiled-in due to disabling of MKL - recompile Votca Tools with MKL support");
};

}}


#ifndef BOOST_HASH2_HMAC_HPP_INCLUDED
#define BOOST_HASH2_HMAC_HPP_INCLUDED

// Copyright 2017, 2018 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.
//
// HMAC message authentication algorithm, https://tools.ietf.org/html/rfc2104

#include <boost/hash2/byte_type.hpp>
#include <boost/hash2/detail/write.hpp>
#include <boost/cstdint.hpp>
#include <boost/assert.hpp>
#include <cstring>
#include <cstddef>

namespace boost
{
namespace hash2
{

template<class H> class hmac
{
public:

    typedef typename H::result_type result_type;
    typedef typename H::size_type size_type;

    static const int block_size = H::block_size;

private:

    H outer_;
    H inner_;

private:

    void init( byte_type const * p, std::ptrdiff_t n )
    {
        int const m = block_size;

        byte_type key[ m ] = {};

        if( n == 0 )
        {
            // memcpy from (NULL, 0) is undefined
        }
        else if( n <= m )
        {
            std::memcpy( key, p, n );
        }
        else
        {
            H h;

            h.update( p, n );

            result_type r = h.result();

            std::memcpy( key, &r[0], r.size() );
        }

        for( int i = 0; i < m; ++i )
        {
            key[ i ] = static_cast<byte_type>( key[ i ] ^ 0x36 );
        }

        inner_.update( key, m );

        for( int i = 0; i < m; ++i )
        {
            key[ i ] = static_cast<byte_type>( key[ i ] ^ 0x36 ^ 0x5C );
        }

        outer_.update( key, m );
    }

public:

    hmac()
    {
        init( 0, 0 );
    }

    explicit hmac( boost::uint64_t seed )
    {
        if( seed == 0 )
        {
            init( 0, 0 );
        }
        else
        {
            byte_type tmp[ 8 ];
            detail::write64le( tmp, seed );

            init( tmp, 8 );
        }
    }

    hmac( byte_type const * p, std::ptrdiff_t n )
    {
        BOOST_ASSERT( n >= 0 );
        init( p, n );
    }

    void update( byte_type const * p, std::ptrdiff_t n )
    {
        BOOST_ASSERT( n >= 0 );
        inner_.update( p, n );
    }

    result_type result()
    {
        result_type r = inner_.result();

        outer_.update( &r[0], r.size() );

        return outer_.result();
    }
};

} // namespace hash2
} // namespace boost

#endif // #ifndef BOOST_HASH2_HMAC_HPP_INCLUDED


/** $Id: Generator.cxx 137064 2015-08-31 18:24:47Z jvansanten $
 * @file
 * @author Jakob van Santen <vansanten@wisc.edu>
 *
 * $Revision: 137064 $
 * $Date: 2015-08-31 12:24:47 -0600 (Mon, 31 Aug 2015) $
 */

#include <MuonGun/Generator.h>
#include <MuonGun/SamplingSurface.h>

#include <icetray/I3Module.h>
#include <dataclasses/physics/I3MCTreeUtils.h>
#include <dataclasses/I3Double.h>
#include <phys-services/I3RandomService.h>
#include <boost/make_shared.hpp>
#include <boost/foreach.hpp>

namespace I3MuonGun {

GenerationProbability::~GenerationProbability() {}
Generator::~Generator() {}

template <typename Archive>
void
GenerationProbability::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("I3FrameObject", base_object<I3FrameObject>(*this));
	ar & make_nvp("NEvents", numEvents_);
}

template <typename Archive>
void
Generator::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("GenerationProbability", base_object<GenerationProbability>(*this));
}

double
GenerationProbability::GetLogGeneratedEvents(const I3Particle &axis, const BundleConfiguration &bundle) const
{
	return std::log(double(numEvents_)) + GetLogGenerationProbability(axis, bundle);
}

double
GenerationProbability::GetGeneratedEvents(const I3Particle &axis, const BundleConfiguration &bundle) const
{
	return numEvents_*std::exp(GetLogGenerationProbability(axis, bundle));
}

GenerationProbabilityCollection::GenerationProbabilityCollection(GenerationProbabilityPtr p1, GenerationProbabilityPtr p2)
{
	push_back(p1);
	push_back(p2);
}

void
GenerationProbabilityCollection::push_back(const GenerationProbabilityPtr &other)
{
	BOOST_FOREACH(value_type &p, *this)
		if (p && p->IsCompatible(other)) {
			p = p->Clone();
			p->SetTotalEvents(p->GetTotalEvents() + other->GetTotalEvents());
			return;
		}
	std::vector<GenerationProbabilityPtr>::push_back(other);
}

bool
GenerationProbabilityCollection::IsCompatible(GenerationProbabilityConstPtr) const
{
	log_fatal("I should never be called.");
}

double
GenerationProbabilityCollection::GetLogGenerationProbability(const I3Particle &axis,
    const BundleConfiguration &bundle) const
{
	// Collect log probabilities from members
	std::vector<double> values(this->size(), -std::numeric_limits<double>::infinity());
	BOOST_FOREACH(const value_type &p, *this)
		if (p)
			values.push_back(p->GetLogGeneratedEvents(axis, bundle));

	// Calculate log(sum(exp)) in a numerically stable way
	double bias = *std::max_element(values.begin(), values.end());
	double prob = 0.;
	BOOST_FOREACH(double v, values)
		prob += std::exp(v-bias);

	return bias + std::log(prob);
}

SamplingSurfaceConstPtr
GenerationProbabilityCollection::GetInjectionSurface() const
{
	SamplingSurfaceConstPtr surface;
	BOOST_FOREACH(const value_type &p, *this) {
		if (p) {
			if (!surface)
				surface = p->GetInjectionSurface();
			else if (!(*surface == *(p->GetInjectionSurface())))
				log_fatal("All injection surfaces in a collection must be identical!");
		}
	}

	return surface;
}

GenerationProbabilityPtr
GenerationProbabilityCollection::Clone() const
{
	return boost::make_shared<GenerationProbabilityCollection>(*this);
}

template <typename Archive>
void
GenerationProbabilityCollection::serialize(Archive &ar, unsigned version)
{
	if (version > 0)
		log_fatal_stream("Version "<<version<<" is from the future");

	ar & make_nvp("GenerationProbability", base_object<GenerationProbability>(*this));
	ar & make_nvp("Vector", base_object<std::vector<GenerationProbabilityPtr> >(*this));
}

GenerationProbabilityPtr
operator*=(GenerationProbabilityPtr p, double n)
{
	p->SetTotalEvents(p->GetTotalEvents()*n);

	return p;
}

GenerationProbabilityPtr
operator*(GenerationProbabilityPtr p, double n)
{
	GenerationProbabilityPtr pn = p->Clone();
	pn *= n;

	return pn;
}

GenerationProbabilityPtr
operator*(double n, GenerationProbabilityPtr p)
{
	return p*n;
}

GenerationProbabilityPtr
operator+(GenerationProbabilityPtr p1, GenerationProbabilityPtr p2)
{

	// If one or both is a collection, merge
	boost::shared_ptr<GenerationProbabilityCollection> c1 =
	    boost::dynamic_pointer_cast<GenerationProbabilityCollection>(p1);
	boost::shared_ptr<GenerationProbabilityCollection> c2 =
	    boost::dynamic_pointer_cast<GenerationProbabilityCollection>(p2);
	if (c1 && c2) {
		c1 = boost::make_shared<GenerationProbabilityCollection>(*c1);
		BOOST_FOREACH(const GenerationProbabilityPtr &other, *c2)
			c1->push_back(other);
		return c1;
	} else if (c1) {
		c1 = boost::make_shared<GenerationProbabilityCollection>(*c1);
		c1->push_back(p2);
		return c1;
	} else if (c2) {
		c2 = boost::make_shared<GenerationProbabilityCollection>(*c2);
		c2->push_back(c1);
		return c2;
	} else if (p1->IsCompatible(p2)) {
		// If the two elements are identical, just scale up.
		GenerationProbabilityPtr p = p1->Clone();
		p->SetTotalEvents(p1->GetTotalEvents() + p2->GetTotalEvents());
		return p;
	} else {
		// If all else fails, start a new collection
		return boost::make_shared<GenerationProbabilityCollection>(p1, p2);
	}
}

I3Particle
Generator::CreateParallelTrack(double radius, double azimuth,
    const I3Surfaces::Surface &surface, const I3Particle &axis)
{
	I3Particle track;
	track.SetLocationType(I3Particle::InIce);
	track.SetType(I3Particle::MuMinus);
	track.SetDir(axis.GetDir());
	track.SetSpeed(I3Constants::c);
	track.SetPos(axis.GetPos());
	track.SetTime(axis.GetTime());

	if (radius > 0) {
		// Shift the track parallel to the axis
		I3Position offset(radius, 0, 0);
		offset.RotateY(axis.GetDir().GetZenith());
		offset.RotateZ(azimuth);
		offset += axis.GetPos();
		// Find the distance from the offset position to the sampling
		// surface, and shift so that all tracks have their origin
		// on the surface, but remain in a plane
		double shift =
		    surface.GetIntersection(offset, track.GetDir()).first
		    -surface.GetIntersection(axis.GetPos(), axis.GetDir()).first;
		if (std::isfinite(shift)) {
			track.SetTime(axis.GetTime() + shift/track.GetSpeed());
			track.SetPos(offset + shift*track.GetDir());
		} else {
			track.SetPos(offset);
		}
	}

	return track;
}

/**
 * @brief Interface between Generator and IceTray
 */
class GeneratorModule : public I3Module {
public:
	GeneratorModule(const I3Context &ctx) : I3Module(ctx), maxEvents_(0), numEvents_(0)
	{
		AddOutBox("OutBox");
		AddParameter("Generator", "Muon bundle generator", generator_);

		mctreeName_ = "I3MCTree";
	}

	void Configure()
	{
		GetParameter("Generator", generator_);

		rng_ = context_.Get<I3RandomServicePtr>();
		if (!rng_)
			log_fatal("No RandomService configured!");
		maxEvents_ = size_t(std::floor(generator_->GetTotalEvents()));

		firstFrame_ = true;
	}

	void DAQ(I3FramePtr frame)
	{
		if (firstFrame_) {
			firstFrame_ = false;
			I3FramePtr sframe = boost::make_shared<I3Frame>('S');
			sframe->Put(GetName(), generator_);
			PushFrame(sframe);
		}

		I3MCTreePtr mctree = boost::make_shared<I3MCTree>();
		BundleConfiguration bundlespec;

		generator_->Generate(*rng_, *mctree, bundlespec);

		frame->Put(mctreeName_, mctree);

		PushFrame(frame);
		if (++numEvents_ >= maxEvents_)
			RequestSuspension();
	}

	void Finish();
private:
	GeneratorPtr generator_;
	I3RandomServicePtr rng_;
	size_t maxEvents_, numEvents_;
	std::string mctreeName_;
	bool firstFrame_;
};

// Out-of-line virtual method definition to force the vtable into this translation unit
void GeneratorModule::Finish() {}

}

I3_SERIALIZABLE(I3MuonGun::GenerationProbability);
I3_SERIALIZABLE(I3MuonGun::GenerationProbabilityCollection);
I3_SERIALIZABLE(I3MuonGun::Generator);
I3_MODULE(I3MuonGun::GeneratorModule);


#include "OcTreeMapper.hpp"

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

#include <pcl/common/transforms.h>

using namespace slam3d;

octomap::OcTree* initOcTree(const OctoMapConfiguration &conf)
{
	octomap::OcTree* tree = new octomap::OcTree(conf.resolution);
	tree->setOccupancyThres(conf.occupancyThres);
	tree->setProbHit(conf.probHit);
	tree->setProbMiss(conf.probMiss);
	tree->setClampingThresMin(conf.clampingThresMin);
	tree->setClampingThresMax(conf.clampingThresMax);
	return tree;
}

OcTreeMapper::OcTreeMapper(std::string const& name)
    : OcTreeMapperBase(name)
{
}

OcTreeMapper::OcTreeMapper(std::string const& name, RTT::ExecutionEngine* engine)
    : OcTreeMapperBase(name, engine)
{
}

OcTreeMapper::~OcTreeMapper()
{
}

void OcTreeMapper::addScanToMap(PointCloudMeasurement::Ptr scan, const Transform& pose)
{
	PointCloud::Ptr tempCloud(new PointCloud);
	Transform sensor = pose * scan->getSensorPose();
	pcl::transformPointCloud(*(scan->getPointCloud()), *tempCloud, sensor.matrix());

	octomap::Pointcloud octoCloud;
	for(PointCloud::iterator it = tempCloud->begin(); it < tempCloud->end(); ++it)
	{
		octoCloud.push_back(octomap::point3d(it->x, it->y,it->z));
	}
	Eigen::Vector3d origin = sensor.translation();
	mOcTree->insertPointCloud(octoCloud, octomap::point3d(origin(0), origin(1), origin(2)), mOctreeConf.rangeMax, true, true);
}

void OcTreeMapper::clearMap()
{
	// Reset OctoMap (OcTree::clear is currently not working)
	delete mOcTree;
	mOcTree = initOcTree(mOctreeConf);
}

void OcTreeMapper::buildMLS()
{
	mLogger->message(WARNING, "Generating MLS from OcTree is not implemented.");
/*
	mLogger->message(DEBUG, "Generating MLS from OcTree.");
	mMultiLayerMap->clear();
	for(octomap::OcTree::leaf_iterator leaf = mOcTree->begin_leafs() ; leaf != mOcTree->end_leafs(); ++leaf)
	{
		if(!mOcTree->isNodeOccupied(*leaf))
		{
			continue;
		}
		double half_size = leaf.getSize() / 2.0;
		double half_res = mGridConf.resolution / 2.0;
		double x_min = leaf.getX() - half_size + half_res;
		double y_min = leaf.getY() - half_size + half_res;
		double x_max = leaf.getX() + half_size;
		double y_max = leaf.getY() + half_size;

		for(double y = y_min; y < y_max; y += mGridConf.resolution)
		{
			for(double x = x_min; x < x_max; x += mGridConf.resolution)
			{
				double z_low  = std::max(mGridConf.min_z, leaf.getZ() - half_size);
				double z_high = std::min(mGridConf.max_z, leaf.getZ() + half_size);
				if(z_high > z_low)
				{
					envire::SurfacePatch patch(z_high, 0, z_high - z_low, envire::SurfacePatch::VERTICAL);
					mMultiLayerMap->update(Eigen::Vector2d(x, y) , patch);
				}
			}
		}
	}
*/
}

void OcTreeMapper::sendMap()
{
	// Build and publish the MLS-Map
	mOcTree->updateInnerOccupancy();
	mOcTree->writeBinaryConst("slam3d_octomap.bt");
	buildMLS();
	mMultiLayerMap.setTime(mCurrentTime);
	_mls.write(mMultiLayerMap.asSpatioTemporal());
}

bool OcTreeMapper::remove_dynamic_objects()
{
	mLogger->message(INFO, "Requested dynamic object removal.");
	timeval start = mClock->now();
	VertexObjectList vertices = mGraph->getVerticesFromSensor(mPclSensor->getName());
	unsigned deleted = 0;

	boost::unique_lock<boost::shared_mutex> guard(mGraphMutex);
	for(VertexObjectList::iterator v = vertices.begin(); v != vertices.end(); ++v)
	{
		// Cast to PointCloudMeasurement
		PointCloudMeasurement::Ptr m = boost::dynamic_pointer_cast<PointCloudMeasurement>(v->measurement);
		if(!m)
		{
			mLogger->message(WARNING, "Vertex given to remove_dynamic_objects is not a Pointcloud!");
			continue;
		}

		// Check each point, if it is in free OctoMap voxel
		PointCloud::Ptr cloud = m->getPointCloud();
		for(PointCloud::iterator p = cloud->begin(); p != cloud->end();)
		{
			base::Vector3d p_tf;
			p_tf[0] = p->x;
			p_tf[1] = p->y;
			p_tf[2] = p->z;
			p_tf = v->corrected_pose * m->getSensorPose() * p_tf;
			octomap::OcTreeNode* node = mOcTree->search(p_tf[0], p_tf[1], p_tf[2]);
			if(node && !mOcTree->isNodeOccupied(node))
			{
				deleted++;
				p = cloud->erase(p);
			}else
			{
				p++;
			}
		}
	}
	int duration = mClock->now().tv_sec - start.tv_sec;
	mLogger->message(INFO, (boost::format("Removed %1% dynamic points in %2% seconds.") % deleted % duration).str());
	return true;
}

bool OcTreeMapper::configureHook()
{
	if (! OcTreeMapperBase::configureHook())
		return false;

	mOctreeConf  = _octo_map_config.get();
	mOcTree = initOcTree(mOctreeConf);

	mLogger->message(INFO, " = OctoMap - Parameters =");
	mLogger->message(INFO, (boost::format("resolution:       %1%") % mOctreeConf.resolution).str());
	mLogger->message(INFO, (boost::format("occupancyThres:   %1%") % mOctreeConf.occupancyThres).str());
	mLogger->message(INFO, (boost::format("probHit:          %1%") % mOctreeConf.probHit).str());
	mLogger->message(INFO, (boost::format("probMiss:         %1%") % mOctreeConf.probMiss).str());
	mLogger->message(INFO, (boost::format("clampingThresMin: %1%") % mOctreeConf.clampingThresMin).str());
	mLogger->message(INFO, (boost::format("clampingThresMax: %1%") % mOctreeConf.clampingThresMax).str());
	mLogger->message(INFO, (boost::format("rangeMax:         %1%") % mOctreeConf.rangeMax).str());

	return true;
}

bool OcTreeMapper::startHook()
{
	if (! OcTreeMapperBase::startHook())
		return false;
	return true;
}

void OcTreeMapper::updateHook()
{
	OcTreeMapperBase::updateHook();
}

void OcTreeMapper::errorHook()
{
	OcTreeMapperBase::errorHook();
}

void OcTreeMapper::stopHook()
{
	OcTreeMapperBase::stopHook();
}

void OcTreeMapper::cleanupHook()
{
	OcTreeMapperBase::cleanupHook();
	delete mOcTree;
}


#include "CoordinateConversions.h"
#include "CoordinateConverter.h"
#include "GeneralFunctions.h"
#include "GeneralDefinitions.h"
#include "AutoThreadLock.h"
#include "AutoWriteLock.h"
#include "AutoReadLock.h"

#include <macgyver/Exception.h>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/math/constants/constants.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <boost/make_shared.hpp>
#include <boost/functional/hash.hpp>
#include <ogr_geometry.h>
#include <ogr_spatialref.h>

#include <newbase/NFmiLocation.h>
#include <unordered_map>

std::unordered_map<std::size_t,SmartMet::CoordinateConverter> *coordinateConverterCache = nullptr;
SmartMet::ModificationLock coordinateConverterCache_modificationLock;

bool convert(const OGRSpatialReference *sr_from,const OGRSpatialReference *sr_to,int nCount,double *x,double *y)
{
  try
  {
    if (coordinateConverterCache == nullptr)
    {
      SmartMet::AutoWriteLock writeLock(&coordinateConverterCache_modificationLock);
      if (coordinateConverterCache == nullptr)
        coordinateConverterCache = new std::unordered_map<std::size_t,SmartMet::CoordinateConverter>;
    }

    std::size_t hash = (std::size_t)sr_from;
    boost::hash_combine(hash, (std::size_t)sr_to);

    {
      SmartMet::AutoReadLock readLock(&coordinateConverterCache_modificationLock);
      auto rec = coordinateConverterCache->find(hash);
      if (rec != coordinateConverterCache->end())
        return rec->second.convert(nCount,x,y);
    }

    SmartMet::AutoWriteLock writeLock(&coordinateConverterCache_modificationLock);
    if (coordinateConverterCache->size() > 1000000)
      coordinateConverterCache->clear();

    SmartMet::CoordinateConverter tr(sr_from,sr_to);
    coordinateConverterCache->insert(std::pair<std::size_t,SmartMet::CoordinateConverter>(hash,tr));
    return tr.convert(nCount,x,y);
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double getValue(double theAngle)
{
  try
  {
    if (::fabs(theAngle - 180) < 0.0000001)
      return 180;
    else if (::fabs(theAngle + 180) < 0.0000001)
      return -180;
    else if (theAngle > 180)
      return getValue(theAngle - 360);
    else if (theAngle < -180)
      return getValue(theAngle + 360);

    return theAngle;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

void latlon_to_rotatedLatlon(double lat,double lon,double southPoleLat,double southPoleLon,double& rotLat,double& rotLon)
{
  try
  {
    if (southPoleLat == -90)
    {
      rotLat = lat;
      rotLon = lon;
      return;
    }

    double SinYPole = sin((southPoleLat + 90) * kDegToRad);
    double CosYPole = cos((southPoleLat + 90) * kDegToRad);

    double SinXReg = sin((lon - southPoleLon) * kDegToRad);
    double CosXReg = cos((lon - southPoleLon) * kDegToRad);
    double SinYReg = sin(lat * kDegToRad);
    double CosYReg = cos(lat * kDegToRad);
    double SinYRot = CosYPole * SinYReg - SinYPole * CosYReg * CosXReg;
    SinYRot = std::min(std::max(SinYRot, -1.), 1.);
    double YRot = asin(SinYRot) * kOneRad;

    double CosYRot = cos(YRot * kDegToRad);
    double CosXRot = (CosYPole * CosYReg * CosXReg + SinYPole * SinYReg) / CosYRot;
    CosXRot = std::min(std::max(CosXRot, -1.), 1.);
    double SinXRot = CosYReg * SinXReg / CosYRot;
    double XRot = acos(CosXRot) * kOneRad;
    if (SinXRot < 0.)
      XRot = getValue(-XRot);
    else
      XRot = getValue(XRot);

    rotLon = XRot;
    rotLat = YRot;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

void rotatedLatlon_to_latlon(double rotLat,double rotLon,double southPoleLat,double southPoleLon,double& lat,double& lon)
{
  try
  {
    if (southPoleLat == -90)
    {
      lat = rotLat;
      lon = rotLon;
      return;
    }

    double SinYPole = sin((southPoleLat + 90) * kDegToRad);
    double CosYPole = cos((southPoleLat + 90) * kDegToRad);

    double SinXRot = sin(rotLon * kDegToRad);
    double CosXRot = cos(rotLon * kDegToRad);
    double SinYRot = sin(rotLat * kDegToRad);
    double CosYRot = cos(rotLat * kDegToRad);
    double SinYReg = CosYPole * SinYRot + SinYPole * CosYRot * CosXRot;
    SinYReg = std::min(std::max(SinYReg, -1.0), 1.0);

    double YReg = asin(SinYReg) * kOneRad;

    double CosYReg = cos(YReg * kDegToRad);
    double CosXReg = (CosYPole * CosYRot * CosXRot - SinYPole * SinYRot) / CosYReg;
    CosXReg = std::min(std::max(CosXReg, -1.0), 1.0);
    double SinXReg = CosYRot * SinXRot / CosYReg;

    double XRegVal = acos(CosXReg) * kOneRad;
    if (SinXReg < 0.)
      XRegVal = -XRegVal;

    XRegVal = XRegVal + southPoleLon;
    double XReg = XRegVal;

    lat = YReg;
    lon = XReg;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double latlon_distance(double lat1, double lon1, double lat2, double lon2)
{
  double p = 0.017453292519943295;    // Math.PI / 180
  double a = 0.5 - cos((lat2 - lat1) * p)/2 +
          cos(lat1 * p) * cos(lat2 * p) *
          (1 - cos((lon2 - lon1) * p))/2;

  return 12742 * asin(sqrt(a)); // 2 * R; R = 6371 km
}

double latlon_width(double lat, double longitudes)
{
  try
  {
    // Metric width of 1 degree
    double w = latlon_distance(lat,0,lat,1);
    return longitudes * w;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

double latlon_height(double lon, double latitudes)
{
  try
  {
    // Metric width of 1 degree
    double h = latlon_distance(0,lon,1,lon);
    return latitudes * h;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}

std::pair<std::vector<SmartMet::T::Coordinate>, std::vector<double>> getIsocirclePoints(double lon1, double lat1, double lon2, double lat2, std::size_t steps)
{
  try
  {
    // Sanity checks
    if (lon1 == lon2 && lat1 == lat2)
      throw Fmi::Exception(BCP, "Ill-defined isocircle: start and end points are equal");

    if (std::abs(lon1 - lon2) == 180 && std::abs(lat1 - (90 - lat2)) == 90)
      throw Fmi::Exception(BCP, "Ill-defined isocircle: points at opposing ends of the earth");

    if (steps < 1 || steps > 10000)
      throw Fmi::Exception(BCP,"Number of points on isocircle must be 1-10000, not " + boost::lexical_cast<std::string>(steps));

    // Calculate bearing and distance to be travelled

    NFmiLocation startpoint(lon1, lat1);
    NFmiLocation endpoint(lon2, lat2);

    double bearing = startpoint.Direction(endpoint);
    double distance = startpoint.Distance(endpoint);

    std::vector<SmartMet::T::Coordinate> coordinates;
    auto cc = startpoint.GetLocation();
    coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));

    std::vector<double> distances;
    distances.emplace_back(0);

    for (std::size_t i = 1; i < steps; i++)
    {
      // Should this be fixed? Probably not - the coordinates should behave the same
      double dist = i * distance / steps;
#ifdef WGS84
      auto loc = startpoint.GetLocation(bearing, dist);
#else
      const bool pacific_view = false;
      auto loc = startpoint.GetLocation(bearing, dist, pacific_view);
#endif
      auto cc = loc.GetLocation();
      coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));
      distances.emplace_back(dist / 1000.0);
    }
    cc = endpoint.GetLocation();
    coordinates.emplace_back(SmartMet::T::Coordinate(cc.X(),cc.Y()));
    distances.emplace_back(distance / 1000.0);

    return std::make_pair(coordinates, distances);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

void latLon_bboxByCenter(double centerX,double centerY,double metricWidth,double metricHeight,double& lon1,double& lat1,double& lon2,double& lat2)
{
  try
  {
    OGRSpatialReference sr_latlon;
    sr_latlon.importFromEPSG(4326);
    sr_latlon.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);

    OGRSpatialReference sr_wgs84_world_mercator;
    sr_wgs84_world_mercator.importFromEPSG(3395);
    sr_wgs84_world_mercator.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);

    OGRCoordinateTransformation *transformation = OGRCreateCoordinateTransformation(&sr_latlon,&sr_wgs84_world_mercator);
    OGRCoordinateTransformation *reverseTransformation = OGRCreateCoordinateTransformation(&sr_wgs84_world_mercator,&sr_latlon);

    transformation->Transform(1,&centerX,&centerY);

    double xx1 = centerX - metricWidth/2;
    double yy1 = centerY - metricHeight/2;
    double xx2 = centerX + metricWidth/2;
    double yy2 = centerY + metricHeight/2;

    // Converting metric coordinates to latlon coordinates.

    reverseTransformation->Transform(1,&xx1,&yy1);
    reverseTransformation->Transform(1,&xx2,&yy2);

    if (transformation != nullptr)
      OCTDestroyCoordinateTransformation(transformation);

    if (reverseTransformation != nullptr)
      OCTDestroyCoordinateTransformation(reverseTransformation);

    lon1 = xx1;
    lat1 = yy1;
    lon2 = xx2;
    lat2 = yy2;
  }
  catch (...)
  {
    throw Fmi::Exception(BCP,"Operation failed!",nullptr);
  }
}


// -*- mode: c++ -*-
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, JSK Lab
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/o2r other materials provided
 *     with the distribution.
 *   * Neither the name of the JSK Lab 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include "jsk_perception/fisheye_to_panorama.h"
#include <jsk_topic_tools/log_utils.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <algorithm>
#include <math.h>
#include <boost/assign.hpp>

#define PI 3.141592

namespace jsk_perception
{
  void FisheyeToPanorama::onInit()
  {
    DiagnosticNodelet::onInit();
    pnh_->param("use_panorama", use_panorama_, false);
    pnh_->param("simple_panorama", simple_panorama_, false);
    pub_undistorted_image_ = advertise<sensor_msgs::Image>(
      *pnh_, "output", 1);
    if(use_panorama_ && simple_panorama_)
      pub_undistorted_bilinear_image_ = advertise<sensor_msgs::Image>(*pnh_, "output_bilinear", 1);

    srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
    dynamic_reconfigure::Server<Config>::CallbackType f =
      boost::bind (&FisheyeToPanorama::configCallback, this, _1, _2);
    srv_->setCallback (f);

    scale_ = 0.5;
    upside_down_ = false;
    offset_degree_ = 180.0;
    onInitPostProcess();
  }

  void FisheyeToPanorama::configCallback(Config &new_config, uint32_t level)
  {
    max_degree_ = new_config.degree;
    scale_ = new_config.scale;
    upside_down_ = new_config.upside_down;
    offset_degree_ = new_config.offset_degree;
  }

  void FisheyeToPanorama::subscribe()
  {
    sub_image_ = pnh_->subscribe("input", 1, &FisheyeToPanorama::rectify, this);
    ros::V_string names = boost::assign::list_of("~input");
    jsk_topic_tools::warnNoRemap(names);
  }

  void FisheyeToPanorama::unsubscribe()
  {
    sub_image_.shutdown();
  }

  void FisheyeToPanorama::rectify(const sensor_msgs::Image::ConstPtr& image_msg)
  {
    cv::Mat distorted = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8)->image;
    int l = distorted.rows / 2;

    if(use_panorama_){
      float min_degree = 30;
      float min_radian = min_degree * 3.14159265 /180.0;
      float tan_min_radian = tan(min_radian);
      const float K = 341.656050955;// x = K * theta
      if(simple_panorama_){
        cv::Mat undistorted(l, l*4, CV_8UC3);
        cv::Mat undistorted_bilinear(l, l*4, CV_8UC3);
        for(int i = 0; i < undistorted.rows; ++i){
          for(int j = 0; j < undistorted.cols; ++j){

            double radius = l - i;
            double theta = 2.0 * PI * (double)(-j) / (double)(4.0 * l);
            double fTrueX = radius * cos(theta);
            double fTrueY = radius * sin(theta);

            int x = (int)round(fTrueX) + l;
            int y = l - (int)round(fTrueY);
            if (x >= 0 && x < (2 * l) && y >= 0 && y < (2 * l))
              {
                for(int c = 0; c < undistorted.channels(); ++c)
                  undistorted.data[ i * undistorted.step + j * undistorted.elemSize() + c ]
                    = distorted.data[x * distorted.step + y * distorted.elemSize() + c];
              }

            fTrueX = fTrueX + (double)l;
            fTrueY = (double)l - fTrueY;

            int iFloorX = (int)floor(fTrueX);
            int iFloorY = (int)floor(fTrueY);
            int iCeilingX = (int)ceil(fTrueX);
            int iCeilingY = (int)ceil(fTrueY);

            if (iFloorX < 0 || iCeilingX < 0 ||
                iFloorX >= (2 * l) || iCeilingX >= (2 * l) ||
                iFloorY < 0 || iCeilingY < 0 ||
                iFloorY >= (2 * l) || iCeilingY >= (2 * l)) continue;

            double fDeltaX = fTrueX - (double)iFloorX;
            double fDeltaY = fTrueY - (double)iFloorY;

            cv::Mat clrTopLeft(1,1, CV_8UC3), clrTopRight(1,1, CV_8UC3), clrBottomLeft(1,1, CV_8UC3), clrBottomRight(1,1, CV_8UC3);
            for(int c = 0; c < undistorted.channels(); ++c){
              clrTopLeft.data[ c ] = distorted.data[iFloorX * distorted.step + iFloorY * distorted.elemSize() + c];
              clrTopRight.data[ c ] = distorted.data[iCeilingX * distorted.step + iFloorY * distorted.elemSize() + c];
              clrBottomLeft.data[ c ] = distorted.data[iFloorX * distorted.step + iCeilingY * distorted.elemSize() + c];
              clrBottomRight.data[ c ] = distorted.data[iCeilingX * distorted.step + iCeilingY * distorted.elemSize() + c];
            }

            double fTop0 = interpolate(fDeltaX, clrTopLeft.data[0], clrTopRight.data[0]);
            double fTop1 = interpolate(fDeltaX, clrTopLeft.data[1], clrTopRight.data[1]);
            double fTop2 = interpolate(fDeltaX, clrTopLeft.data[2], clrTopRight.data[2]);
            double fBottom0 = interpolate(fDeltaX, clrBottomLeft.data[0], clrBottomRight.data[0]);
            double fBottom1 = interpolate(fDeltaX, clrBottomLeft.data[1], clrBottomRight.data[1]);
            double fBottom2 = interpolate(fDeltaX, clrBottomLeft.data[2], clrBottomRight.data[2]);

            int i0 = (int)round(interpolate(fDeltaY, fTop0, fBottom0));
            int i1 = (int)round(interpolate(fDeltaY, fTop1, fBottom1));
            int i2 = (int)round(interpolate(fDeltaY, fTop2, fBottom2));

            i0 = std::min(255, std::max(i0, 0));
            i1 = std::min(255, std::max(i1, 0));
            i2 = std::min(255, std::max(i2, 0));

            undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 0] = i0;
            undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 1] = i1;
            undistorted_bilinear.data[ i * undistorted_bilinear.step + j * undistorted_bilinear.elemSize() + 2] = i2;
          }
        }

        pub_undistorted_image_.publish(
                                       cv_bridge::CvImage(
                                                          image_msg->header,
                                                          image_msg->encoding,
                                                          undistorted).toImageMsg());
        pub_undistorted_bilinear_image_.publish(cv_bridge::CvImage(
                                                                   image_msg->header,
                                                                   image_msg->encoding,
                                                                   undistorted_bilinear).toImageMsg());
      }else{
        cv::Mat undistorted(int(l * 1.0 / tan_min_radian*scale_), int(l *  2.0 * PI * scale_), CV_8UC3);
        int center_x = distorted.rows/2, center_y = distorted.cols/2;

	int offset_jndex = offset_degree_ / 180.0 * PI * l * scale_;
        for(int i = 0; i < undistorted.rows; ++i){
          for(int j = 0; j < undistorted.cols; ++j){
            float phi = PI / 2;
            if(i)
              phi = atan(l * 1.0 /i*scale_) + 0.5;
            float theta = (j-int(undistorted.cols/2))/scale_ * 1.0/l;
            int x = K * phi * cos(theta) + center_x;
            int y = K * phi * sin(theta) + center_y;
            for(int c = 0; c < undistorted.channels(); ++c){
	      int index = undistorted.rows - 1 - i;
	      if( upside_down_ )
		index = i;
	      int jndex = j + offset_jndex;
	      if(jndex > undistorted.cols - 1)
		jndex -= undistorted.cols - 1;
              undistorted.data[ index  * undistorted.step + jndex * undistorted.elemSize() + c ]
                = distorted.data[ x * distorted.step + y * distorted.elemSize() + c];
            }
          }
        }
        pub_undistorted_image_.publish(
                                       cv_bridge::CvImage(
                                                          image_msg->header,
                                                          image_msg->encoding,
                                                          undistorted).toImageMsg());
      }
    }else{
      float max_degree = max_degree_;
      float max_radian = max_degree * 3.14159265 /180.0;
      float tan_max_radian = tan(max_radian);
      const float K = 341.656050955;
      const float absolute_max_degree = 85;
      const float absolute_max_radian = absolute_max_degree * 3.14159265 /180.0;
      float max_radius = max_radian * K;
      cv::Mat undistorted(int(l * tan_max_radian * 2 * scale_), int(l * tan_max_radian * 2 * scale_), CV_8UC3);
      int center_x = distorted.rows/2, center_y = distorted.cols/2;
      int un_center_x = undistorted.rows/(2 * scale_), un_center_y = undistorted.cols/(2*scale_);

      for(int i = 0; i < undistorted.rows; ++i){
        for(int j = 0; j < undistorted.cols; ++j){
          int diff_x = i/scale_-un_center_x, diff_y = j/scale_-un_center_y;
          float radius = sqrt(pow(diff_x, 2) + pow(diff_y, 2));
          float radian = atan(radius/l);
          if( radian < absolute_max_radian ){
            float multi = 0, new_x = center_x, new_y = center_y;
            if(radius){
              // float new_radius = radian * K;
              multi = radian * K / radius;
              new_x += diff_x * multi;
              new_y += diff_y * multi;
            }

            for(int c = 0; c < undistorted.channels(); ++c){
              undistorted.data[  i * undistorted.step + j * undistorted.elemSize() + c ]
                = distorted.data[ int(new_x) * distorted.step + int(new_y) * distorted.elemSize() + c];
            }
          }
        }
      }
      pub_undistorted_image_.publish(
                                     cv_bridge::CvImage(
                                                        image_msg->header,
                                                        image_msg->encoding,
                                                        undistorted).toImageMsg());
    }
  }
}

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS (jsk_perception::FisheyeToPanorama, nodelet::Nodelet);


//
// Copyright (c) 2009, Markus Rickert
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <algorithm>
#include <stack>
#include <boost/graph/graphviz.hpp>
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Spatial.h>
#include <rl/math/Unit.h>

#include "Exception.h"
#include "Dynamic.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		Dynamic::Dynamic() :
			Kinematic(),
			G(),
			invM(),
			invMx(),
			M(),
			V()
		{
		}

		Dynamic::~Dynamic()
		{
		}

		void
		Dynamic::calculateCentrifugalCoriolis()
		{
			this->calculateCentrifugalCoriolis(this->V);
		}

		void
		Dynamic::calculateCentrifugalCoriolis(::rl::math::Vector& V)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setAcceleration(tmp);
			this->setWorldGravity(0, 0, 0);

			this->inverseDynamics();
			V = this->getTorque();

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateGravity()
		{
			this->calculateGravity(this->G);
		}

		void
		Dynamic::calculateGravity(::rl::math::Vector& G)
		{
			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setAcceleration(tmp);

			this->inverseDynamics();
			G = this->getTorque();
		}

		void
		Dynamic::calculateMassMatrix()
		{
			this->calculateMassMatrix(this->M);
		}

		void
		Dynamic::calculateMassMatrix(::rl::math::Matrix& M)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setWorldGravity(0, 0, 0);

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setAcceleration(tmp);
				this->inverseDynamics();

				M.col(i) = this->getTorque();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateMassMatrixInverse()
		{
			this->calculateMassMatrixInverse(this->invM);
		}

		void
		Dynamic::calculateMassMatrixInverse(::rl::math::Matrix& invM)
		{
			::rl::math::Vector g(3);
			this->getWorldGravity(g);

			::rl::math::Vector tmp(this->getDof());
			tmp.setZero(); //TODO

			this->setVelocity(tmp);
			this->setWorldGravity(0, 0, 0);

			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				for (::std::size_t j = 0; j < this->getDof(); ++j)
				{
					tmp(j) = i == j ? 1 : 0;
				}

				this->setTorque(tmp);
				this->forwardDynamics();

				invM.col(i) = this->getAcceleration();
			}

			this->setWorldGravity(g);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse()
		{
			this->calculateOperationalMassMatrixInverse(this->J, this->invM, this->invMx);
		}

		void
		Dynamic::calculateOperationalMassMatrixInverse(const ::rl::math::Matrix& J, const ::rl::math::Matrix& invM, ::rl::math::Matrix& invMx) const
		{
			invMx = J * invM * J.transpose(); // TODO
		}

		Model*
		Dynamic::clone() const
		{
			return new Dynamic(*this);
		}

		void
		Dynamic::eulerCauchy(const ::rl::math::Real& dt)
		{
			::rl::math::Vector y = this->getPosition();
			::rl::math::Vector dy = this->getVelocity();

			this->forwardDynamics();

			// f
			::rl::math::Vector f = this->getAcceleration();

			// y_0 + dy_0 * dt
			y += dt * dy; // TODO
			// dy_0 + f * dt
			dy += dt * f; // TODO

			this->setPosition(y);
			this->setVelocity(dy);
			this->setAcceleration(f);
		}

		void
		Dynamic::forwardDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->forwardDynamics2();
			}

			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->forwardDynamics3();
			}
		}

		const ::rl::math::Vector&
		Dynamic::getCentrifugalCoriolis() const
		{
			return this->V;
		}

		const ::rl::math::Vector&
		Dynamic::getGravity() const
		{
			return this->G;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrixInverse() const
		{
			return this->invM;
		}

		const ::rl::math::Matrix&
		Dynamic::getMassMatrix() const
		{
			return this->M;
		}

		const ::rl::math::Matrix&
		Dynamic::getOperationalMassMatrixInverse() const
		{
			return this->invMx;
		}

		void
		Dynamic::getWorldGravity(::rl::math::Real& x, ::rl::math::Real& y, ::rl::math::Real& z) const
		{
			dynamic_cast<World*>(this->tree[this->root].get())->getGravity(x, y, z);
		}

		void
		Dynamic::getWorldGravity(::rl::math::Vector& xyz) const
		{
			this->getWorldGravity(xyz(0), xyz(1), xyz(2));
		}

		void
		Dynamic::inverseDynamics()
		{
			for (::std::vector<Element*>::iterator i = this->elements.begin(); i != this->elements.end(); ++i)
			{
				(*i)->inverseDynamics1();
			}

			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseDynamics2();
			}
		}

		void
		Dynamic::inverseForce()
		{
			for (::std::vector<Element*>::reverse_iterator i = this->elements.rbegin(); i != this->elements.rend(); ++i)
			{
				(*i)->inverseForce();
			}
		}

		void
		Dynamic::rungeKuttaNystrom(const ::rl::math::Real& dt)
		{
			::rl::math::Vector y0 = this->getPosition();
			::rl::math::Vector dy0 = this->getVelocity();

			this->forwardDynamics();

			::rl::math::Vector f = this->getAcceleration();

			// k1 = dt / 2 * f
			::rl::math::Vector k1 = dt / 2 * f;

			// y_0 + dt / 2 * dy_0 + dt / 4 * k_1
			::rl::math::Vector y = y0 + dt / 2 * dy0 + dt / 4 * k1; // TODO
			// dy_0 + k1
			::rl::math::Vector dy = dy0 + k1;

			this->setPosition(y);
			this->setVelocity(dy);
			this->forwardDynamics();

			// k2 = dt / 2 * f
			::rl::math::Vector k2 = dt / 2 * this->getAcceleration();

			// dy_0 + k_2
			dy = dy0 + k2;

			this->setVelocity(dy);
			this->forwardDynamics();

			// k3 = dt / 2 * f
			::rl::math::Vector k3 = dt / 2 * this->getAcceleration();

			// y_0 + dt * dy_0 + dt * k_3
			y = y0 + dt * dy0 + dt * k3; // TODO
			// dy_0 + 2 * k_3
			dy = dy0 + 2 * k3;

			this->setPosition(y);
			this->setVelocity(dy);
			this->forwardDynamics();

			// k4 = dt / 2 * f
			::rl::math::Vector k4 = dt / 2 * this->getAcceleration();

			// y_0 + dy_0 * dt + dt / 3 * (k_1 + k_2 + k_3)
			y = y0 + dy0 * dt + dt / 3 * (k1 + k2 + k3); // TODO
			// dy_0 + 1 / 3 * (k_1 + 2 * k_2 + 2 * k_3 + k_4)
			dy = dy0 + 1.0f / 3.0f * (k1 + 2 * k2 + 2 * k3 + k4); // TODO

			this->setPosition(y);
			this->setVelocity(dy);
			this->setAcceleration(f);
		}

		void
		Dynamic::setWorldGravity(const ::rl::math::Real& x, const ::rl::math::Real& y, const ::rl::math::Real& z)
		{
			dynamic_cast<World*>(this->tree[this->root].get())->setGravity(x, y, z);
		}

		void
		Dynamic::setWorldGravity(const ::rl::math::Vector& xyz)
		{
			this->setWorldGravity(xyz(0), xyz(1), xyz(2));
		}

		void
		Dynamic::update()
		{
			Kinematic::update();

			this->M.resize(this->getDof(), this->getDof());
			this->V.resize(this->getDof());
			this->G.resize(this->getDof());
			this->invM.resize(this->getDof(), this->getDof());
			this->invMx.resize(6 * this->getOperationalDof(), 6 * this->getOperationalDof());
		}
	}
}


#include <algorithm>
#include <cassert>
#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <random>

#include <clipper/constants.hpp>
#include <clipper/containers.hpp>
#include <clipper/dlib_dependencies.hpp>
#include <clipper/logging.hpp>
#include <clipper/metrics.hpp>
#include <clipper/threadpool.hpp>
#include <clipper/util.hpp>

#include <boost/circular_buffer.hpp>

namespace clipper {

const std::string LOGGING_TAG_CONTAINERS = "CONTAINERS";

ModelContainer::~ModelContainer() {
  std::lock_guard<std::mutex> activity_lock(*activity_mtx_);
  *active_ = false;
}

ModelContainer::ModelContainer(VersionedModelId model, int container_id,
                               int replica_id, InputType input_type,
                               int batch_size)
    : model_(model),
      container_id_(container_id),
      replica_id_(replica_id),
      input_type_(input_type),
      batch_size_(batch_size),
      latency_hist_("container:" + model.serialize() + ":" +
                        std::to_string(replica_id) + ":prediction_latency",
                    "microseconds", HISTOGRAM_SAMPLE_SIZE),
      active_(std::make_shared<bool>(true)),
      activity_mtx_(std::make_shared<std::mutex>()),
      max_batch_size_(1),
      max_latency_(0),
      // These distribution constants were selected based on the empirical
      // observation that they result in reasonably fast, yet controlled
      // exploration of new batch sizes
      explore_dist_mu_(.1),
      explore_dist_std_(.05),
      // The budget decay constant was selected based on empirical tuning
      budget_decay_(.92),
      exploration_distribution_(std::normal_distribution<double>(
          explore_dist_mu_, explore_dist_std_)),
      exploration_engine_(std::default_random_engine(
          std::chrono::system_clock::now().time_since_epoch().count())) {
  std::string model_str = model.serialize();
  log_info_formatted(LOGGING_TAG_CONTAINERS,
                     "Creating new ModelContainer for model {}, id: {}",
                     model_str, std::to_string(container_id));
}

void ModelContainer::set_inactive() { connected_ = false; }

bool ModelContainer::is_active() { return connected_; }

void ModelContainer::add_processing_datapoint(
    size_t batch_size, long long processing_latency_micros) {
  if (batch_size <= 0 || processing_latency_micros <= 0) {
    throw std::invalid_argument(
        "Invalid processing datapoint: Batch size and latency must be "
        "positive.");
  }

  latency_hist_.insert(processing_latency_micros);

  boost::unique_lock<boost::shared_mutex> lock(datapoints_mtx_);

  auto bs_search = processing_datapoints_.find(batch_size);
  if (bs_search == processing_datapoints_.end()) {
    double info_size = 1;
    double std = 0;
    LatencyInfo latency_info = std::make_tuple(
        std::move(info_size), static_cast<double>(processing_latency_micros),
        std::move(std));
    processing_datapoints_.emplace(static_cast<EstimatorBatchSize>(batch_size),
                                   std::move(latency_info));
  } else {
    LatencyInfo &old_info = bs_search->second;
    LatencyInfo new_info = update_mean_std(
        old_info, static_cast<double>(processing_latency_micros));
    processing_datapoints_[batch_size] = std::move(new_info);
  }

  max_latency_ = std::max(processing_latency_micros, max_latency_);

  EstimatorFittingThreadPool::submit_job(model_, replica_id_, [
    this, container_activity_mtx = activity_mtx_, container_active = active_
  ]() {
    std::lock_guard<std::mutex> activity_lock(*container_activity_mtx);
    if (!(*container_active)) {
      return;
    }
    try {
      fit_estimator();
    } catch (std::exception const &ex) {
      log_error_formatted(LOGGING_TAG_CONTAINERS,
                          "Error fitting batch size estimator: {}", ex.what());
    }
  });
}

ModelContainer::LatencyInfo ModelContainer::update_mean_std(
    LatencyInfo &info, double new_latency) {
  double info_size = std::get<0>(info);
  double mu = std::get<1>(info);
  double std = std::get<2>(info);

  double new_mu;
  double new_std;
  std::tie(new_mu, new_std) =
      IterativeUpdater::calculate_new_mean_std(info_size, mu, std, new_latency);

  return std::make_tuple(info_size + 1, new_mu, new_std);
}

void ModelContainer::set_batch_size(int batch_size) {
  batch_size_ = batch_size;
}

BatchSizeInfo ModelContainer::get_batch_size(Deadline deadline) {
  BatchSizeDeterminationMethod method;
  if (batch_size_ != DEFAULT_BATCH_SIZE) {
    method = BatchSizeDeterminationMethod::Default;
    return std::make_pair(batch_size_, method);
  }

  double budget =
      static_cast<double>(std::chrono::duration_cast<std::chrono::microseconds>(
                              deadline - std::chrono::system_clock::now())
                              .count());
  // Decay the provided latency budget by a pre-specified factor
  // in order to provide enough slack for delivering the response
  // to the user and/or coping with an anomolously high
  // processing latency
  budget = budget * budget_decay_;

  size_t curr_batch_size;
  if (budget > static_cast<double>(max_latency_)) {
    curr_batch_size = explore();
    method = BatchSizeDeterminationMethod::Exploration;
  } else {
    curr_batch_size = estimate(budget);
    method = BatchSizeDeterminationMethod::Estimation;
  }
  max_batch_size_ = std::max(curr_batch_size, max_batch_size_);
  return std::make_pair(curr_batch_size, method);
}

void ModelContainer::fit_estimator() {
  std::unique_lock<std::mutex> estimator_lock(estimator_mtx_);
  boost::shared_lock<boost::shared_mutex> datapoints_lock(datapoints_mtx_);

  size_t num_datapoints = processing_datapoints_.size();

  std::vector<EstimatorLatency> x_vals;
  x_vals.reserve(num_datapoints);
  std::vector<EstimatorBatchSize> y_vals;
  y_vals.reserve(num_datapoints);

  // Calculate the pooled processing latency
  // variance across all batch sizes. This will
  // be used to obtain an estimate for the p99
  // processing latency at each batch size
  //
  // Pooled variance (https://en.wikipedia.org/wiki/Pooled_variance)
  // is used under the empirically tested assumption that latency variance
  // does not change significantly with batch size

  double pooled_std_num = 0;
  double pooled_std_denom = -1 * static_cast<double>(num_datapoints);

  for (auto &entry : processing_datapoints_) {
    LatencyInfo &latency_info = entry.second;
    double info_size = std::get<0>(latency_info);
    if (info_size >= MINIMUM_BATCH_SAMPLE_SIZE) {
      double lats_std = std::get<2>(latency_info);
      double lats_var = std::pow(lats_std, 2);
      pooled_std_num += (info_size - 1) * lats_var;
      pooled_std_denom += info_size;
    }
  }

  double pooled_std =
      std::sqrt(pooled_std_num / std::max(1.0, pooled_std_denom));

  // Using the pooled variance, obtain the
  // estimated p99 latency for each batch size

  EstimatorLatency fitting_lat;
  for (auto &entry : processing_datapoints_) {
    EstimatorBatchSize batch_size = entry.first;
    LatencyInfo &latency_info = entry.second;
    double lats_mean = std::get<1>(latency_info);
    double upper_bound_lat = lats_mean + (LATENCY_Z_SCORE * pooled_std);
    // Scale the upper bound latency in order to moderate the regularization
    // term associated with ridge regression
    fitting_lat(0) = upper_bound_lat * REGRESSION_DATA_SCALE_FACTOR;
    x_vals.push_back(fitting_lat);
    y_vals.push_back(std::move(batch_size));
  }

  datapoints_lock.unlock();

  estimator_ = estimator_trainer_.train(x_vals, y_vals);
}

size_t ModelContainer::explore() {
  auto mb_search =
      processing_datapoints_.find(static_cast<double>(max_batch_size_));
  if (mb_search != processing_datapoints_.end() &&
      std::get<0>(mb_search->second) < MINIMUM_BATCH_SAMPLE_SIZE) {
    // We don't have a large enough latency sample
    // corresponding to the maximum batch size, so
    // we won't update the maximum size
    return max_batch_size_;
  } else if (max_batch_size_ < ADDITIVE_EXPANSION_THRESHOLD) {
    return max_batch_size_ + 1;
  } else {
    double expansion_factor = exploration_distribution_(exploration_engine_);
    expansion_factor = std::max(0.0, expansion_factor);
    return static_cast<size_t>((1 + expansion_factor) * max_batch_size_);
  }
}

size_t ModelContainer::estimate(double budget) {
  std::lock_guard<std::mutex> lock(estimator_mtx_);
  EstimatorLatency estimator_budget;
  estimator_budget(0) =
      static_cast<double>(budget) * REGRESSION_DATA_SCALE_FACTOR;
  double estimate = estimator_(estimator_budget);
  return static_cast<size_t>(std::max(1.0, std::floor(estimate)));
}

ActiveContainers::ActiveContainers()
    : batch_sizes_(std::unordered_map<VersionedModelId, int>()),
      containers_(
          std::unordered_map<VersionedModelId,
                             std::map<int, std::shared_ptr<ModelContainer>>>(
              {})) {}

void ActiveContainers::add_container(VersionedModelId model, int connection_id,
                                     int replica_id, InputType input_type) {
  log_info_formatted(LOGGING_TAG_CONTAINERS,
                     "Adding new container - model: {}, version: {}, "
                     "connection ID: {}, replica ID: {}, input_type: {}",
                     model.get_name(), model.get_id(), connection_id,
                     replica_id, get_readable_input_type(input_type));
  boost::unique_lock<boost::shared_mutex> l{m_};

  // Set a default batch size of -1
  int batch_size = DEFAULT_BATCH_SIZE;
  auto batch_size_search = batch_sizes_.find(model);
  if (batch_size_search != batch_sizes_.end()) {
    batch_size = batch_size_search->second;
  }

  auto new_container = std::make_shared<ModelContainer>(
      model, connection_id, replica_id, input_type, batch_size);
  auto entry = containers_[new_container->model_];
  entry.emplace(replica_id, new_container);
  containers_[new_container->model_] = entry;
  assert(containers_[new_container->model_].size() > 0);
  log_active_containers();
}

void ActiveContainers::remove_container(VersionedModelId model,
                                        int replica_id) {
  boost::unique_lock<boost::shared_mutex> l{m_};

  auto replicas_map_entry = containers_.find(model);
  if (replicas_map_entry == containers_.end()) {
    log_error_formatted(LOGGING_TAG_CONTAINERS,
                        "Requested removing container {} for model {} NOT FOUND",
                        replica_id, model.serialize());
    return;
  }

  log_info_formatted(LOGGING_TAG_CONTAINERS,
                     "Removing container {} for model {}",
                     replica_id, model.serialize());

  size_t initial_size = containers_[model].size();

  for (auto it = containers_[model].begin(); it != containers_[model].end();) {
    if (it->first == replica_id) {
      (it->second)->set_inactive();
      it = containers_[model].erase(it);
    } else {
      ++it;
    }
  }

  assert(containers_[model].size() == initial_size - 1);

  if (containers_[model].size() == 0) {
    log_info_formatted(LOGGING_TAG_CONTAINERS,
                       "All containers of model {} are removed. Remove itself",
                       model.serialize());
    containers_.erase(model);
  }

  log_active_containers();
}

void ActiveContainers::register_batch_size(VersionedModelId model,
                                           int batch_size) {
  auto batch_size_entry = batch_sizes_.find(model);
  if (batch_size_entry != batch_sizes_.end()) {
    batch_sizes_.erase(model);
  }
  batch_sizes_.emplace(model, batch_size);
  auto matching_containers_entry = containers_.find(model);
  if (matching_containers_entry != containers_.end()) {
    for (auto &container : matching_containers_entry->second) {
      container.second->set_batch_size(batch_size);
    }
  }
}

void ActiveContainers::unregister_batch_size(VersionedModelId model) {
  auto batch_size_entry = batch_sizes_.find(model);
  if (batch_size_entry != batch_sizes_.end()) {
    batch_sizes_.erase(model);
  }
}

std::shared_ptr<ModelContainer> ActiveContainers::get_model_replica(
    const VersionedModelId &model, const int replica_id) {
  boost::shared_lock<boost::shared_mutex> l{m_};

  auto replicas_map_entry = containers_.find(model);
  if (replicas_map_entry == containers_.end()) {
    log_error_formatted(LOGGING_TAG_CONTAINERS,
                        "Requested container {} for model {} NOT FOUND",
                        replica_id, model.serialize());
    return nullptr;
  }

  std::map<int, std::shared_ptr<ModelContainer>> replicas_map =
      replicas_map_entry->second;
  auto replica_entry = replicas_map.find(replica_id);
  if (replica_entry != replicas_map.end()) {
    return replica_entry->second;
  } else {
    log_error_formatted(LOGGING_TAG_CONTAINERS,
                        "Requested replica {} for model {} NOT FOUND",
                        replica_id, model.serialize());
    return nullptr;
  }
}

std::map<int, std::shared_ptr<ModelContainer>>
ActiveContainers::get_replicas_for_model(const VersionedModelId &model) {
  boost::shared_lock<boost::shared_mutex> l{m_};

  auto replicas_map_entry = containers_.find(model);
  if (replicas_map_entry == containers_.end()) {
    log_error_formatted(LOGGING_TAG_CONTAINERS, "Requested model {} NOT FOUND",
                        model.serialize());
    return {};
  }

  return replicas_map_entry->second;
}

std::vector<VersionedModelId> ActiveContainers::get_known_models() {
  boost::shared_lock<boost::shared_mutex> l{m_};
  std::vector<VersionedModelId> keys;
  for (auto m : containers_) {
    keys.push_back(m.first);
  }
  return keys;
}

void ActiveContainers::log_active_containers() {
  std::stringstream log_msg;
  log_msg << "\nActive containers:\n";
  for (auto model : containers_) {
    log_msg << "\tModel: " << model.first.serialize() << "\n";
    for (auto r : model.second) {
      log_msg << "\t\trep_id: " << r.first
              << ", container_id: " << r.second->container_id_ << "\n";
    }
  }
  log_info(LOGGING_TAG_CONTAINERS, log_msg.str());
}
}  // namespace clipper


#ifndef INCLUDED_scheme_nest_maps_util_get_quaternion_component_HH
#define INCLUDED_scheme_nest_maps_util_get_quaternion_component_HH

#include "scheme/util/meta/util.hh"
#include <boost/utility/enable_if.hpp>

namespace scheme {
namespace nest {
namespace pmap {

/// all this get_quaternion_component mess is to handle eigen quaternions
template<class Q, class Index>
typename boost::enable_if<
	util::meta::has_const_subscript_oper<Q,typename Q::value_type const &,Index> ,
	typename Q::value_type const &    >::type
get_quaternion_component(Q const & q, Index i){ return q[i]; }

template<class Q, class Index>
typename boost::enable_if<
	util::meta::has_subscript_oper<Q,typename Q::value_type &,Index> ,
	typename Q::value_type &    >::type
get_quaternion_component(Q & q, Index i){ return q[i]; }

template<class Q, class Index>
typename boost::disable_if<
	util::meta::has_const_subscript_oper<Q,typename Q::Scalar const &,Index> ,
	typename Q::Scalar const &    >::type
get_quaternion_component(Q const & q, Index i){ return q.coeffs()[i]; }

template<class Q, class Index>
typename boost::disable_if<
	util::meta::has_subscript_oper<Q,typename Q::Scalar &,Index> ,
	typename Q::Scalar &    >::type
get_quaternion_component(Q & q, Index i){ return q.coeffs()[i]; }

}
}
}

#endif


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

#include <cmath>
#include <limits>
#include <utility>
#include <vector>

#include <amathutils_lib/amathutils.hpp>
#include <tf2_eigen/tf2_eigen.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "libwaypoint_follower/libwaypoint_follower.h"

using amathutils::deg2rad;

int WayPoints::getSize() const
{
  if (current_waypoints_.waypoints.empty())
    return 0;
  else
    return current_waypoints_.waypoints.size();
}

double WayPoints::getInterval() const
{
  if (current_waypoints_.waypoints.empty())
    return 0;

  // interval between 2 waypoints
  tf::Vector3 v1(current_waypoints_.waypoints[0].pose.pose.position.x,
                 current_waypoints_.waypoints[0].pose.pose.position.y, 0);

  tf::Vector3 v2(current_waypoints_.waypoints[1].pose.pose.position.x,
                 current_waypoints_.waypoints[1].pose.pose.position.y, 0);
  return tf::tfDistance(v1, v2);
}

geometry_msgs::Point WayPoints::getWaypointPosition(int waypoint) const
{
  geometry_msgs::Point p;
  if (waypoint > getSize() - 1 || waypoint < 0)
    return p;

  p = current_waypoints_.waypoints[waypoint].pose.pose.position;
  return p;
}

geometry_msgs::Quaternion WayPoints::getWaypointOrientation(int waypoint) const
{
  geometry_msgs::Quaternion q;
  if (waypoint > getSize() - 1 || waypoint < 0)
    return q;

  q = current_waypoints_.waypoints[waypoint].pose.pose.orientation;
  return q;
}

geometry_msgs::Pose WayPoints::getWaypointPose(int waypoint) const
{
  geometry_msgs::Pose pose;
  if (waypoint > getSize() - 1 || waypoint < 0)
    return pose;

  pose = current_waypoints_.waypoints[waypoint].pose.pose;
  return pose;
}

double WayPoints::getWaypointVelocityMPS(int waypoint) const
{
  if (waypoint > getSize() - 1 || waypoint < 0)
    return 0;

  return current_waypoints_.waypoints[waypoint].twist.twist.linear.x;
}

bool WayPoints::inDrivingDirection(int waypoint, geometry_msgs::Pose current_pose) const
{
  const LaneDirection dir = getLaneDirection(current_waypoints_);
  double x = calcRelativeCoordinate(current_waypoints_.waypoints[waypoint].pose.pose.position, current_pose).x;
  return (x < 0.0 && dir == LaneDirection::Backward) || (x >= 0.0 && dir == LaneDirection::Forward);
}

double DecelerateVelocity(double distance, double prev_velocity)
{
  double decel_ms = 1.0;  // m/s
  double decel_velocity_ms = std::sqrt(2 * decel_ms * distance);

  std::cout << "velocity/prev_velocity :" << decel_velocity_ms << "/" << prev_velocity << std::endl;
  if (decel_velocity_ms < prev_velocity)
  {
    return decel_velocity_ms;
  }
  else
  {
    return prev_velocity;
  }
}

// calculation relative coordinate of point from current_pose frame
geometry_msgs::Point calcRelativeCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(current_pose, inverse);
  tf::Transform transform = inverse.inverse();

  tf::Point p;
  pointMsgToTF(point_msg, p);
  tf::Point tf_p = transform * p;
  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);

  return tf_point_msg;
}

// calculation absolute coordinate of point on current_pose frame
geometry_msgs::Point calcAbsoluteCoordinate(geometry_msgs::Point point_msg, geometry_msgs::Pose current_pose)
{
  tf::Transform inverse;
  tf::poseMsgToTF(current_pose, inverse);

  tf::Point p;
  pointMsgToTF(point_msg, p);
  tf::Point tf_p = inverse * p;
  geometry_msgs::Point tf_point_msg;
  pointTFToMsg(tf_p, tf_point_msg);
  return tf_point_msg;
}

// distance between target 1 and target2 in 2-D
double getPlaneDistance(geometry_msgs::Point target1, geometry_msgs::Point target2)
{
  tf::Vector3 v1 = point2vector(target1);
  v1.setZ(0);
  tf::Vector3 v2 = point2vector(target2);
  v2.setZ(0);
  return tf::tfDistance(v1, v2);
}

double getRelativeAngle(geometry_msgs::Pose waypoint_pose, geometry_msgs::Pose vehicle_pose)
{
  geometry_msgs::Point relative_p1 = calcRelativeCoordinate(waypoint_pose.position, vehicle_pose);
  geometry_msgs::Point p2;
  p2.x = 1.0;
  geometry_msgs::Point relative_p2 = calcRelativeCoordinate(calcAbsoluteCoordinate(p2, waypoint_pose), vehicle_pose);
  tf::Vector3 relative_waypoint_v(relative_p2.x - relative_p1.x, relative_p2.y - relative_p1.y,
                                  relative_p2.z - relative_p1.z);
  relative_waypoint_v.normalize();
  tf::Vector3 relative_pose_v(1, 0, 0);
  double angle = relative_pose_v.angle(relative_waypoint_v) * 180 / M_PI;

  return angle;
}

LaneDirection getLaneDirection(const autoware_msgs::Lane& current_path)
{
  const LaneDirection pos_ret = getLaneDirectionByPosition(current_path);
  const LaneDirection vel_ret = getLaneDirectionByVelocity(current_path);
  const bool is_conflict =
    (pos_ret != vel_ret) && (pos_ret != LaneDirection::Error) && (vel_ret != LaneDirection::Error);
  return is_conflict ? LaneDirection::Error : (pos_ret != LaneDirection::Error) ? pos_ret : vel_ret;
}

LaneDirection getLaneDirectionByPosition(const autoware_msgs::Lane& current_path)
{
  if (current_path.waypoints.size() < 2)
  {
    return LaneDirection::Error;
  }
  LaneDirection positional_direction = LaneDirection::Error;
  for (size_t i = 1; i < current_path.waypoints.size(); i++)
  {
    const geometry_msgs::Pose& prev_pose = current_path.waypoints[i - 1].pose.pose;
    const geometry_msgs::Pose& next_pose = current_path.waypoints[i].pose.pose;
    const double rlt_x = calcRelativeCoordinate(next_pose.position, prev_pose).x;
    if (std::fabs(rlt_x) < 1e-3)
    {
      continue;
    }
    positional_direction = (rlt_x < 0) ? LaneDirection::Backward : LaneDirection::Forward;
    break;
  }
  return positional_direction;
}

LaneDirection getLaneDirectionByVelocity(const autoware_msgs::Lane& current_path)
{
  LaneDirection velocity_direction = LaneDirection::Error;
  for (const auto waypoint : current_path.waypoints)
  {
    const double& vel = waypoint.twist.twist.linear.x;
    if (std::fabs(vel) < 0.01)
    {
      continue;
    }
    velocity_direction = (vel < 0) ? LaneDirection::Backward : LaneDirection::Forward;
    break;
  }
  return velocity_direction;
}

class MinIDSearch
{
private:
  double val_min_;
  int idx_min_;
public:
  MinIDSearch() : val_min_(DBL_MAX), idx_min_(-1) {}
  void update(int index, double v)
  {
    if (v < val_min_)
    {
      idx_min_ = index;
      val_min_ = v;
    }
  }
  const int result() const
  {
    return idx_min_;
  }
  const bool isOK() const
  {
    return (idx_min_ != -1);
  }
};

// get closest waypoint from current pose
int getClosestWaypoint(const autoware_msgs::Lane &current_path, geometry_msgs::Pose current_pose)
{
  if (current_path.waypoints.size() < 2 || getLaneDirection(current_path) == LaneDirection::Error)
  {
    return -1;
  }

  WayPoints wp;
  wp.setPath(current_path);

  // search closest candidate within a certain meter
  double search_distance = 5.0;
  double angle_threshold = 90;
  MinIDSearch cand_idx, not_cand_idx;
  for (int i = 0; i < wp.getSize(); i++)
  {
    if (!wp.inDrivingDirection(i, current_pose))
      continue;
    double distance = getPlaneDistance(wp.getWaypointPosition(i), current_pose.position);
    not_cand_idx.update(i, distance);
    if (distance > search_distance)
      continue;
    if (getRelativeAngle(wp.getWaypointPose(i), current_pose) > angle_threshold)
      continue;
    cand_idx.update(i, distance);
  }
  return (!cand_idx.isOK()) ? not_cand_idx.result() : cand_idx.result();
}

// let the linear equation be "ax + by + c = 0"
// if there are two points (x1,y1) , (x2,y2), a = "y2-y1, b = "(-1) * x2 - x1" ,c = "(-1) * (y2-y1)x1 + (x2-x1)y1"
bool getLinearEquation(geometry_msgs::Point start, geometry_msgs::Point end, double *a, double *b, double *c)
{
  // (x1, y1) = (start.x, star.y), (x2, y2) = (end.x, end.y)
  double sub_x = std::fabs(start.x - end.x);
  double sub_y = std::fabs(start.y - end.y);
  double error = std::pow(10, -5);  // 0.00001

  if (sub_x < error && sub_y < error)
  {
    return false;
  }

  *a = end.y - start.y;
  *b = (-1) * (end.x - start.x);
  *c = (-1) * (end.y - start.y) * start.x + (end.x - start.x) * start.y;

  return true;
}
double getDistanceBetweenLineAndPoint(geometry_msgs::Point point, double a, double b, double c)
{
  double d = std::fabs(a * point.x + b * point.y + c) / std::sqrt(std::pow(a, 2) + std::pow(b, 2));

  return d;
}

tf::Vector3 point2vector(geometry_msgs::Point point)
{
  tf::Vector3 vector(point.x, point.y, point.z);
  return vector;
}

geometry_msgs::Point vector2point(tf::Vector3 vector)
{
  geometry_msgs::Point point;
  point.x = vector.getX();
  point.y = vector.getY();
  point.z = vector.getZ();
  return point;
}

tf::Vector3 rotateUnitVector(tf::Vector3 unit_vector, double degree)
{
  tf::Vector3 w1(cos(deg2rad(degree)) * unit_vector.getX() - sin(deg2rad(degree)) * unit_vector.getY(),
                 sin(deg2rad(degree)) * unit_vector.getX() + cos(deg2rad(degree)) * unit_vector.getY(), 0);
  tf::Vector3 unit_w1 = w1.normalize();

  return unit_w1;
}

geometry_msgs::Point rotatePoint(geometry_msgs::Point point, double degree)
{
  geometry_msgs::Point rotate;
  rotate.x = cos(deg2rad(degree)) * point.x - sin(deg2rad(degree)) * point.y;
  rotate.y = sin(deg2rad(degree)) * point.x + cos(deg2rad(degree)) * point.y;

  return rotate;
}

double calcCurvature(const geometry_msgs::Point &target, const geometry_msgs::Pose &curr_pose)
{
  constexpr double KAPPA_MAX = 1e9;
  const double radius = calcRadius(target, curr_pose);

  if (std::fabs(radius) > 0)
  {
    return 1.0 / radius;
  }
  else
  {
    return KAPPA_MAX;
  }
}

double calcDistSquared2D(const geometry_msgs::Point &p, const geometry_msgs::Point &q)
{
  const double dx = p.x - q.x;
  const double dy = p.y - q.y;
  return (dx * dx + dy * dy);
}

double calcLateralError2D(const geometry_msgs::Point &line_s, const geometry_msgs::Point &line_e,
                          const geometry_msgs::Point &point)
{
  tf2::Vector3 a_vec((line_e.x - line_s.x), (line_e.y - line_s.y), 0.0);
  tf2::Vector3 b_vec((point.x - line_s.x), (point.y - line_s.y), 0.0);

  double lat_err = (a_vec.length() > 0) ? a_vec.cross(b_vec).z() / a_vec.length() : 0.0;
  return lat_err;
}

double calcRadius(const geometry_msgs::Point &target, const geometry_msgs::Pose &current_pose)
{
  constexpr double RADIUS_MAX = 1e9;
  const double denominator = 2.0 * transformToRelativeCoordinate2D(target, current_pose).y;
  const double numerator = calcDistSquared2D(target, current_pose.position);

  if (std::fabs(denominator) > 0)
    return numerator / denominator;
  else
    return RADIUS_MAX;
}

std::vector<geometry_msgs::Pose> extractPoses(const autoware_msgs::Lane &lane)
{
  std::vector<geometry_msgs::Pose> poses;

  for (const auto &el : lane.waypoints)
    poses.push_back(el.pose.pose);

  return poses;
}

std::vector<geometry_msgs::Pose> extractPoses(const std::vector<autoware_msgs::Waypoint> &wps)
{
  std::vector<geometry_msgs::Pose> poses;

  for (const auto &el : wps)
    poses.push_back(el.pose.pose);

  return poses;
}

std::pair<bool, int32_t> findClosestIdxWithDistAngThr(const std::vector<geometry_msgs::Pose> &curr_ps,
                                                      const geometry_msgs::Pose &curr_pose,
                                                      double dist_thr,
                                                      double angle_thr)
{
  double dist_squared_min = std::numeric_limits<double>::max();
  int32_t idx_min = -1;

  for (int32_t i = 0; i < static_cast<int32_t>(curr_ps.size()); ++i)
  {
    const double ds = calcDistSquared2D(curr_ps.at(i).position, curr_pose.position);
    if (ds > dist_thr * dist_thr)
      continue;

    double yaw_pose = tf2::getYaw(curr_pose.orientation);
    double yaw_ps = tf2::getYaw(curr_ps.at(i).orientation);
    double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_ps);
    if (std::fabs(yaw_diff) > angle_thr)
      continue;

    if (ds < dist_squared_min)
    {
      dist_squared_min = ds;
      idx_min = i;
    }
  }

  return (idx_min >= 0) ? std::make_pair(true, idx_min) : std::make_pair(false, idx_min);
}

geometry_msgs::Quaternion getQuaternionFromYaw(const double &_yaw)
{
  tf2::Quaternion q;
  q.setRPY(0, 0, _yaw);
  return tf2::toMsg(q);
}

bool isDirectionForward(const std::vector<geometry_msgs::Pose> &poses)
{
  geometry_msgs::Point rel_p = transformToRelativeCoordinate2D(poses.at(2).position, poses.at(1));
  bool is_forward = (rel_p.x > 0.0) ? true : false;
  return is_forward;
}

double normalizeEulerAngle(double euler)
{
  double res = euler;
  while (res > M_PI)
  {
    res -= (2.0 * M_PI);
  }
  while (res < -M_PI)
  {
    res += 2.0 * M_PI;
  }

  return res;
}

geometry_msgs::Point transformToAbsoluteCoordinate2D(const geometry_msgs::Point &point,
                                                                      const geometry_msgs::Pose &origin)
{
  // rotation
  geometry_msgs::Point rot_p;
  double yaw = tf2::getYaw(origin.orientation);
  rot_p.x = (cos(yaw) * point.x) + ((-1.0) * sin(yaw) * point.y);
  rot_p.y = (sin(yaw) * point.x) + (cos(yaw) * point.y);

  // translation
  geometry_msgs::Point res;
  res.x = rot_p.x + origin.position.x;
  res.y = rot_p.y + origin.position.y;
  res.z = origin.position.z;

  return res;
}

geometry_msgs::Point transformToAbsoluteCoordinate3D(const geometry_msgs::Point &point,
                                                                      const geometry_msgs::Pose &origin)
{
  Eigen::Translation3d trans(origin.position.x, origin.position.y, origin.position.z);
  Eigen::Quaterniond rot(origin.orientation.w, origin.orientation.x, origin.orientation.y, origin.orientation.z);

  Eigen::Vector3d v(point.x, point.y, point.z);
  Eigen::Vector3d transformed_v;
  transformed_v = trans * rot.inverse() * v;

  geometry_msgs::Point transformed_p = tf2::toMsg(transformed_v);
  return transformed_p;
}

geometry_msgs::Point transformToRelativeCoordinate2D(const geometry_msgs::Point &point,
                                                                      const geometry_msgs::Pose &origin)
{
  // translation
  geometry_msgs::Point trans_p;
  trans_p.x = point.x - origin.position.x;
  trans_p.y = point.y - origin.position.y;

  // rotation (use inverse matrix of rotation)
  double yaw = tf2::getYaw(origin.orientation);

  geometry_msgs::Point res;
  res.x = (cos(yaw) * trans_p.x) + (sin(yaw) * trans_p.y);
  res.y = ((-1.0) * sin(yaw) * trans_p.x) + (cos(yaw) * trans_p.y);
  res.z = origin.position.z;

  return res;
}

geometry_msgs::Point transformToRelativeCoordinate3D(const geometry_msgs::Point &point,
                                                                      const geometry_msgs::Pose &origin)
{
  Eigen::Translation3d trans(-origin.position.x, -origin.position.y, -origin.position.z);
  Eigen::Quaterniond rot(origin.orientation.w, origin.orientation.x, origin.orientation.y, origin.orientation.z);

  Eigen::Vector3d v(point.x, point.y, point.z);
  Eigen::Vector3d transformed_v;
  transformed_v = trans * rot * v;

  geometry_msgs::Point transformed_p = tf2::toMsg(transformed_v);
  return transformed_p;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include "right_hand.h"

using namespace  std;
using namespace  Eigen;

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

void  SendGazebo(vector<double> q){

    std_msgs::Float64 data;
    data.data=q[1];
    pub1.publish(data);
    data.data=q[2];
    pub2.publish(data);
    data.data=q[3];
    pub3.publish(data);
    data.data=q[4];
    pub4.publish(data);
    data.data=q[5];
    pub5.publish(data);
    data.data=q[6];
    pub6.publish(data);
    data.data=q[7];
    pub7.publish(data);
    data.data=q[8];
    pub8.publish(data);
    data.data=q[9];
    pub9.publish(data);
    data.data=q[10];
    pub10.publish(data);
    data.data=q[11];
    pub11.publish(data);
    data.data=q[12];
    pub12.publish(data);
    data.data=q[13];
    pub13.publish(data);
    data.data=q[14];
    pub14.publish(data);
    data.data=q[15];
    pub15.publish(data);
    data.data=q[16];
    pub16.publish(data);
    data.data=q[17];
    pub17.publish(data);
    data.data=q[18];
    pub18.publish(data);
    data.data=q[19];
    pub19.publish(data);
    data.data=q[20];
    pub20.publish(data);
    data.data=q[21];
    pub21.publish(data);
    data.data=q[22];
    pub22.publish(data);
    data.data=q[23];
    pub23.publish(data);
    data.data=q[24];
    pub24.publish(data);
    data.data=q[25];
    pub25.publish(data);
    data.data=q[26];
    pub26.publish(data);
    data.data=q[27];
    pub27.publish(data);
    data.data=q[28];
    pub28.publish(data);

}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);

    right_hand hand_funcs;
    VectorXd r_target(3);
    VectorXd r_shaking(3);
    VectorXd r_shaking2(3);

    MatrixXd R_target(3,3);
    //shaking

    r_shaking<<.5,
            .05,
            -.35;
    r_shaking2<<.5,
            .05,
            -.25;

    R_target=hand_funcs.rot(2,80*M_PI/180,3);

VectorXd q0(7);
q0<<0,
        0,
        0,
        -10*M_PI/180,
        0,
        0,
        0;

right_hand hand0(q0,r_target,R_target);
double d0=hand0.dist;
double d=d0;
double d_des=hand0.d_des;
double theta=hand0.theta;
double theta_target=hand0.theta_target;
double sai=hand0.sai;
double sai_target=hand0.sai_target;
double phi=hand0.phi;
double phi_target=hand0.phi_target;

QVector<double> qr1;
QVector<double> qr2;
QVector<double> qr3;
QVector<double> qr4;
QVector<double> qr5;
QVector<double> qr6;
QVector<double> qr7;

int i=0;
VectorXd q_ra;
q_ra=q0;

qr1.append(q_ra(0));
qr2.append(-q_ra(1));
qr3.append(-q_ra(2));
qr4.append(q_ra(3));
qr5.append(-q_ra(4));
qr6.append(-q_ra(5));
qr7.append(q_ra(6));

vector<double> q_init(30);
for (int i = 0; i < 30; ++i) {
    q_init[i]=0;
}
q_init[15]=qr1[0];
q_init[16]=qr2[0];
q_init[17]=qr3[0];
q_init[18]=qr4[0];
q_init[19]=qr5[0];
q_init[20]=qr6[0];
q_init[21]=qr7[0];

SendGazebo(q_init);

vector<double> q(30);

ros::Rate loop_rate(int(1.0/hand0.T));
int count = 0;
int kk=-1;
    while (ros::ok())
    {
        if (kk==6){break;}
        for (int k = 0; k < 7; ++k) {
            kk=k;

            if (k%2==1){r_target=r_shaking;}
            else{r_target=r_shaking2;}

right_hand hand00(q_ra,r_target,R_target,i,d0,v0,v_target);
int j=i;
d=hand00.dist;
        while (d>d_des  || (abs(theta-theta_target)>.02)   || (abs(sai-sai_target)>.02)  || (abs(phi-phi_target)>.02)  ) {

        right_hand hand(q_ra,r_target,R_target,d0,v0,v_target);
        hand.doQP(q_ra);
        q_ra=hand.q_next;

        qr1.append(q_ra(0));
        qr2.append(-q_ra(1));
        qr3.append(-q_ra(2));
        qr4.append(q_ra(3));
        qr5.append(-q_ra(4));
        qr6.append(-q_ra(5));
        qr7.append(q_ra(6));

        d=hand.dist;
        theta=hand.theta;
        sai=hand.sai;
        phi=hand.phi;

        i++;

        for (int l = 0; l < 30; ++l) {
            q[l]=0;
        }
        q[15]=qr1[count];
        q[16]=qr2[count];
        q[17]=qr3[count];
        q[18]=qr4[count];
        q[19]=qr5[count];
        q[20]=qr6[count];
        q[21]=qr7[count];

       ROS_INFO("q1=%f\tq2=%f\tq3=%f\tq4=%f\tq5=%f\tq6=%f\tq7=%f\t",q[15],q[16],q[17],q[18],q[19],q[20],q[21]);
        SendGazebo(q);

        //chatter_pub.publish(msg);

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
}

        }

           }
    return 0;
}


#include <memo/silo/Strip.hh>

#include <elle/algorithm.hh>

#include <memo/model/Address.hh>

#include <boost/algorithm/string.hpp>

#include <elle/factory.hh>
#include <elle/reactor/Scope.hh>

namespace memo
{
  namespace silo
  {
    Strip::Strip(std::vector<std::unique_ptr<Silo>> backend)
      : _backend(std::move(backend))
    {
      // This assumes that the metrics are already correct in the
      // "backends".
      for (auto const& b: _backend)
      {
        _usage += b->usage();
        _block_count += b->block_count();
      }
    }

    elle::Buffer
    Strip::_get(Key k) const
    {
      return _storage_of(k).get(k);
    }

    int
    Strip::_set(Key k, elle::Buffer const& value, bool insert, bool update)
    {
      return _storage_of(k).set(k, value, insert, update);
    }

    int
    Strip::_erase(Key k)
    {
      return _storage_of(k).erase(k);
    }

    namespace
    {
      /// The sum of the digits of this address.
      int
      sum(Key k)
      {
        int res = 0;
        for (auto c: k.value())
          res += c;
        return res;
      }
    }

    Silo&
    Strip::_storage_of(Key k) const
    {
      return *_backend[sum(k) % _backend.size()];
    }

    std::vector<Key>
    Strip::_list()
    {
      auto res = std::vector<Key>{};
      for (auto const& b: _backend)
        elle::push_back(res, b->list());
      return res;
    }

    static
    std::unique_ptr<Silo>
    make(std::vector<std::string> const& args)
    {
      auto backends = std::vector<std::unique_ptr<Silo>>{};
      for (unsigned int i = 0; i < args.size(); i += 2)
        backends.emplace_back(instantiate(args[i], args[i+1]));
      return std::make_unique<Strip>(std::move(backends));
    }

    StripSiloConfig::StripSiloConfig(
      Silos storages,
      boost::optional<int64_t> capacity,
      boost::optional<std::string> description)
      : SiloConfig(
          "multi-storage", std::move(capacity), std::move(description))
      , storage(std::move(storages))
    {}

    StripSiloConfig::StripSiloConfig(elle::serialization::SerializerIn& s)
      : SiloConfig(s)
      , storage(s.deserialize<Silos>("backend"))
    {}

    void
    StripSiloConfig::serialize(elle::serialization::Serializer& s)
    {
      SiloConfig::serialize(s);
      s.serialize("backend", this->storage);
    }

    std::unique_ptr<memo::silo::Silo>
    StripSiloConfig::StripSiloConfig::make()
    {
      std::vector<std::unique_ptr<memo::silo::Silo>> s;
      for(auto const& c: storage)
        s.push_back(c->make());
      return std::make_unique<memo::silo::Strip>(
        std::move(s));
    }

    static const elle::serialization::Hierarchy<SiloConfig>::
    Register<StripSiloConfig>
    _register_StripSiloConfig("strip");
  }
}

FACTORY_REGISTER(memo::silo::Silo, "strip", &memo::silo::make);