/*
 For more information, please see: http://software.sci.utah.edu

 The MIT License

 Copyright (c) 2015 Scientific Computing and Imaging Institute,
 University of Utah.

 License for the specific language governing rights and limitations under
 Permission is hereby granted, free of charge, to any person obtaining a
 copy of this software and associated documentation files (the "Software"),
 to deal in the Software without restriction, including without limitation
 the rights to use, copy, modify, merge, publish, distribute, sublicense,
 and/or sell copies of the Software, and to permit persons to whom the
 Software is furnished to do so, subject to the following conditions:

 The above copyright notice and this permission notice shall be included
 in all copies or substantial portions of the Software.
../../src//Core/Algorithms/Math/ConvertMatrixType.cc:
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
 THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 DEALINGS IN THE SOFTWARE.
 */

#include <Core/Algorithms/Base/AlgorithmPreconditions.h>
#include <Core/Algorithms/BrainStimulator/BiotSavartSolverAlgorithm.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/Legacy/Field/Field.h>
#include <Core/Datatypes/Legacy/Field/VField.h>
#include <Core/Datatypes/Legacy/Field/VMesh.h>
#include <Core/Math/MiscMath.h>
#include <Core/Datatypes/Legacy/Field/FieldInformation.h>
#include <Core/Thread/Barrier.h>
#include <Core/Thread/Parallel.h>
#include <string>
#include <cassert>
#include <memory>
#include <Core/Logging/Log.h>
#include <boost/lexical_cast.hpp>

using namespace SCIRun::Core::Algorithms;
using namespace SCIRun::Core::Algorithms::BrainStimulator;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Thread;
using namespace SCIRun::Core::Geometry;
using namespace SCIRun::Core::Logging;
using namespace SCIRun;

ALGORITHM_PARAMETER_DEF(BrainStimulator, Mesh);
ALGORITHM_PARAMETER_DEF(BrainStimulator, Coil);
ALGORITHM_PARAMETER_DEF(BrainStimulator, VectorBField);
ALGORITHM_PARAMETER_DEF(BrainStimulator, VectorAField);
ALGORITHM_PARAMETER_DEF(BrainStimulator, OutType);

	class KernelBase
		{

		public:
			KernelBase(const AlgorithmBase* algo, int t) :
			  ref_cnt(0),
			  algo_(algo),
			  numprocessors_(Parallel::NumCores()),
			  barrier_("BSV KernelBase Barrier", numprocessors_),
			  typeOut(t),
			  matOut(0)
			{
			}

			virtual ~KernelBase()
			{
			}

			//! Local entry function, must be implemented by each specific kernel
			virtual bool Integrate(FieldHandle& mesh, FieldHandle& coil, MatrixHandle& outdata) = 0;

			//! Global reference counting
			int ref_cnt;

		protected:

			//! ref to the executing algorithm context
			const AlgorithmBase* algo_;
			unsigned int numprocessors_;

			//! model miscs.
			VMesh* vmesh;
			VField* vfield;
			size_type modelSize;

			//! coil miscs.
			VMesh* vcoil;
			VField* vcoilField;
			size_type coilSize;

			//! parallel essential primitives
			Barrier barrier_;
			std::vector<bool> success;

			//! output Field
			int typeOut;
			DenseMatrix *matOut;
			MatrixHandle matOutHandle;

			bool PreIntegration( FieldHandle& mesh, FieldHandle& coil )
			{
					this->vmesh = mesh->vmesh();
					assert(vmesh);

					this->vcoil = coil->vmesh();
					assert(vcoil);

					this->vfield = mesh->vfield();
					assert(vfield);

					this->vcoilField = coil->vfield();
					assert(vcoilField);

					this->numprocessors_ = Parallel::NumCores();

                    int numproc = Parallel::NumCores();

					if (numproc > 0)
					{
						numprocessors_ = numproc;
					}

					#ifdef _DEBUG
						//! DEBUG when we want to test with one CPU only
						numprocessors_ = 1;
					#endif

					algo_->remark("number of processors:  " + boost::lexical_cast<std::string>(this->numprocessors_));

					success.resize(numprocessors_,true);

					//! get number of nodes for the model
					modelSize = vmesh->num_nodes();
					assert(modelSize > 0);

					try
					{
						matOut = new DenseMatrix(static_cast<int>(modelSize),3);
						matOutHandle = static_cast<MatrixHandle>(matOut);
					}
					catch (...)
					{
						algo_->error("Error alocating output matrix");
						return (false);
					}

					return (true);
			}

			bool PostIntegration( MatrixHandle& outdata )
			{
				//! check for error
				for (size_t j=0; j<success.size(); j++)
				{
					if (success[j] == false) return (false);
				}

				outdata = matOutHandle;

				return (true);
			}
		};

	class PieceWiseKernel : public KernelBase
		{
			public:

				PieceWiseKernel(const AlgorithmBase* algo, int t ) : KernelBase(algo,t)
				{
					//we keep last calculated step
					//however if segments lenght varies,
					//it makes more sense to keep a look-up table of previous steps for given lenght
					autostep = 0.1;
					extstep = -1.0;

				}

				~PieceWiseKernel()
				{
				}

				//! Complexity O(M*N) ,where M is the number of nodes of the model and N is the numbder of nodes of the coil
				virtual bool Integrate(FieldHandle& mesh, FieldHandle& coil, MatrixHandle& outdata)
				{

					if(!PreIntegration(mesh,coil))
					{
						return (false);
					}

					vmesh->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

					VMesh::Node::array_type enodes;
					Point enode1;
					Point enode2;

					//! get numbder of nodes for the coil
					coilSize = vcoil->num_nodes();

					//! basic assumption
					assert(modelSize > 0 && coilSize > 1);

					coilNodes.clear();
					coilNodes.reserve(coilSize);

					for(VMesh::Edge::index_type i = 0; i < vcoil->num_edges(); i++)
					{
						vcoil->get_nodes(enodes,i);
						vcoil->get_point(enode1,enodes[0]);
						vcoil->get_point(enode2,enodes[1]);
						coilNodes.push_back(Vector(enode1));
						coilNodes.push_back(Vector(enode2));
					}

					//! Start the multi threaded
					Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

					return PostIntegration(outdata);
				}

				void SetIntegrationStep(double step)
				{
					assert(step >= 0.0);
					extstep = step;
				}

				double GetIntegrationStep() const
				{
					return extstep;
				}

			private:

				//! integration step, will auto adapt
				double autostep;

				//! integration step, externally provided
				double extstep;

				//! keep nodes on the coil cached
				std::vector<Vector> coilNodes;

				//! execute in parallel
				void ParallelKernel(int proc_num)
				{

					assert(proc_num >= 0);

					int cnt = 0;
					double current = 1.0;
					Point modelNode;

					const index_type begins = (modelSize * proc_num) / numprocessors_;
					const index_type ends  = (modelSize * (proc_num+1)) / numprocessors_;

					assert( begins <= ends );

					//! buffer of points used for integration
					std::vector<Vector> integrPoints;
					integrPoints.reserve(256);

					//! keep previous step length
					//! used for optimization purpose
					double prevSegLen = 123456789.12345678;

					//! number of integration points
					int nips = 0;
					index_type helpme=0;

					try{

						for(index_type iM = begins;
							iM < ends;
							iM++)
						{
						        helpme++;
							vmesh->get_node(modelNode,iM);

							// result
							Vector F;

							for( size_t iC0 = 0, iC1 =1, iCV = 0;
								iC0 < coilNodes.size();
								iC0+=2, iC1+=2, iCV++)
							{
								vcoilField->get_value(current,iCV);

								current = current == 0.0 ? 1.0 : current;

								Vector coilNodeThis;
								Vector coilNodeNext;

								if(current >= 0.0)
								{
									coilNodeThis = coilNodes[iC0];
									coilNodeNext = coilNodes[iC1];
								}
								else
								{
									coilNodeThis = coilNodes[iC1];
									coilNodeNext = coilNodes[iC0];
								}

								//! Length of the curve element
								Vector diffNodes = coilNodeNext - coilNodeThis;
								double newSegLen = diffNodes.length();

								//first check if externally suplied integration step is available and use it
								if(extstep > 0)
								{
									nips = newSegLen / extstep;
								}
								else
								{
									//! optimization
									//! only rexompute integration step only if segment length changes
									if( Abs(prevSegLen - newSegLen ) > 0.00000001 )
									{
										prevSegLen = newSegLen;

										//auto adaptive integration step calculation
										nips =  AdjustNumberOfIntegrationPoints(newSegLen);
									}
								}

								if( nips < 3 )
								{
									algo_->warning("integration step too big");
								}

								integrPoints.clear();

								//! curve segment discretization
								for(int iip = 0; iip < nips; iip++)
								{
									double interpolant = static_cast<double>(iip) / static_cast<double>(nips);
									Vector v = Interpolate( coilNodeThis, coilNodeNext, interpolant );
									integrPoints.push_back( v );
								}

								//! integration step over line segment
								for(int iip = 0; iip < nips -1; iip++)
								{
									//! Vector connecting the infinitesimal curve-element
									Vector Rxyz = (integrPoints[iip] + integrPoints[iip+1] ) / 2  - Vector(modelNode);

									//! Infinitesimal curve-element components
									Vector dLxyz = integrPoints[iip+1] - integrPoints[iip];

									double Rn = Rxyz.length();

									//! check for distance between coil and model close to zero
									//! it might cause numerical stability issues with respect to the cross-product
									if(Rn < 0.00001)
									{
										algo_->warning("coil<->model distance approaching zero!");
									}

									if(typeOut == 1)
									{
										//! Biot-Savart Magnetic Field
										F +=  1.0e-7 * Cross( Rxyz, dLxyz ) * ( Abs(current) / (Rn*Rn*Rn) );

									}

									if(typeOut == 2)
									{
										//! Biot-Savart Magnetic Vector Potential Field
										F += 1.0e-7 * dLxyz * ( Abs(current) / (Rn) );
									}

								}

							}

							matOut->put(iM,0, F[0]);
							matOut->put(iM,1, F[1]);
							matOut->put(iM,2, F[2]);

							//! progress reporter
							if (proc_num == 0)
							{
								cnt++;
								if (cnt == 200)
								{
									cnt = 0;
									algo_->update_progress(iM/(ends-begins));
						                  /// The progress bar update does not work ... and it also counts to iM/2 in other classes strange!
								}
							}
						}

						success[proc_num] = true;
					}
					catch (...)
					{
						algo_->error(std::string("PieceWiseKernel crashed while integrating"));
						success[proc_num] = false;
					}

					//! check point
					barrier_.wait();

					// Bail out if one of the processes failed
					for (size_t q=0; q<numprocessors_;q++)
						if (success[q] == false) return;

				}

				//! Auto adjust accuracy of integration
				int AdjustNumberOfIntegrationPoints(double len)
				{
					//assert(step < len);

					int minNP = 100;//more than 1 for sure
					int maxNP = 200;//no more than 1000
					int NP = 0;
					bool over = false;
					bool under = false;

					do
					{
						NP = ceil( len / autostep );

						under = NP < minNP ? true : false;
						over = NP > maxNP ? true : false;

						if(under) autostep *= 0.5;
						if(over) autostep *= 1.5;

					}while( under || over );

					return NP;
				}

		};

//! TODO
		class VolumetricKernel : public KernelBase
		{
			public:

				VolumetricKernel(const AlgorithmBase* algo, int t) : KernelBase(algo,t)
				{
				}

				~VolumetricKernel()
				{
				}

				virtual bool Integrate(FieldHandle& mesh, FieldHandle& coil, MatrixHandle& outdata)
				{
					if(!PreIntegration(mesh,coil))
					{
						return (false);
					}

					//! get numbder of nodes for the coil
					coilSize = vcoil->num_elems();

					//! basic assumption
					assert(modelSize > 0 && coilSize > 1);

					vmesh->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

					//! Start the multi threaded
					Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

					return PostIntegration(outdata);
				}

			private:

				//! execute in parallel
				void ParallelKernel(int proc_num)
				{
					assert(proc_num >= 0);

					int cnt = 0;
					Point modelNode;
					Point coilCenter;
					Vector current;

					const VMesh::Node::index_type begins = (modelSize * proc_num) / numprocessors_;
					const VMesh::Node::index_type ends  = (modelSize * (proc_num+1)) / numprocessors_;

					assert( begins <= ends );

					try{

						for(VMesh::Node::index_type iM = begins; iM < ends;	iM++)
						{
							vmesh->get_node(modelNode,iM);

							//! accumulatedresult
							Vector F;

							Vector R;

							double evol = 0.0;

							double Rl;

							for(VMesh::Elem::index_type  iC = 0; iC < coilSize; iC++)
							{
								vcoilField->get_value(current,iC);

								vcoilField->get_center(coilCenter, iC);//auto resolve based on basis_order

								evol = vcoil->get_volume(iC);

								R = coilCenter - modelNode;

								Rl = R.length();

								if(typeOut == 1)
								{
									//! Biot-Savart Magnetic Field
									F += Cross ( current , R ) * ( evol / (4.0 * M_PI * Rl) );
								}

								if(typeOut == 2)
								{
									//! Biot-Savart Magnetic Vector Potential Field
									F += current * ( evol / (4.0 * M_PI * Rl) );
								}

							}

							matOut->put(iM,0, F[0]);
							matOut->put(iM,1, F[1]);
							matOut->put(iM,2, F[2]);

							//! progress reporter
							if (proc_num == 0)
							{
								cnt++;
								if (cnt == 200)
								{
									cnt = 0;
									algo_->update_progress(iM/2*(begins-ends));
								}
							}
						}

						success[proc_num] = true;
					}
					catch (...)
					{
						algo_->error(std::string("VolumetricKernel crashed while integrating"));
						success[proc_num] = false;
					}

					//! check point
					barrier_.wait();

					// Bail out if one of the processes failed
					for (size_t q=0; q<numprocessors_;q++)
						if (success[q] == false) return;

				}
		};

		//! Magnetic Dipoles solver
		class DipolesKernel : public KernelBase
		{
			public:

				DipolesKernel(const AlgorithmBase* algo, int t) : KernelBase(algo,t)
				{
				}

				~DipolesKernel()
				{
				}

				virtual bool Integrate(FieldHandle& mesh, FieldHandle& coil, MatrixHandle& outdata)
				{
					if(!PreIntegration(mesh,coil))
					{
						return (false);
					}

					//! get numbder of nodes for the coil
					coilSize = vcoil->num_elems();

					//! basic assumption
					assert(modelSize > 0 && coilSize > 1);

					//needed?
					vmesh->synchronize(Mesh::NODES_E | Mesh::EDGES_E);

					//! Start the multi threaded
					Parallel::RunTasks([this](int i) { ParallelKernel(i); }, numprocessors_);

					return PostIntegration(outdata);
				}

			private:

				//! execute in parallel
				void ParallelKernel(int proc_num)
				{
					assert(proc_num >= 0);

					int cnt = 0;
					Point modelNode;
					Point dipoleLocation;
					Vector dipoleMoment;

					const VMesh::Node::index_type begins = (modelSize * proc_num) / numprocessors_;
					const VMesh::Node::index_type ends  = (modelSize * (proc_num+1)) / numprocessors_;

					assert( begins <= ends );

					try{

						for(VMesh::Node::index_type iM = begins; iM < ends;	iM++)
						{
							vmesh->get_node(modelNode,iM);

							//! accumulated result
							Vector F;

							Vector R;

							double Rl;

							for(VMesh::Elem::index_type  iC = 0; iC < coilSize; iC++)
							{
								vcoilField->get_value(dipoleMoment,iC);

								vcoilField->get_center(dipoleLocation, iC);//auto resolve based on basis_order

								R = dipoleLocation - modelNode;

								Rl = R.length();

								if(typeOut == 1)
								{
									//! Biot-Savart Magnetic Field
									F += 1.0e-7 * ( 3 * R * Dot ( dipoleMoment, R ) / (Rl*Rl*Rl*Rl*Rl) - dipoleMoment / (Rl*Rl*Rl) ) ;
								}

								if(typeOut == 2)
								{
									//! Biot-Savart Magnetic Vector Potential Field
									F += 1.0e-7 * Cross ( dipoleMoment , R ) / (Rl*Rl*Rl) ;
								}

							}

							matOut->put(iM,0, F[0]);
							matOut->put(iM,1, F[1]);
							matOut->put(iM,2, F[2]);

							//! progress reporter
							if (proc_num == 0)
							{
								cnt++;
								if (cnt == 200)
								{
									cnt = 0;
									algo_->update_progress(iM/2*(begins-ends));
								}
							}
						}

						success[proc_num] = true;
					}
					catch (...)
					{
						algo_->error(std::string("DipoleKernel crashed while integrating"));
						success[proc_num] = false;
					}

					//! check point
					barrier_.wait();

					// Bail out if one of the processes failed
					for (size_t q=0; q<numprocessors_;q++)
						if (success[q] == false) return;

				}
		};

bool BiotSavartSolverAlgorithm::run(FieldHandle mesh, FieldHandle coil, MatrixHandle &outdata, int outtype) const
{
  if (!mesh)
  {
    error("No input domain field");
    return (false);
  }

  if (!coil)
  {
    error("No input coil source field");
    return (false);
  }

  if (coil->vfield()->basis_order()  == -1)
  {
   error("Need data on coil mesh.");
   return (false);
  }

  if( coil->vmesh()->is_curvemesh() )
  {
    if(coil->vfield()->is_constantdata() && coil->vfield()->is_scalar())
    {
      auto pwk = std::unique_ptr<KernelBase>(new PieceWiseKernel(this, outtype));
      //pwk->SetIntegrationStep(this->istep);
      if( !pwk->Integrate(mesh,coil,outdata) )
      {
       error("Aborted during integration");
       return (false);
      }
    }
    else
    {
      error("Curve mesh expected with constant scalar data.");
      return (false);
    }
  }
  else if(coil->vmesh()->is_pointcloudmesh())
  {
   if((coil->vfield()->is_lineardata() || coil->vfield()->is_constantdata() ) && coil->vfield()->is_vector())
   {
    auto dp = std::unique_ptr<KernelBase>(new DipolesKernel(this, outtype));
    if( !dp->Integrate(mesh,coil,outdata) )
      {
       error("Aborted during integration");
       return (false);
      }
   }
   else
   {
    error("Pointcloud expected with linear vector data.");
    return (false);
   }
  }
  else if( coil->vmesh()->is_volume() )
  {
   if(  coil->vfield()->is_constantdata() && coil->vfield()->is_vector() )
   {
   auto vp = std::unique_ptr<KernelBase>(new VolumetricKernel(this, outtype));
   if( !vp->Integrate(mesh,coil,outdata) )
      {
       error("Aborted during integration");
       return (false);
      }
   }
   else
   {
    error("Volumetric mesh expected with constant vector data.");
    return (false);
   }
  }
  else
  {
   error("Unsupported mesh type! Only curve or volumetric.");
   return (false);
  }

  return (true);
}

AlgorithmOutput BiotSavartSolverAlgorithm::run(const AlgorithmInput& input) const
{
 AlgorithmOutput output;

 auto mesh = input.get<Field>(Parameters::Mesh);
 auto coil = input.get<Field>(Parameters::Coil);

 MatrixHandle outdata1,outdata2;

 auto oports = get(Parameters::OutType).toInt();

 if (oports==3) //both are output ports
 {
  if(!run(mesh, coil, outdata1, 1))
  {
    error("Error: Algorithm of BiotSavartSolver failed.");
  }
  output[Parameters::VectorBField] = outdata1;

  if(!run(mesh, coil, outdata2, 2))
  {
    error("Error: Algorithm of BiotSavartSolver failed.");
  }
  output[Parameters::VectorAField] = outdata2;

  return output;
 } else
 {
  if(oports==1)
  {
   if(!run(mesh, coil, outdata1, oports))
   {
    error("Error: Algorithm of BiotSavartSolver failed.");
   }
   output[Parameters::VectorBField] = outdata1;
  } else
  if (oports==2)
  {
   if(!run(mesh, coil, outdata2, oports))
   {
    error("Error: Algorithm of BiotSavartSolver failed.");
   }
   output[Parameters::VectorAField] = outdata2;
  }
 }

 return output;
}


#pragma once
#include <Eigen/Dense>

#include <tbb/tbb.h>

#include "Utilities/Utility.hpp"
#include "States/utils.hpp"
#include "Observables/Observable.hpp"

namespace yannq
{
template<class Machine, class SamplingResult, class Matrix>
void constructDelta(const Machine& qs, SamplingResult&& sr, Matrix& deltas)
{
	using MatrixRowMajor = Eigen::Matrix<typename Machine::Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
	using Range = tbb::blocked_range<std::size_t>;

	//MatrixRowMajor deltas(sr.size(), qs.getDim());
	deltas.resize(sr.size(), qs.getDim());
	if(sr.size() >= 64)
	{
		tbb::affinity_partitioner ap;
		tbb::parallel_for(Range(std::size_t(0u), sr.size()),
			[&](const Range& r)
		{
		/*
			Matrix tmp(r.end()-r.begin(), qs.getDim());
			uint32_t start = r.begin();
			uint32_t end = r.end();
			for(uint32_t l = 0; l < end-start; ++l)
			{
				tmp.row(l) = qs.logDeriv(sr[l+start]);
				//qs.makeData(toSigma(N, basis[l+start]))
			}
			deltas.block(start, 0, end-start, qs.getDim()) = tmp;
		*/
			for(uint32_t l = r.begin(); l < r.end(); ++l)
			{
				deltas.row(l) = qs.logDeriv(sr[l]);
			}
		}, ap);
	}
	else
	{
		for(uint32_t k = 0; k < sr.size(); k++)
		{
			deltas.row(k) = qs.logDeriv(sr[k]);
		}
	}
	//return deltas;
}

namespace detail
{

template<typename Derived>
void initObs(uint32_t nsmp, Observable<Derived>& obs)
{
	obs.initIter(nsmp);
}
template<typename Derived, typename ...Derived2>
void initObs(uint32_t nsmp, Observable<Derived>& obs, Observable<Derived2>&... args)
{
	initObs(nsmp, obs);
	initObs(nsmp, args...);
}

template<typename State, typename Derived>
void eachSampleObs(std::size_t idx, State&& state, Observable<Derived>& obs)
{
	obs.eachSample(idx, std::forward<State>(state));
}

template<typename State, typename Derived, typename ...Derived2>
void ecahSampleObs(size_t idx, State&& state,
		Observable<Derived>& obs, Observable<Derived2>&... args)
{
	eachSampleObs(idx, std::forward<State>(state), obs);
	eachSampleObs(idx, std::forward<State>(state), args...);
}
template<typename Derived>
void finObs(Observable<Derived>& obs)
{
	obs.finIter();
}
template<typename Derived, typename ...Derived2>
void findObs(Observable<Derived>& obs, Observable<Derived2>&... args)
{
	finObs(obs);
	finObs(args...);
}

template<typename RealVector, typename Derived>
void finObsWeights( [[maybe_unused]]  const Eigen::Ref<RealVector>& weights, Observable<Derived>& obs)
{
	obs.finIter();
}
template<typename RealVector, typename Derived, typename ...Derived2>
void findObsWeights(const Eigen::Ref<RealVector>& weights,
		Observable<Derived>& obs, Observable<Derived2>&... args)
{
	finObsWeights(weights, obs);
	finObsWeights(weights, args...);
}

} //namspeace yannq::detail

template<class Machine, class SamplingResult, typename ...Derived>
void constructObs(const Machine& qs, SamplingResult&& sr, Observable<Derived>&... obs)
{
	using Matrix = typename Machine::Matrix;
	using Range = tbb::blocked_range<std::size_t>;
	uint32_t nsmp = sr.size();
	detail::initObs(nsmp, obs...);

	tbb::parallel_for(std::size_t(0u), sr.size(),
		[&](std::size_t idx)
	{
		const auto& elt = sr[idx];
		auto state = construct_state
				<typename MachineStateTypes<Machine>::StateRef>(qs, elt);
		detail::eachSampleObs(idx, state, obs...);
	});

	detail::finObs(obs...);
}
template<class Machine, class SamplingResult, typename ...Derived>
void constructObsWeights(const Machine& qs, SamplingResult&& sr,
		const Eigen::Ref<const typename Machine::RealVector>& weights,
		Observable<Derived>&... obs)
{
	using Matrix = typename Machine::Matrix;
	using Range = tbb::blocked_range<std::size_t>;
	uint32_t nsmp = sr.size();
	detail::initObs(nsmp, obs...);

	tbb::parallel_for(std::size_t(0u), sr.size(),
		[&](std::size_t idx)
	{
		const auto& elt = sr[idx];
		auto state = construct_state
				<typename MachineStateTypes<Machine>::StateRef>(qs, elt);
		detail::eachSampleObs(idx, state, obs...);
	});

	detail::finObsWeights(weights, obs...);
}
} //namespace yannq


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_StandardSoftElasticElectronDataGenerator.cpp
//! \author Luke Kersting
//! \brief  The standard soft elastic electron data generator class def.
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_StandardSoftElasticElectronDataGenerator.hpp"
#include "Utility_SloanRadauQuadrature.hpp"
#include "Utility_ExceptionTestMacros.hpp"
#include "Utility_ContractException.hpp"
#include "Utility_HistogramDistribution.hpp"

namespace DataGen{
/*
// Constructor
StandardSoftElasticElectronDataGenerator::StandardSoftElasticElectronDataGenerator(
	   const unsigned atomic_number,
	   const Teuchos::RCP<const Data::EvaluatedElectronDataContainer>& native_eedl_data,
	   const double min_electron_energy,
	   const double max_electron_energy,
       const double cutoff_angle,
       const unsigned number_of_discrete_angles )
  : SoftElasticElectronDataGenerator( atomic_number ),
    d_native_eedl_data( native_eedl_data ),
    d_min_electron_energy( min_electron_energy ),
    d_max_electron_energy( max_electron_energy ),
    d_cutoff_angle( cutoff_angle ),
    d_number_of_discrete_angles( number_of_discrete_angles )
{
  // Make sure the atomic number is valid
  testPrecondition( atomic_number <= 100u );
  testPrecondition( atomic_number == native_eedl_data->extractAtomicNumber() );
  // Make sure the endl data is valid
  testPrecondition( !native_eedl_data.is_null() );
  // Make sure the electron energy limits are valid
  testPrecondition( min_electron_energy > 0.0 );
  testPrecondition( min_electron_energy < max_electron_energy );
}

// Populate the soft elastic electron data container
void StandardSoftElasticElectronDataGenerator::populateSoftElasticDataContainer(
			   Data::SoftElasticElectronVolatileDataContainer&
			   data_container ) const
{
  // Set the atomic number
  this->setAtomicNumber( data_container );

  // Set the soft elastic electron data
  std::cout << "Setting the soft elastic electron data: " << std::endl;
  this->setSoftElasticElectronData( data_container );
}

// Set the soft elastic electron data
void StandardSoftElasticElectronDataGenerator::setSoftElasticElectronData(
			   Data::SoftElasticElectronVolatileDataContainer&
			   data_container ) const
{
  // Set cutoff angle cosine
  data_container.setCutoffAngleCosine( d_cutoff_angle );

  // Extract the energy grid and cross section
  Teuchos::ArrayRCP<double> energy_grid;
  energy_grid.deepCopy( d_native_eedl_data->extractElectronEnergyGrid() );

  Teuchos::Array<double> elastic_cross_section =
    d_native_eedl_data->extractElasticCrossSection();

  // Extract the elastic scattering information data block (ELASI)
  Teuchos::ArrayView<const double> elasi_block(
				      d_native_eedl_data->extractELASIBlock() );

  // Extract the number of tabulated distributions
  int size = elasi_block.size()/3;

  // Extract the energy grid for elastic scattering angular distributions
  Teuchos::Array<double> elastic_energy_grid(elasi_block(0,size));

  // Extract the table lengths for elastic scattering angular distributions
  Teuchos::Array<double> table_length(elasi_block(size,size));

  // Extract the offsets for elastic scattering angular distributions
  Teuchos::Array<double> offset(elasi_block(2*size,size));

  // Extract the elastic scattering angular distributions block (elas)
  Teuchos::ArrayView<const double> elas_block =
    d_native_eedl_data->extractELASBlock();

  // Create the elastic scattering distributions
  Teuchos::Array<Utility::Pair<double,Teuchos::RCP<const Utility::TabularOneDDistribution> > >
    elastic_scattering_function( size );

  for( unsigned n = 0; n < size; ++n )
  {
    elastic_scattering_function[n].first = elastic_energy_grid[n];

    elastic_scattering_function[n].second.reset(
	  new Utility::HistogramDistribution(
		 elas_block( offset[n], table_length[n] ),
		 elas_block( offset[n] + 1 + table_length[n], table_length[n]-1 ),
         true ) );
  }

  // Set the elastic angular energy grid
  data_container.setElasticAngularEnergyGrid( elastic_energy_grid.toVector() );

  Teuchos::RCP<const MonteCarlo::HardElasticElectronScatteringDistribution>
    elastic_distribution;
  elastic_distribution.reset(
	      new MonteCarlo::HardElasticElectronScatteringDistribution(
                                        d_native_eedl_data->extractAtomicNumber(),
                                        elastic_scattering_function ) );

  // Get the moment of the elastic scattering distribution
  Teuchos::RCP<DataGen::ElasticElectronMomentsEvaluator> moments_evaluator;
  moments_evaluator.reset(
    new DataGen::ElasticElectronMomentsEvaluator( *d_native_eedl_data,
                                                  elastic_distribution,
                                                  d_cutoff_angle ) );

  Teuchos::Array<double> discrete_angles, weights;
  Teuchos::Array<Utility::long_float> legendre_moments;
  Teuchos::RCP<Utility::SloanRadauQuadrature> radau_quadrature;
  double precision = 1e-13;
  int n = ( d_number_of_discrete_angles+1 )*2 + 2+10;

  // iterate through all angular energy bins
  for ( unsigned i = 0; i < size; i++ )
  {
    // Get the discrete angles and weights
    moments_evaluator->evaluateElasticMoment( legendre_moments,
                                              i,
                                              n,
                                              precision );

    std::cout << std::setprecision(20) << "legendre_moments[0] = \t" << legendre_moments[0] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[1] = \t" << legendre_moments[1] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[2] = \t" << legendre_moments[2] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[3] = \t" << legendre_moments[3] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[4] = \t" << legendre_moments[4] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[5] = \t" << legendre_moments[5] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[6] = \t" << legendre_moments[6] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[7] = \t" << legendre_moments[7] << std::endl;
    std::cout << std::setprecision(20) << "legendre_moments[8] = \t" << legendre_moments[8] << std::endl;

    // Use radau quadrature to find the discrete angles and weights from the moments
    radau_quadrature.reset(
      new Utility::SloanRadauQuadrature( legendre_moments ) );

    radau_quadrature->getRadauNodesAndWeights( discrete_angles,
                                               weights,
                                               d_number_of_discrete_angles+1 );

    data_container.setNumberOfDiscreteAngles( i, discrete_angles.size() );
    data_container.setSoftElasticDiscreteAngles( i,
                                                 discrete_angles.toVector() );
    data_container.setSoftElasticWeights( i, weights.toVector() );
  }
}
*/
} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_StandardSoftElasticElectronDataGenerator.cpp
//---------------------------------------------------------------------------//


#include <mps/sdf/SDF.h>
#include <mps/planner/util/Math.h>
#include <mps/planner/util/Logging.h>
#include <fstream>
#include <yaml-cpp/yaml.h>
#include <sim_env/utils/YamlUtils.h>
#include <boost/filesystem/path.hpp>
#include <stack>

using namespace mps::sdf;
using namespace mps::planner::util::math;
using namespace sim_env;

SDF::SDF() : _grid(Eigen::Vector3f(0.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 0.0f), 1.0f) {
}

SDF::~SDF() = default;
SDF::SDF(const SDF& other) = default;
SDF& SDF::operator=(const SDF& other) = default;

void SDF::load(const std::string& filename) {
    std::fstream fs;
    fs.open(filename, std::fstream::in | std::fstream::binary);
    fs >> _grid;
    fs >> _approximation_box.min_corner[0] >> _approximation_box.min_corner[1] >> _approximation_box.min_corner[2];
    fs >> _approximation_box.max_corner[0] >> _approximation_box.max_corner[1] >> _approximation_box.max_corner[2];
}

void SDF::save(const std::string& filename) const {
    std::fstream fs;
    fs.open(filename, std::fstream::out | std::fstream::binary);
    fs << _grid;
    fs << _approximation_box.min_corner[0] << _approximation_box.min_corner[1] << _approximation_box.min_corner[2];
    fs << _approximation_box.max_corner[0] << _approximation_box.max_corner[1] << _approximation_box.max_corner[2];
    fs.close();
}

void SDF::computeSDF(sim_env::WorldPtr world, const sim_env::BoundingBox& aabb,
                     float cell_size, bool ignore_robots, const std::vector<std::string>& ignore_list)
{
    _grid = grid::VoxelGrid<float, float>(aabb.min_corner, aabb.max_corner, cell_size, 0.0f);
    grid::VoxelGrid<float, int> collision_map(aabb.min_corner, aabb.max_corner, cell_size, 1);
    computeSCM(collision_map, world, ignore_robots, ignore_list);
    mps::sdf::fmm::computeSDF<float>(collision_map, _grid, cell_size);
    _approximation_box = aabb;
}

void SDF::setTransform(const Eigen::Affine3f& tf) {
    _grid.setTransform(tf);
}

Eigen::Affine3f SDF::getTransform() const {
    return _grid.getTransform();
}

float SDF::getHeuristicDistance(const Eigen::Vector3f& pos) const {
    grid::UnsignedIndex idx;
    Eigen::Vector3f local_pos;
    _grid.mapToGrid(pos, local_pos, idx);
    return getLocalHeuristicDistance(pos);
}

float SDF::getDistance(const Eigen::Vector3f& pos) const {
    Eigen::Vector3f local_pos;
    grid::UnsignedIndex idx;
    bool valid_idx = _grid.mapToGrid(pos, local_pos, idx);
    if (valid_idx) {
        // TODO do trilinear interpolation
        return _grid(idx);
    }
    return getLocalHeuristicDistance(local_pos);
}

void SDF::setApproximationBox(const sim_env::BoundingBox& aabb) {
    _approximation_box = aabb;
}

float SDF::getLocalHeuristicDistance(const Eigen::Vector3f& pos) const {
    Eigen::Vector3f projected_pos(clamp(pos[0], _approximation_box.min_corner[0], _approximation_box.max_corner[0]),
                                  clamp(pos[1], _approximation_box.min_corner[1], _approximation_box.max_corner[1]),
                                  clamp(pos[2], _approximation_box.min_corner[2], _approximation_box.max_corner[2]));
    return (projected_pos - pos).norm();
}

void SDF::computeSCM(grid::VoxelGrid<float, int>& collision_map, sim_env::WorldPtr world,
                     bool ignore_robots, const std::vector<std::string>& ignore_list)
{
    // first create a set so we can quickly look up whether we have to ignore an objects
    std::unordered_set<std::string> ignore_set;
    for (auto ignore_name : ignore_list) {
        ignore_set.insert(ignore_name);
    }
    // next create init our data structures for our algorithm
    grid::UnsignedIndex min_idx(0, 0, 0);
    grid::UnsignedIndex max_idx(_grid.getXSize(), _grid.getYSize(), _grid.getZSize());
    std::stack<std::pair<grid::UnsignedIndex, grid::UnsignedIndex> > index_stack;
    index_stack.emplace(std::make_pair(min_idx, max_idx));
    // loop over index ranges that are in collision until none are left
    while (not index_stack.empty()) {
        auto index_pair = index_stack.top();
        index_stack.pop();
        auto& min_idx = index_pair.first;
        auto& max_idx = index_pair.second;
        auto box_size = max_idx - min_idx;
        if (box_size.ix == 1 and box_size.iy == 1 and box_size.iz == 1) {
            // Base case, we are looking at only one cell, store collision in this cell
            collision_map(min_idx) = -1.0;
        } else {
            // else we need to split this cell up and see which child ranges are in collision
            std::array<std::array<size_t, 2>, 3> half_sizes;
            splitBox(box_size, half_sizes);
            std::array<std::array<size_t, 2>, 3> idx_offsets = {{{{0, half_sizes[0][0]}},
                                                                {{0, half_sizes[1][0]}},
                                                                {{0, half_sizes[2][0]}}}};
            // now we create the actual ranges for each of the 8 children
            for (unsigned int ix = 0; ix < 2; ++ix) {
                for (unsigned int iy = 0; iy < 2; ++iy) {
                    for (unsigned int iz = 0; iz < 2; ++iz) {
                        // if the current child range actually has some volume/cells
                        if (half_sizes[0][ix] * half_sizes[1][iy] * half_sizes[2][iz]) {
                            // create indices that describe the current child
                            grid::UnsignedIndex child_min_idx(idx_offsets[0][ix], idx_offsets[1][iy],
                                                              idx_offsets[2][iz]);
                            grid::UnsignedIndex child_max_idx(idx_offsets[0][ix] + half_sizes[0][ix],
                                                            idx_offsets[1][iy] + half_sizes[1][iy],
                                                            idx_offsets[2][iz] + half_sizes[2][iz]);
                            child_min_idx += min_idx;
                            child_max_idx += min_idx;
                            // create the bounding box that covers this child
                            sim_env::BoundingBox query_aabb;
                            collision_map.getCellPosition(child_min_idx, query_aabb.min_corner, false);
                            collision_map.getCellPosition(child_max_idx, query_aabb.max_corner, false);
                            // query the world for colliding objects
                            std::vector<sim_env::ObjectPtr> colliding_objects;
                            world->getObjects(query_aabb, colliding_objects, ignore_robots);
                            // check whether any of those can not be ignored
                            bool b_collision = false;
                            for (auto obj : colliding_objects) {
                                // if this object is not in our ignore list, we have a collision
                                b_collision |= ignore_set.count(obj->getName()) == 0;
                            }
                            if (b_collision) {
                                index_stack.emplace(std::make_pair(child_min_idx, child_max_idx));
                            }
                        }
                    }
                }
            }
        }
    }
}

void SDF::splitBox(const sim_env::grid::UnsignedIndex& box_size,
                std::array<std::array<size_t, 2>, 3>& half_sizes) const
{
    half_sizes[0][0] = box_size.ix / 2; // we split this box into 8 children by dividing each axis by 2
    half_sizes[0][1] = box_size.ix - half_sizes[0][0];
    half_sizes[1][0] = box_size.iy / 2;
    half_sizes[1][1] = box_size.iy - half_sizes[1][0];
    half_sizes[2][0] = box_size.iz / 2;
    half_sizes[2][1] = box_size.iz - half_sizes[2][0];
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////// SceneSDFConstructionState //////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SceneSDF::SceneSDFConstructionState::SceneSDFConstructionState() = default;

SceneSDF::SceneSDFConstructionState::SceneSDFConstructionState(const SceneSDFConstructionState& other) = default;

SceneSDF::SceneSDFConstructionState::SceneSDFConstructionState(sim_env::WorldPtr world, const sim_env::BoundingBox& aabb) {
    set(world, aabb);
}

SceneSDF::SceneSDFConstructionState& SceneSDF::SceneSDFConstructionState::operator=(const SceneSDFConstructionState& other) = default;

void SceneSDF::SceneSDFConstructionState::set(sim_env::WorldPtr world, const sim_env::BoundingBox& aabb) {
    std::vector<sim_env::ObjectPtr> objects;
    world->getObjects(objects, false);
    for (auto object : objects) {
        if (object->isStatic()) {
            _static_tfs[object->getName()] = object->getTransform();
        }
        if (object->getType() == sim_env::EntityType::Robot) {
            _robot_names.insert(object->getName());
        } else {
            _object_names.insert(object->getName());
        }
    }
    _aabb = aabb;
}

bool SceneSDF::SceneSDFConstructionState::isDifferent(const SceneSDFConstructionState& other,
                                                      const std::vector<std::string>& ignore_list)
{
    std::unordered_set<std::string> ignore_set;
    if (not ignore_list.empty()) {
        ignore_set.insert(ignore_list.begin(), ignore_list.end());
    }
    // first check whether we have the same objects
    for (auto object_name : _object_names) {
        if (!other._object_names.count(object_name)) {
            if (!ignore_set.count(object_name)) {
                return true;
            }
        }
    }
    // next check whether we have the same robots
    for (auto robot_name : _robot_names) {
        if (!other._robot_names.count(robot_name)) {
            if (!ignore_set.count(robot_name)) {
                return true;
            }
        }
    }
    // finally check whether all statics have the same tfs
    for (auto key_value_pair : _static_tfs) {
        auto other_iter = other._static_tfs.find(key_value_pair.first);
        // if this static doesn't exist in the other, we are different
        if (other_iter == other._static_tfs.end()) {
            return true; // this should actually never happen due to the tests above
        }
        // else
        if (not key_value_pair.second.isApprox(other_iter->second)) {
            return true;
        }
    }
    return false;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////// SceneSDF ///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
SceneSDF::SceneSDF(sim_env::WorldPtr world) : _world(world), _has_statics(false) {
}

SceneSDF::~SceneSDF() = default;

bool SceneSDF::load(const std::string& filename) {
    static const std::string log_prefix("mps::sdf::SceneSDF::load");
    // first reset sdf
    reset();
    // setup
    bool success = true;
    boost::filesystem::path boost_path(filename);
    auto dir_path = boost_path.parent_path();
    YAML::Node base_node = YAML::Load(filename);
    // let's see what objects the world has
    std::vector<sim_env::ObjectPtr> objects;
    _world->getObjects(objects, false);
    for (auto object : objects) {
        if (object->isStatic()) {
            _has_statics = true;
            continue;
        }
        if (base_node[object->getName()]) {
            _movables.push_back(object);
            _movable_sdfs.push_back(SDF());
             boost::filesystem::path movable_filename(dir_path);
             movable_filename += base_node[object->getName()].as<std::string>();
            _movable_sdfs[_movable_sdfs.size() - 1].load(movable_filename.string());
        } else {
            mps::planner::util::logging::logErr("Could not load SDF for object " + object->getName(), log_prefix);
            success = false;
        }
    }
    if (_has_statics) {
        if (base_node["___static_sdf___"]) {
            boost::filesystem::path static_filename(dir_path);
            static_filename += base_node["___static_sdf___"].as<std::string>();
            _static_sdf.load(static_filename.string());
        } else {
            mps::planner::util::logging::logErr("Could not load SDF for statics", log_prefix);
            success = false;
        }
    }
    _bounding_box.min_corner = base_node["___min_point___"].as<Eigen::Vector3f>();
    _bounding_box.max_corner = base_node["___max_point___"].as<Eigen::Vector3f>();
    return success;
}

void SceneSDF::save(const std::string& filename) const {
    boost::filesystem::path yaml_file_path(filename);
    auto dir_path = yaml_file_path.parent_path();
    auto sdf_name = yaml_file_path.filename();
    YAML::Emitter out;
    out << YAML::BeginMap;
    if (_has_statics) {
        std::string static_filename(sdf_name.string() + ".static.sdf");
        boost::filesystem::path static_path(dir_path);
        static_path += static_filename;
        out << YAML::Key << "___static_sdf___";
        out << YAML::Value << static_filename;
        _static_sdf.save(static_path.string());
    }
    for (size_t i = 0; i < _movables.size(); ++i) {
        auto movable = _movables[i].lock();
        if (!movable) throw std::logic_error("Could not access movable. Weak pointer expired!");
        std::string movable_name = movable->getName();
        std::string movable_filename(sdf_name.string() + "." + movable_name + ".sdf");
        boost::filesystem::path movable_path(dir_path);
        dir_path += movable_filename;
        out << YAML::Key << movable_name;
        out << YAML::Value << movable_filename;
        _movable_sdfs[i].save(movable_path.string());
    }
    out << YAML::Key << "___min__point___";
    YAML::convert<Eigen::Vector3f> converter;
    out << YAML::Value << converter.encode(_bounding_box.min_corner);
    out << YAML::Key << "___max__point___";
    out << YAML::Value << converter.encode(_bounding_box.max_corner);
    out << YAML::EndMap;
    std::fstream fs(yaml_file_path.string(), std::fstream::out);
    fs << out.c_str();
    fs.close();
}

SceneSDF::SceneSDFConstructionState SceneSDF::computeSDF(const sim_env::BoundingBox& aabb, float static_cell_size,
                                                         float movable_cell_size, float max_approx_error,
                                                         const StrToFloatMap& radii,
                                                         const StrToStrMap& movable_sdfs)
{
    static const std::string log_prefix("[mps::sdf::SceneSDF::computeSDF]");
    auto logger = _world->getLogger();
    logger->logDebug("Resetting", log_prefix);
    reset();
    SceneSDFConstructionState construction_state(_world, aabb);
    // first save world state
    _world->saveState();
    _bounding_box = aabb;
    // classify objects based on whether they are static or not
    logger->logDebug("Classifying objects", log_prefix);
    std::vector<std::string> static_obstacles;
    std::vector<std::string> movable_objects;
    std::vector<sim_env::ObjectPtr> objects;
    _world->getObjects(aabb, objects, true);
    for (auto object : objects) {
        if (object->isStatic()) {
            static_obstacles.push_back(object->getName());
        } else {
            movable_objects.push_back(object->getName());
            _movables.push_back(object);
            _movable_sdfs.push_back(SDF());
        }
    }
    // now, let's create the static sdf first
    _has_statics = not static_obstacles.empty();
    if (_has_statics) {
        logger->logDebug("Creating static SDF", log_prefix);
        _static_sdf.computeSDF(_world, aabb, static_cell_size, true, movable_objects);
    }

    // next create an sdf for each movable object
    Eigen::Affine3f identity_tf;
    identity_tf.setIdentity();
    if (not movable_objects.empty()) { // if we have movable objects
        logger->logDebug("Creating movable SDFs", log_prefix);
        // create ignore list that always contains all object names except for the movable we create an sdf for
        std::vector<std::string> ignore_list;
        if (not static_obstacles.empty()) {
            ignore_list.insert(ignore_list.begin(), static_obstacles.begin(), static_obstacles.end());
        }
        size_t offset = ignore_list.size();
        for (auto movable_name : movable_objects) {
            ignore_list.push_back(movable_name);
        }
        for (size_t m = 0; m < movable_objects.size(); ++m) { // run over all movable objects
            auto current_obj_name = movable_objects[m];
            if (movable_sdfs.count(current_obj_name)) { // if we have an sdf for it, load it
                logger->logDebug("Loading sdf for object " + current_obj_name, log_prefix);
                _movable_sdfs[m].load(movable_sdfs.at(current_obj_name));
            } else { // else create a new one
                // move the current object name to the end of the ignore list
                assert(current_obj_name == ignore_list[m + offset]);
                std::swap(ignore_list[m + offset], ignore_list[ignore_list.size() - 1]);
                ignore_list.pop_back(); // temporary delete current_obj_name from ignore list
                // next align object and world frame
                auto movable = _movables[m].lock();
                if (!movable) throw std::logic_error("Could not acquire access to object. Weak pointer expired");
                auto orig_tf = movable->getTransform();
                movable->setTransform(identity_tf);
                // compute the size we need for the sdf
                auto object_aabb = movable->getLocalAABB();  // TODO this is configuration dependent -> we actually need SDFs for each link
                float radius = 0.0f;
                if (radii.count(current_obj_name)) {
                    radius = radii.at(current_obj_name);
                }
                auto sdf_aabb = computeSDFSize(object_aabb, max_approx_error, radius);
                // now we are ready to compute the sdf for this object
                logger->logDebug("Creating SDF for object " + current_obj_name, log_prefix);
                _movable_sdfs[m].computeSDF(_world, sdf_aabb, movable_cell_size, true, ignore_list);
                _movable_sdfs[m].setApproximationBox(object_aabb);
                // after this, clean up by resetting the tf and fixing ignore list again
                ignore_list.push_back(current_obj_name);
                std::swap(ignore_list[m + offset], ignore_list[ignore_list.size() - 1]);
                movable->setTransform(orig_tf);
                assert(current_obj_name == ignore_list[m + offset]);
            }
        }
    }
    _world->restoreState();
    logger->logDebug("SDF creation finished.", log_prefix);
    return construction_state;
}

float SceneSDF::getDistance(const Eigen::Vector3f& position)
{
    float min_distance = std::numeric_limits<float>::max();
    for (size_t i = 0; i < _movables.size(); ++i) {
        auto movable = _movables[i].lock();
        auto tf = movable->getTransform();
        _movable_sdfs[i].setTransform(tf);
        min_distance = std::min(min_distance, _movable_sdfs[i].getDistance(position));
    }
    if (_has_statics) {
        min_distance = std::min(min_distance, _static_sdf.getDistance(position));
    }
    return min_distance;
}

void SceneSDF::getDistances(const std::vector<Eigen::Vector3f>& positions, std::vector<float>& distances)
{
    distances.resize(positions.size(), std::numeric_limits<float>::max());
    for (size_t i = 0; i < _movables.size(); ++i) {
        auto movable = _movables[i].lock();
        auto tf = movable->getTransform();
        _movable_sdfs[i].setTransform(tf);
        for (size_t j = 0; j < positions.size(); ++j) {
            distances[j] = std::min(distances[j], _movable_sdfs[i].getDistance(positions[j]));
        }
    }
    if (_has_statics) {
        for (size_t j = 0; j < positions.size(); ++j) {
            distances[j] = std::min(distances[j], _static_sdf.getDistance(positions[j]));
        }
    }
}

void SceneSDF::updateTransforms() {
    for (size_t i = 0; i < _movables.size(); ++i) {
        auto movable = _movables[i].lock();
        auto tf = movable->getTransform();
        _movable_sdfs[i].setTransform(tf);
    }
}

float SceneSDF::getDistanceDirty(const Eigen::Vector3f& position) {
    float min_distance = std::numeric_limits<float>::max();
    for (auto& movable_sdf : _movable_sdfs) {
        min_distance = std::min(min_distance, movable_sdf.getDistance(position));
    }
    if (_has_statics) {
        min_distance = std::min(min_distance, _static_sdf.getDistance(position));
    }
    return min_distance;
}

sim_env::BoundingBox SceneSDF::getBoundingBox() const {
    return _bounding_box;
}

sim_env::BoundingBox SceneSDF::computeSDFSize(const sim_env::BoundingBox& aabb, float approx_error, float radius) const
{
    auto extents = aabb.extents();
    auto center = aabb.center();
    float scaling_factor = (1.0f - (1.0f - approx_error) * radius / extents.norm()) / approx_error;
    auto scaled_extents = scaling_factor * extents;
    sim_env::BoundingBox new_aabb;
    new_aabb.min_corner = center - scaled_extents;
    new_aabb.max_corner = center + scaled_extents;
    return new_aabb;
}

void SceneSDF::reset() {
    _movables.clear();
    _movable_sdfs.clear();
    _has_statics = false;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file parelm_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE parelm_test
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/modal_formula/parse.h"
#include "mcrl2/pbes/complement.h"
#include "mcrl2/pbes/lps2pbes.h"
#include "mcrl2/pbes/parelm.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

const std::string SPECIFICATION =
  "act a:Nat;                               \n"
  "                                         \n"
  "map smaller: Nat#Nat -> Bool;            \n"
  "                                         \n"
  "var x,y : Nat;                           \n"
  "                                         \n"
  "eqn smaller(x,y) = x < y;                \n"
  "                                         \n"
  "proc P(n:Nat) = sum m: Nat. a(m). P(m);  \n"
  "                                         \n"
  "init P(0);                               \n";

const std::string TRIVIAL_FORMULA  = "[true*]<true*>true";

BOOST_AUTO_TEST_CASE(test_parelm1)
{
  lps::specification spec = lps::remove_stochastic_operators(lps::linearise(lps::detail::ABP_SPECIFICATION()));
  state_formulas::state_formula formula = state_formulas::parse_state_formula(TRIVIAL_FORMULA, spec);
  bool timed = false;
  pbes p = lps2pbes(spec, formula, timed);
  pbes_parelm_algorithm algorithm;
  algorithm.run(p);
  BOOST_CHECK(p.is_well_typed());
}


///
/// if force € [0, FORCE_F_1_2_THRESH] then state 1 : lower cup
/// if force € (FORCE_F_1_2_THRESH, FORCE_F_2_3_THRESH] then state 2 : do nothing
/// if force € (FORCE_F_2_3_THRESH, MAX] then state 3 : raise cup
///
/// Fallback velocity functions
///	Vel.x				                                  Vel.y
/// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
///   90 to 180	  y =  0.004444444*x - 0.8
///
///
///

#include <ros/console.h>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseStamped.h"
#include "mutex"
#include "angles/angles.h"
#include "std_msgs/Int32.h"
#include "audio_emergency/AudioMessage.h"

#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"

#include <hri_package/Sens_Force.h>
#include <kinova_msgs/PoseVelocity.h>

#include <fstream>
#include <vector>
#include <iterator>
#include <string>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#define FORCE_F_1_2_THRESH 0.05
#define FORCE_F_2_3_THRESH 0.5
#define NUMBER_OF_ARM_SUB_STATES 10
#define UPPER_FEED_ANGLE_THRESH 180.00 // was 140
#define FORCE_SAFETY 5
#define SUB_SAMPLED_SIZE 10

#define ARC_DOWN 1
#define ARC_UP 2
#define TRANSLATE_BACK 3
#define RAISE_CUP 4
#define LOWER_CUP 5
#define TRANSLATE_UP 6
#define TRANSLATE_DOWN 7
#define TRANSLATE_FRONT 8
#define END_EFF_FRAME "j2s7s300_end_effector"
#define BASE_FRAME "j2s7s300_link_base"
#define SENSOR_FRAME "forcesensor"

#define MAX_STEPS 200
#define VEL_LIN_MAX 0.04
#define VEL_ANG_MAX 0.4
#define VEL_CMD_DURATION 0.8

#define ACTION_DOWN 0
#define ACTION_STAY 1
#define ACTION_UP 2

double lower_angle_thresh = 85;
float thresh_lin = 0.01;
float thresh_ang = 0.05;
double rotation_step;
bool emergency = false;

int step_count;

std::string result;
std::mutex lock_pose;
std::mutex lock_status;
std::mutex lock_proc;
std::mutex lock_force;
std::mutex lock_emerg;
double force_f, force_b;
bool processing=false;
ros::Publisher cmd_pos;
ros::Publisher cmd_vel;
geometry_msgs::PoseStamped current_pose, initial_pose;

void getRPYFromQuaternionMSG(geometry_msgs::Quaternion orientation, double& roll,double& pitch, double& yaw)
{
  tf::Quaternion quat;
  // Suppress a weird warning that tells me to normalise the quats even though I do that.
  if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) ros::console::notifyLoggerLevelsChanged();
  tf::quaternionMsgToTF(orientation,quat);
  if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) ros::console::notifyLoggerLevelsChanged();
  quat.normalize();

  tf::Matrix3x3 mat(quat);

  mat.getRPY(roll, pitch,yaw);

}

void waitForActionCompleted()
{
  std::string temp_res="";
  while(ros::ok())
  {
    ros::spinOnce();
    lock_status.lock();
    temp_res = result;
    lock_status.unlock();

    if (temp_res=="Stopped")
    {
      ROS_INFO_STREAM("Status : " << temp_res);
      //Wait a little extra for arm to settle due to inertia.
      ros::Time start = ros::Time::now();
      while(ros::Time::now()-start < ros::Duration(0.2)) ros::spinOnce();
      break;
    }
  }
}

void setPoseForDirection(int direction, geometry_msgs::PoseStamped& start_pose,double  distance)
{
  geometry_msgs::Quaternion quat;
  switch(direction)
  {
  /// END EFFECTOR FRAME  Z-forward-blue, Y-up-green, X-left-red
  case ARC_DOWN :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(rotation_step),angles::from_degrees(0),angles::from_degrees(0)); //Turn down
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0.01;  // Slightly up
    start_pose.pose.position.z=-0.01; // Slightly back
    start_pose.pose.orientation = quat;
    break;
  case ARC_UP :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(-rotation_step),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=-0.01;
    start_pose.pose.position.z=0.01;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_BACK :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=-distance;
    start_pose.pose.orientation = quat;
    break;
  case TRANSLATE_FRONT :
    quat =  tf::createQuaternionMsgFromRollPitchYaw(angles::from_degrees(0),angles::from_degrees(0),angles::from_degrees(0));
    start_pose.header.frame_id=END_EFF_FRAME;
    start_pose.pose.position.x=0;
    start_pose.pose.position.y=0;
    start_pose.pose.position.z=distance;
    start_pose.pose.orientation = quat;
    break;

  /// SENSORFRAME  X-forward-red, Y-left-green, Z-up-blue
  }
}

geometry_msgs::TwistStamped getTwistForDirection(int direction)
{
  geometry_msgs::TwistStamped twist;
  twist.header.stamp=ros::Time::now();
  twist.header.frame_id="j2s7s300_link_7";
  switch(direction)
  {
    case ARC_DOWN:
      ROS_INFO("Arc down");
      twist.twist.linear.z=VEL_LIN_MAX;
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case ARC_UP:
      ROS_INFO("Arc up");
      twist.twist.linear.z=-VEL_LIN_MAX;
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_BACK:
      ROS_INFO("Translate back");
      twist.twist.linear.x=-VEL_LIN_MAX;
    break;

    case RAISE_CUP:
      ROS_INFO("Raise Cup");
      twist.twist.angular.x=VEL_ANG_MAX;
    break;

    case LOWER_CUP:
      ROS_INFO("Lower Cup");
      twist.twist.angular.x=-VEL_ANG_MAX;
    break;

    case TRANSLATE_UP:
      ROS_INFO("Translate up");
      twist.twist.linear.z=VEL_LIN_MAX;
    break;

    case TRANSLATE_DOWN:
      ROS_INFO("Translate down");
      twist.twist.linear.z=-VEL_LIN_MAX;
    break;

  }
  return twist;
}

bool checkUpperAngleThreshold();
bool checkLowerAngleThreshold();

void publishTwistForDuration(geometry_msgs::TwistStamped twist_msg, double duration)
{
  ros::Time time_start = ros::Time::now();
  while (ros::Time::now() - time_start < ros::Duration(duration))
  {
    ros::spinOnce();
    cmd_vel.publish(twist_msg);
  }
}

void positionControlDriveForDirection(int direction, double distance)
{
  geometry_msgs::PoseStamped start_pose, pose_in_base;
  tf::TransformListener tf_listener;
  //Wait for TF buffer to fill up
  ros::Duration(0.25).sleep();
  ros::Rate loop_rate(10);

  while(ros::ok())
  {
    try
    {
      lock_pose.lock();
      start_pose = current_pose;
      lock_pose.unlock();

      setPoseForDirection(direction, start_pose, distance);

      tf_listener.transformPose(BASE_FRAME,start_pose,pose_in_base);
      break;

    }
    catch (tf::ExtrapolationException e)
    {
     /* ROS_INFO_STREAM("Waiting for transform "
                      //<< e.what()
                      );*/
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  cmd_pos.publish(pose_in_base);
}

bool isForceSafe()
{
  double local_force_f;
  lock_force.lock();
  local_force_f=force_f;
  lock_force.unlock();

  if (local_force_f >= FORCE_SAFETY)
  {
    ROS_WARN_STREAM("PAIN THRESHOLD BREACHED!!");
    return false;
  }
  else
    return true;
}

bool isAudioSafe()
{
  lock_emerg.lock();
  bool emerg = emergency;
  lock_emerg.unlock();

  if (emerg) ROS_WARN_STREAM("AUDIO EMERGENCY DETECTED!!");
  return !emerg;
}

void fallback(bool emerg=false);

void driveToRollGoalWithVelocity(int direction)
{
  if(direction == RAISE_CUP)
    ROS_INFO_STREAM("Raise cup");
  else if (direction == LOWER_CUP)
    ROS_INFO_STREAM("Lower cup");
  else
  {
    ROS_ERROR_STREAM("Wrong direction given!!");
    return;
  }

  geometry_msgs::PoseStamped start_pose, goal_pose, temp_pose;

  lock_pose.lock();
  start_pose=current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pit, temp_yaw;
  double goal_rol, goal_pit, goal_yaw;
  double start_rol, start_pit, start_yaw;
  double del_rol;

  goal_pose = start_pose;

  tf::Quaternion q_rot = tf::createQuaternionFromRPY(angles::from_degrees( direction==RAISE_CUP?rotation_step:-rotation_step),angles::from_degrees(0),angles::from_degrees(0)); // Rotate about x by 20 degrees
  tf::Quaternion q_start; tf::quaternionMsgToTF(start_pose.pose.orientation, q_start);
  tf::Quaternion q_goal = q_start*q_rot;

  tf::quaternionTFToMsg(q_goal,goal_pose.pose.orientation);

  ros::Rate loop_rate(100);
  //ros::Time start = ros::Time::now();
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    getRPYFromQuaternionMSG(goal_pose.pose.orientation, goal_rol, goal_pit, goal_yaw);
    getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pit, temp_yaw);
    getRPYFromQuaternionMSG(start_pose.pose.orientation, start_rol, start_pit, start_yaw);

    if (goal_rol < 0 ) goal_rol = goal_rol + angles::from_degrees(360);
    if (temp_rol < 0 ) temp_rol = temp_rol + angles::from_degrees(360);

    del_rol = goal_rol - temp_rol;

    if (std::fabs(del_rol)>=thresh_ang)
    {
      float linear_vel = 0.02;
      geometry_msgs::TwistStamped twist_msg;
      twist_msg.twist.linear.x = 0;
      twist_msg.twist.linear.y = 0;
      // Comment out next line ONLY if using end-effector mode ie: Mode 1 on jostick is active and 4th blue LED on far right is active.
//      twist_msg.twist.linear.z = direction==RAISE_CUP ? linear_vel : -linear_vel;
      twist_msg.twist.angular.x= del_rol>0?VEL_ANG_MAX:-VEL_ANG_MAX;
      twist_msg.twist.angular.y = 0;
      twist_msg.twist.angular.z= 0;

      if(isForceSafe() && isAudioSafe())
      {
        cmd_vel.publish(twist_msg);
      }
      else
      {
        ROS_WARN("ABORTING SEQUENCE!!!");
        fallback(true);
        ros::shutdown();
      }
    }
    else
    {
      //ROS_INFO_STREAM("Goal reached, breaking loop");
      break;
    }

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

}

void moveCup(int direction, double duration=VEL_CMD_DURATION, double distance=0.1)
{
  if (direction==TRANSLATE_BACK || direction==TRANSLATE_FRONT)
  {
    positionControlDriveForDirection(direction, distance);
    return;
  }
  else
  {
    geometry_msgs::TwistStamped twist_msg = getTwistForDirection(direction);
    publishTwistForDuration(twist_msg, duration);

  }
  return;
}

void poseGrabber(geometry_msgs::PoseStamped pose)
{
  lock_pose.lock();
  current_pose=pose;
  lock_pose.unlock();
}

void forceGrabber(const hri_package::Sens_Force::ConstPtr msg)
{
  lock_force.lock();
  force_f=msg->forceF;
  force_b=msg->forceB;
  lock_force.unlock();
}

void statusGrabber(std_msgs::String::ConstPtr status)
{
  lock_status.lock();
  result=status->data;
  lock_status.unlock();
}

void waitForPoseDataAvailable()
{
  // This loop to wait until subscriber starts returning valid poses.
  ros::Rate loop_rate(9);
  geometry_msgs::PoseStamped temp_pose;
  while(ros::ok())
  {
    lock_pose.lock();
    temp_pose = current_pose;
    lock_pose.unlock();

    ros::spinOnce();
    loop_rate.sleep();

    if (temp_pose.pose.position.x != 0) break;
  }
  ROS_INFO("Pose data available");
}

bool checkUpperAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ( angles::to_degrees(temp_roll) > UPPER_FEED_ANGLE_THRESH )
  {
    ROS_WARN_STREAM("MAX UPPER FEED ANGLE REACHED");
    return false;
  }
  else
  {
   return true;
  }
}

bool checkLowerAngleThreshold()
{
  geometry_msgs::PoseStamped temp_pose;

  lock_pose.lock();
  temp_pose=current_pose;
  lock_pose.unlock();

  double temp_roll, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation,temp_roll, temp_pitch, temp_yaw);

  if ((int)angles::to_degrees(temp_roll) - (int)angles::to_degrees(lower_angle_thresh) < rotation_step/2 && temp_roll > 0)
  {
    //ROS_WARN_STREAM("MAX LOWER ANGLE REACHED. LIMIT: " << angles::to_degrees(lower_angle_thresh) << " CURRENT: " << angles::to_degrees(temp_roll));
    return false;
  }
  else
  {
//   ROS_INFO_STREAM("Current roll: " << sangles::to_degrees(temp_roll));
   return true;
  }
}

double getCurrentRoll()
{
  lock_pose.lock();
  geometry_msgs::PoseStamped temp_pose = current_pose;
  lock_pose.unlock();

  double temp_rol, temp_pitch, temp_yaw;
  getRPYFromQuaternionMSG(temp_pose.pose.orientation, temp_rol, temp_pitch, temp_yaw);

  return temp_rol<0?temp_rol+angles::from_degrees(360):temp_rol;
}

void callFallbackTimer(double duration)
{

  ros::Time start = ros::Time::now();
  ros::Duration run_for(duration);
  ros::Rate loop_rate(10);
  ROS_WARN_STREAM("Starting fallback timeout for: " << duration << "s.");
  while(ros::Time::now() - start <= run_for)
  {
    lock_force.lock();
    double force = force_f;
    lock_force.unlock();

    if (force>= FORCE_F_2_3_THRESH)
    {
      ROS_INFO_STREAM("Force detected, breaking out of timer");
      return;
    }

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

  ROS_WARN_STREAM("Time out! Initialising fallback");
 // moveCup(TRANSLATE_BACK);
  fallback();
  sleep(1);
  ros::shutdown();
}

void fallback(bool emerg)
{
  lock_pose.lock();
  geometry_msgs::PoseStamped  start_pose=current_pose;
  lock_pose.unlock();

  double roll, pitch, yaw;
  getRPYFromQuaternionMSG(start_pose.pose.orientation, roll, pitch, yaw);

  int yaw_degrees = angles::to_degrees(yaw);

  // ROS_INFO_STREAM("Current yaw float: " << angles::to_degrees(yaw) << " int " << yaw_degrees);

  geometry_msgs::TwistStamped twist_cmd;

  // y = mx + c
  double m = 0.004444444;
  double velx_c = 0.8;
  double vely_c = 0.4;

  /// Fallback velocity functions
  ///	Vel.x				                                  Vel.y
  /// -180 to -90	  y =  0.004444444*x + 0.8			  -180 to   0	y =  0.004444444*x + 0.4
  ///  -90 to  90	  y = -0.004444444*x 			           0 to 180	y = -0.004444444*x + 0.4
  ///   90 to 180	  y =  0.004444444*x - 0.8

  if (yaw_degrees >= -180 && yaw_degrees < -90)
  {
    //ROS_INFO_STREAM("Sector 1");
    twist_cmd.twist.linear.x = m * yaw_degrees + velx_c;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= -90 &&  yaw_degrees <0)
  {
    //ROS_INFO_STREAM("Sector 2");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 0 &&  yaw_degrees <90)
  {
    //ROS_INFO_STREAM("Sector 3");
    twist_cmd.twist.linear.x = -m * yaw_degrees;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  else if (yaw_degrees >= 90 &&  yaw_degrees <=180)
  {
    //ROS_INFO_STREAM("Sector 4");
    twist_cmd.twist.linear.x = m * yaw_degrees - velx_c;
    twist_cmd.twist.linear.y = -m * yaw_degrees + vely_c;
  }
  //ROS_INFO_STREAM("Publishing twist : ");
  //std::cout << (twist_cmd) << std::endl;
  if (emerg==true) twist_cmd.twist.angular.x = -1;
  publishTwistForDuration(twist_cmd,1);
}

class CSVReader
{
    std::string fileName;
    std::string delimeter;

public:
    CSVReader(std::string filename, std::string delm = ",") :
        fileName(filename), delimeter(delm)
    { }

    // Function to fetch data from a CSV File
    std::vector<std::vector<std::string> > getData();
};

std::vector<std::vector<std::string> > CSVReader::getData()
{
    std::ifstream file(fileName);

    std::vector<std::vector<std::string> > dataList;

    std::string line = "";
    // Iterate through each line and split the content using delimeter
    while (getline(file, line))
    {
        std::vector<std::string> vec;
        boost::algorithm::split(vec, line, boost::is_any_of(delimeter));
        dataList.push_back(vec);
    }
    // Close the File
    file.close();

    return dataList;
}

std::vector<std::vector<float>> getGMRFromCsv(std::string file)
{

   CSVReader reader(file);

   std::vector<std::vector<std::string> > dataList = reader.getData();

   int RR = dataList.size();
   int CC = dataList[0].size();
    std::vector<std::vector<float>> gmr(RR);
    for ( int i = 0 ; i < RR ; i++ )
       gmr[i].resize(CC);

  ROS_INFO_STREAM("Fetching GMR data from " << file );
  for (int i=0; i<dataList.size(); i++)
   {
     std::vector<std::string> vec = dataList[i];
     for(int j=0; j< vec.size();j++ )
     {
       gmr[i][j]= std::stof(vec[j]);
     }
   }
    return gmr;
}

/**
 * @brief Calculate the delta pose vector i.e.: the difference of each pose from the first pose
 * @param gmr
 * @return pose_delta
 */
std::vector<std::vector<float>> getPoseDeltaFromGMR(std::vector<std::vector<float>> gmr)
{

   int RR = gmr.size();
   int CC = 6; // 6 columns for deltas in x,y,z,roll,pitch,yaw
   std::vector<std::vector<float>> pose_delta(RR);
   for ( int i = 0 ; i < RR ; i++ )
      pose_delta[i].resize(CC);

   geometry_msgs::PoseStamped first_pose;
   first_pose.pose.position.x = gmr[0][1]; //columen 0 is index number, dropping it
   first_pose.pose.position.y = gmr[0][2];
   first_pose.pose.position.z = gmr[0][3];
   first_pose.pose.orientation.x = gmr[0][4];
   first_pose.pose.orientation.y = gmr[0][5];
   first_pose.pose.orientation.z = gmr[0][6];
   first_pose.pose.orientation.w = gmr[0][7];
   double roll_first, pitch_first, yaw_first;
   getRPYFromQuaternionMSG(first_pose.pose.orientation, roll_first, pitch_first, yaw_first);

// First row/pose is always 0 since it is the reference point.
   for (int i=0; i<RR; i++)
   {
     geometry_msgs::PoseStamped current_pose;
     current_pose.pose.position.x = gmr[i][1];  //columen 0 is index number, dropping it
     current_pose.pose.position.y = gmr[i][2];
     current_pose.pose.position.z = gmr[i][3];
     current_pose.pose.orientation.x = gmr[i][4];
     current_pose.pose.orientation.y = gmr[i][5];
     current_pose.pose.orientation.z = gmr[i][6];
     current_pose.pose.orientation.w = gmr[i][7];

     double roll_curr, pitch_curr, yaw_curr;

     getRPYFromQuaternionMSG(current_pose.pose.orientation,roll_curr, pitch_curr, yaw_curr);

     double delta_roll, delta_pitch, delta_yaw;
     delta_roll =  roll_curr - roll_first ;
     delta_pitch = pitch_curr - pitch_first ;
     delta_yaw = yaw_curr - yaw_first;

     pose_delta[i][0] = current_pose.pose.position.x - first_pose.pose.position.x;
     pose_delta[i][1] = current_pose.pose.position.y - first_pose.pose.position.y;
     pose_delta[i][2] = current_pose.pose.position.z - first_pose.pose.position.z;
     pose_delta[i][3] = delta_roll; // Forward feeding approach changes only roll. Change in pitch and yaw <1° and hence ignored. Direct addition of single euler angle works between quaternions. NOT multiple angles.
//     pose_delta[i][4] = delta_pitch;    // Even if there are pitches and yaws due to human error, they need to be suppressed to avoid_discomfort to the user/spillage_.
//     pose_delta[i][5] = delta_yaw;
   }

   return pose_delta;
}

// Take the actual starting pose from current run, consider the index of the GMR trajectory required, add the delta to take the starting pose to new index
geometry_msgs::PoseStamped getNewPoseAtIndex(geometry_msgs::PoseStamped starting_pose, int new_index_in_trajectory, std::vector<std::vector<float>> pose_delta_subsampled )
{
  geometry_msgs::PoseStamped new_pose;
  new_pose.pose.position.x = starting_pose.pose.position.x + pose_delta_subsampled[new_index_in_trajectory][0];
  new_pose.pose.position.y = starting_pose.pose.position.y + pose_delta_subsampled[new_index_in_trajectory][1];
  new_pose.pose.position.z = starting_pose.pose.position.z + pose_delta_subsampled[new_index_in_trajectory][2];

  double starting_roll, starting_pitch, starting_yaw, new_roll, new_pitch, new_yaw;
  getRPYFromQuaternionMSG(starting_pose.pose.orientation, starting_roll, starting_pitch, starting_yaw);
  new_roll = starting_roll + pose_delta_subsampled[new_index_in_trajectory][3];
  new_pitch = starting_pitch + pose_delta_subsampled[new_index_in_trajectory][4];
  new_yaw = starting_yaw+ pose_delta_subsampled[new_index_in_trajectory][5];

  tf::Quaternion q_new = tf::createQuaternionFromRPY(new_roll, new_pitch, new_yaw);
  q_new.normalize();
  tf::quaternionTFToMsg(q_new,new_pose.pose.orientation);

  return new_pose;
}

void audioGrabber(audio_emergency::AudioMessage::ConstPtr msg)
{
  lock_emerg.lock();
  if (msg->result == "talk") emergency = true;
  lock_emerg.unlock();
}

void safePauseFor(float duration)
{
  ros::Time start = ros::Time::now();
  ros::Rate loop_rate(10);
  while(ros::Time::now()-start < ros::Duration(duration))
  {
    ros::spinOnce();
    if (!isAudioSafe() || !isForceSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }
    loop_rate.sleep();
  }
}

void printPose(geometry_msgs::PoseStamped pose, std::string text)
{
  double roll, pitch, yaw;
  getRPYFromQuaternionMSG(pose.pose.orientation, roll, pitch, yaw);
  ROS_INFO_STREAM(text << pose.pose.position.x << " "
                  << pose.pose.position.y << " "
                  << pose.pose.position.z << " "
                  << angles::to_degrees(roll) << " "
                  << angles::to_degrees(pitch) << " "
                  << angles::to_degrees(yaw) << " " );
}

void printGMRRow(std::vector<std::vector<float>> gmr, int row, std::string text)
{

    double roll, pitch, yaw;
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = gmr[row][1];
    pose.pose.position.y = gmr[row][2];
    pose.pose.position.z = gmr[row][3];
    pose.pose.orientation.x = gmr[row][4];
    pose.pose.orientation.y = gmr[row][5];
    pose.pose.orientation.z = gmr[row][6];
    pose.pose.orientation.w = gmr[row][7];

    getRPYFromQuaternionMSG(pose.pose.orientation, roll, pitch, yaw);

    ROS_INFO_STREAM(text << pose.pose.position.x << " "
                    << pose.pose.position.y << " "
                    << pose.pose.position.z << " "
                    << angles::to_degrees(roll) << " "
                    << angles::to_degrees(pitch) << " "
                    << angles::to_degrees(yaw) << " " );

}

std::vector<std::vector<float>> subSamplePoseDelta(std::vector<std::vector<float>> pose_delta, int resultant_length)
{
  int sampling_freq = pose_delta.size()/resultant_length;
  std::vector<std::vector<float>> pose_delta_subsampled(resultant_length);
  // Resize the subsampled set to have the number of rows as required by resultant length but number of columns same as pose_delta
  for(int i=0; i< resultant_length; i++)
  {
    pose_delta_subsampled[i].resize(pose_delta[0].size());
  }

  // Subsample pose_delta according to sampling frequency
  for (int i=0; i<resultant_length; i++)
  {
    pose_delta_subsampled[i] = pose_delta[i*sampling_freq];
  }

  return pose_delta_subsampled;
}

void driveToPoseStep(int step_count,std::vector<std::vector<float>> pose_delta_subsampled )
{
  ros::spinOnce();

  lock_pose.lock();
  geometry_msgs::PoseStamped temp_pose=current_pose;
  lock_pose.unlock();

  ROS_INFO_STREAM("Moving to WP" << step_count);
  geometry_msgs::PoseStamped pose_new = getNewPoseAtIndex(initial_pose, step_count, pose_delta_subsampled);
  printPose(temp_pose , "Cur pose: ");
  printPose(pose_new, "New pose: ");
  cmd_pos.publish(pose_new);
  waitForActionCompleted();
  ROS_INFO_STREAM("Done");
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "feeder_gmm");
  ros::NodeHandle nh;

  std::vector<std::vector<float>> gmr = getGMRFromCsv(argv[1]);
  std::vector<std::vector<float>> pose_delta = getPoseDeltaFromGMR(gmr);
  std::vector<std::vector<float>> pose_delta_subsampled = subSamplePoseDelta(pose_delta, SUB_SAMPLED_SIZE);

  ROS_INFO_STREAM("Size of GMR : " << gmr.size() << " pose_delta : " << pose_delta.size() << " pose_delta_subsampled : " << pose_delta_subsampled.size());

  tf::TransformListener tf_listener;

  ros::Subscriber sub = nh.subscribe("/force_values", 1000, forceGrabber);
  cmd_vel = nh.advertise<geometry_msgs::TwistStamped>("/RobotControl/VelocityControl", 1000);
  cmd_pos = nh.advertise<geometry_msgs::PoseStamped>("/RobotControl/PoseControl", 1000);
  ros::Subscriber tool_pose = nh.subscribe("/j2s7s300_driver/out/tool_pose",1000, poseGrabber );
  ros::Subscriber control_status = nh.subscribe("/RobotControl/Status",1000, statusGrabber );
  ros::Publisher arm_pose_pub = nh.advertise<std_msgs::Int32>("/arm_state", 1000);
  ros::Subscriber audio_emerg = nh.subscribe("/audio_emergency",1000, audioGrabber );

  waitForPoseDataAvailable();

  double local_force_f;
  bool print_once_only=true;

  lock_pose.lock();
  initial_pose=current_pose;
  lock_pose.unlock();

 //TODO : Implement the driver below

  step_count = 0;
  int prev_step_count = 0;

  ros::Rate loop_rate(10);
  while(ros::ok())
  {
    ros::spinOnce();

    lock_force.lock();
    local_force_f=force_f;
    lock_force.unlock();

    if (local_force_f >= 0  && local_force_f <= FORCE_F_1_2_THRESH && step_count >0)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("STEP_DOWN for " << local_force_f << "N from step " << step_count);
      prev_step_count = step_count--;
      driveToPoseStep(step_count,pose_delta_subsampled);
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");

      if (prev_step_count > step_count && step_count==0)
      {
        callFallbackTimer(3);
      }
    }

    else if (local_force_f >= FORCE_F_2_3_THRESH && local_force_f <= FORCE_SAFETY && step_count < SUB_SAMPLED_SIZE-1)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("STEP_UP for " << local_force_f << "N from step " << step_count);
      prev_step_count =  step_count++;
      driveToPoseStep(step_count,pose_delta_subsampled);
      print_once_only=true;
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");
    }

    else if (print_once_only)
    {
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO_STREAM("Do nothing.");
      print_once_only=false;
      ROS_WARN_STREAM("Step : " << prev_step_count << " -> " << step_count  << " @ roll : " << angles::to_degrees(getCurrentRoll()));
      ROS_INFO("---------------------------------------------------------------------");
      ROS_INFO(" ");

    }

    loop_rate.sleep();
    std_msgs::Int32 arm_pose_msg;
    arm_pose_msg.data=step_count;
    arm_pose_pub.publish(arm_pose_msg);
    ros::spinOnce();

    if (!isAudioSafe())
    {
      ROS_WARN_STREAM("ABORTING SEQUENCE!!");
      fallback(true);
      ros::shutdown();
    }
  }
  return 0;
}


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

#include <boost/simd/meta/is_power_of_2.hpp>
#include <boost/simd/preprocessor/parameters.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/assert.hpp>
#include <cstddef>

namespace boost { namespace simd {  namespace meta
{
  /*!
    @brief Align integral value on arbitrary boundary

    Statically align an integral value on an arbitrary alignment boundary.

    @par Semantic

    For any integral value @c Value and any power of 2 @c Align :

    @code
    typedef align_on_c<Value,Align>::type r;
    @endcode

    is equivalent to

    @code
    typedef integral_constant<std::size_t,N>::type r;
    @endcode

    where @c N verifies :

    @code
    N >= Value && N % Align = 0
    @endcode

    If @c Align is not a power of two, a @c BOOST_SIMD_INVALID_ALIGNMENT_VALUE
    static assertion is triggered.

    @usage{meta/align_on_c.cpp}

    @tparam Value Value to align.
    @tparam Align Integral power of two alignment boundary. By default,
            @c Align is equal to current SIMD extension alignment boundary.
  **/
  template< std::size_t Value
          , std::size_t Align = BOOST_SIMD_CONFIG_ALIGNMENT
          >
  struct  align_on_c
    #if !defined(DOXYGEN_ONLY)
        : boost::mpl::integral_c<std::size_t, (Value+Align-1) & ~(Align-1) >
    #endif
  {
    #if !defined(DOXYGEN_ONLY)
    //==========================================================================
    //               ****BOOST_SIMD_INVALID_ALIGNMENT_VALUE****
    //       If you trigger this assert, you tried to align a value on
    //   a boundary which is not a power of two. Fix your code accordingly.
    //               ****BOOST_SIMD_INVALID_ALIGNMENT_VALUE****
    //==========================================================================
    /// INTERNAL ONLY
    BOOST_MPL_ASSERT_MSG( (meta::is_power_of_2_c<Align>::value)
                        , BOOST_SIMD_INVALID_ALIGNMENT_VALUE
                        , (boost::mpl::integral_c<std::size_t,Align>)
                        );
    #endif
  };

  /*!
    @brief Align @mplint on arbitrary boundary

    Statically align @mplint value on arbitrary alignment boundary.

    @par Semantic

    For any @mplint @c Value and any power of 2 @c Align :

    @code
    typedef align_on<Value,Align>::type r;
    @endcode

    is equivalent to

    @code
    typedef integral_constant<Value::value_type,N>::type r;
    @endcode

    where @c N verifies :

    @code
    N >= Value::value && N % Align::value = 0
    @endcode

    If @c Align is not a power of two, a  @c BOOST_SIMD_INVALID_ALIGNMENT_VALUE
    static assertion is triggered.

    @par Models:

    @metafunction

    @usage{meta/align_on.cpp}

    @tparam Value Value to align
    @tparam Align @mplint power of two alignment boundary. By default,
            @c Align is equal to current SIMD extension alignment boundary.
  **/
  template< typename Value
          , typename Align = boost::mpl::integral_c<std::size_t,BOOST_SIMD_CONFIG_ALIGNMENT>
          >
  struct align_on
#if !defined(DOXYGEN_ONLY)
       :  boost::mpl::integral_c
          < typename Value::value_type
          , typename Value::value_type(align_on_c < std::size_t(Value::value)
                                                  , std::size_t(Align::value)
                                                  >::value
                                      )
          >
#endif
  {};
} } }

#endif