#include "AinurState.h"
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

namespace PsimagLite {

template<typename T>
boost::spirit::qi::rule<std::string::iterator,
std::vector<T>(),
boost::spirit::qi::space_type>
ruleRows();

template<>
boost::spirit::qi::rule<std::string::iterator,
std::vector<double>(),
boost::spirit::qi::space_type>
ruleRows<double>()
{
	return "[" >> -(boost::spirit::double_ % ",") >> "]";
}

template<>
boost::spirit::qi::rule<std::string::iterator,
std::vector<SizeType>(),
boost::spirit::qi::space_type>
ruleRows<SizeType>()
{
	return "[" >> -(boost::spirit::int_ % ",") >> "]";
}

//---------
template<typename T>
boost::spirit::qi::rule<std::string::iterator,
T(),
boost::spirit::qi::space_type>
ruleElipsis();

template<>
boost::spirit::qi::rule<std::string::iterator,
double(),
boost::spirit::qi::space_type>
ruleElipsis<double>()
{
	return  "[" >> boost::spirit::double_  >> "," >> "..." >> "]";
}

template<>
boost::spirit::qi::rule<std::string::iterator,
SizeType(),
boost::spirit::qi::space_type>
ruleElipsis<SizeType>()
{
	return "[" >> boost::spirit::int_ >> "," >> "..." >> "]";
}

void AinurState::assign(String k, String v)
{
	int x = storageIndexByName(k);
	if (x < 0)
		err(errLabel(ERR_PARSE_UNDECLARED, k));

	assert(static_cast<SizeType>(x) < values_.size());
	values_[x] = v;
}

template <typename T>
template <typename A, typename ContextType>
typename EnableIf<!TypesEqual<std::vector<std::vector<T> >, A>::True,void>::Type
AinurState::ActionMatrix<T>::operator()(A& attr,
                                        ContextType&,
                                        bool&) const
{
	err("Ainur: Not sure how to handle this\n");
}

template <typename T>
template <typename A, typename ContextType>
typename EnableIf<TypesEqual<std::vector<std::vector<T> >, A>::True,void>::Type
AinurState::ActionMatrix<T>::operator()(A& attr,
                                        ContextType&,
                                        bool&) const
{
	SizeType rows = attr.size();
	if (rows == 0) return;
	SizeType cols = attr[0].size();
	t_.resize(rows, cols);
	for (SizeType i = 0; i < rows; ++i) {
		if (attr[i].size() != cols)
			err("Ainur: Problem reading matrix\n");
		for (SizeType j = 0; j < cols; ++j)
			t_(i, j) = attr[i][j];
	}
}

template <typename T>
template <typename A, typename ContextType>
typename EnableIf<TypesEqual<A,T>::True,void>::Type
AinurState::Action<T>::operator()(A& attr,
                                  ContextType&,
                                  bool&) const
{
	if (name_ == "elipsis") {
		SizeType total = t_.size();
		if (total == 0)
			err("Elipsis cannot be used for this vector, because size is unknown\n");

		for (SizeType i = 0; i < total; ++i)
			t_[i] = attr;
		return;
	}

	t_.push_back(attr);
}

template <typename T>
template <typename A, typename ContextType>
typename EnableIf<!TypesEqual<A,T>::True, void>::Type
AinurState::Action<T>::operator()(A& attr,
                                  ContextType&,
                                  bool&) const
{
	t_ = attr;
}

template <typename A, typename ContextType>
typename EnableIf<TypesEqual<A, double>::True, void>::Type
AinurState::ActionCmplx::operator()(A& attr,
                                         ContextType&,
                                         bool&) const
{
	if (name_ == "rows2")
		t_.push_back(std::complex<double>(0.0, attr));
	else if (name_ == "rows3")
		t_.push_back(attr);
	else
		err("Error parsing complex number\n");
}

template <typename A, typename ContextType>
typename EnableIf<!TypesEqual<A, double>::True, void>::Type
AinurState::ActionCmplx::operator()(A& attr,
                                         ContextType&,
                                         bool&) const
{
	if (name_ == "rows1") {
		t_.push_back(std::complex<double>(
		                 boost::fusion::at_c<0>(attr),
		                 boost::fusion::at_c<1>(attr)));
	} else {
		err("Error parsing complex number\n");
	}
}

template<typename T>
void AinurState::convertInternal(Matrix<T>& t,
                                 String value,
                                 typename EnableIf<Loki::TypeTraits<T>::isArith,
                                 int>::Type) const
{
	namespace qi = boost::spirit::qi;
	typedef std::string::iterator IteratorType;
	typedef std::vector<T> LocalVectorType;
	typedef std::vector<LocalVectorType> LocalVectorVectorType;

	IteratorType it = value.begin();
	qi::rule<IteratorType, LocalVectorType(), qi::space_type> ruRows = ruleRows<T>();

	qi::rule<IteratorType, LocalVectorVectorType(), qi::space_type> full =
	        "[" >> -(ruRows  % ",") >> "]";

	ActionMatrix<T> actionMatrix("matrix", t);
	bool r = qi::phrase_parse(it,
	                          value.end(),
	                          full [actionMatrix],
	                          qi::space);

	//check if we have a match
	if (!r)
		err("matrix parsing failed\n");

	if (it != value.end())
		std::cerr << "matrix parsing: unmatched part exists\n";
}

template<typename T>
void AinurState::convertInternal(std::vector<T>& t,
                                 String value,
                                 typename EnableIf<Loki::TypeTraits<T>::isArith,
                                 int>::Type) const
{
	namespace qi = boost::spirit::qi;
	typedef std::string::iterator IteratorType;
	typedef std::vector<T> LocalVectorType;

	IteratorType it = value.begin();
	qi::rule<IteratorType, LocalVectorType(), qi::space_type> ruRows = ruleRows<T>();
	qi::rule<IteratorType, T(), qi::space_type> ruElipsis = ruleElipsis<T>();

	Action<T> actionRows("rows", t);
	Action<T> actionElipsis("elipsis", t);

	bool r = qi::phrase_parse(it,
	                          value.end(),
	                          ruRows [actionRows] | ruElipsis[actionElipsis],
	                          qi::space);

	//check if we have a match
	if (!r)
		err("vector parsing failed\n");

	if (it != value.end())
		std::cerr << "vector parsing: unmatched part exists\n";
}

template<typename T>
void AinurState::convertInternal(std::vector<std::complex<T> >& t,
                                 String value,
                                 typename EnableIf<Loki::TypeTraits<T>::isArith,
                                 int>::Type) const
{
	namespace qi = boost::spirit::qi;
	ActionCmplx actionRows1("rows1", t);
	ActionCmplx actionRows2("rows2", t);
	ActionCmplx actionRows3("rows3", t);
#define MY_COMPLEX (qi::double_ >> qi::double_ >>  "i") [actionRows1] | (qi::double_ >> "i") [actionRows2] | qi::double_ [actionRows3]

	typedef BOOST_TYPEOF(MY_COMPLEX) OneType;
	typedef std::string::iterator IteratorType;

	IteratorType it = value.begin();

	OneType cmplxN =  MY_COMPLEX;
#define MY_V_OF_COMPLEX "[" >> -(cmplxN  % ",") >> "]"
	typedef BOOST_TYPEOF(MY_V_OF_COMPLEX) ManyTypes;
	ManyTypes ruRows = MY_V_OF_COMPLEX;
	//	qi::rule<IteratorType, std::complex<T>(), qi::space_type> ruElipsis =
	//	        ruleElipsis<std::complex<T> >();

	//Action<std::complex<T> > actionElipsis("elipsis", t);

	bool r = qi::phrase_parse(it,
	                          value.end(),
	                          ruRows,// | ruElipsis[actionElipsis],
	                          qi::space);

	//check if we have a match
	if (!r)
		err("vector parsing failed\n");

	if (it != value.end())
		std::cerr << "vector parsing: unmatched part exists\n";
}

template void AinurState::convertInternal(Matrix<double>&,String, int) const;

template void AinurState::convertInternal(std::vector<double>&, String, int) const;
template void AinurState::convertInternal(std::vector<SizeType>&, String, int) const;

template void AinurState::convertInternal(std::vector<std::complex<double> >&,
String, int) const;
} // namespace PsimagLite


#define BOOST_TEST_MODULE pcraster discr voxel_stack
#include <boost/test/unit_test.hpp>
#include "dal_MathUtils.h"
#include "discr_voxelstack.h"

BOOST_AUTO_TEST_CASE(constructor)
{
  using namespace discr;

  {
    VoxelStack stack;
    BOOST_CHECK(!stack.isMV());
    BOOST_CHECK(stack.baseElevation() == VoxelStack::value_type(0.0));
    BOOST_CHECK(stack.empty());
    BOOST_CHECK(stack.size() == 0);
  }

  {
    VoxelStack stack(5.0);
    BOOST_CHECK(!stack.isMV());
    BOOST_CHECK(stack.baseElevation() == VoxelStack::value_type(5.0));
    BOOST_CHECK(stack.empty());
    BOOST_CHECK(stack.size() == 0);
  }

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.1F);
    thicknesses.push_back(2.2F);
    thicknesses.push_back(0.0F);
    thicknesses.push_back(3.3F);
    VoxelStack stack(thicknesses.begin(), thicknesses.end());
    BOOST_CHECK(!stack.isMV());
    BOOST_CHECK(stack.baseElevation() == VoxelStack::value_type(0.0));
    BOOST_CHECK(stack.size() == thicknesses.size());
    BOOST_CHECK(dal::comparable(stack[0], VoxelStack::value_type(1.1)));
    BOOST_CHECK(dal::comparable(stack[1], VoxelStack::value_type(2.2)));
    BOOST_CHECK(dal::comparable(stack[3], VoxelStack::value_type(3.3)));
  }
}

BOOST_AUTO_TEST_CASE(set_mv)
{
  using namespace discr;

  {
    VoxelStack stack;
    BOOST_CHECK(!stack.isMV());
    stack.setMV();
    BOOST_CHECK(stack.isMV());
  }
}

BOOST_AUTO_TEST_CASE(set_base_elevation)
{
  using namespace discr;

  {
    VoxelStack stack;
    stack.setBaseElevation(3.0);
    BOOST_CHECK(stack.baseElevation() == VoxelStack::value_type(3.0));
  }
}

BOOST_AUTO_TEST_CASE(thickness)
{
  using namespace discr;

  {
    VoxelStack stack;
    BOOST_CHECK(stack.thickness() == VoxelStack::value_type(0.0));

    stack.push_back(1.0);
    stack.push_back(2.0);
    BOOST_CHECK(stack.thickness() == VoxelStack::value_type(3.0));
  }
}

BOOST_AUTO_TEST_CASE(is_regular)
{
  using namespace discr;

  {
    VoxelStack stack;
    BOOST_CHECK(stack.isRegular());
  }

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.3F);
    VoxelStack stack(thicknesses.begin(), thicknesses.end());
    BOOST_CHECK(!stack.isRegular());
    stack[2] = 1.2F;
    BOOST_CHECK(stack.isRegular());
  }

}

BOOST_AUTO_TEST_CASE(surface_elevation)
{
  using namespace discr;

  {
    VoxelStack stack;
    BOOST_CHECK(dal::comparable(stack.surfaceElevation(),
         VoxelStack::value_type(0.0)));
  }

  {
    VoxelStack stack(5.0);
    BOOST_CHECK(dal::comparable(stack.surfaceElevation(),
         VoxelStack::value_type(5.0)));
  }

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.3F);
    VoxelStack stack(thicknesses.begin(), thicknesses.end());
    BOOST_CHECK(dal::comparable(stack.surfaceElevation(),
         VoxelStack::value_type(3.7)));
  }
}

BOOST_AUTO_TEST_CASE(bottom_elevation)
{
  using namespace discr;

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.3F);
    thicknesses.push_back(1.4F);
    VoxelStack stack(thicknesses.begin(), thicknesses.end());
    BOOST_CHECK(dal::comparable(stack.bottomElevation(0),
         VoxelStack::value_type(0.0)));
    BOOST_CHECK(dal::comparable(stack.bottomElevation(1),
         VoxelStack::value_type(1.2)));
    BOOST_CHECK(dal::comparable(stack.bottomElevation(2),
         VoxelStack::value_type(2.5)));
  }
}

BOOST_AUTO_TEST_CASE(top_elevation)
{
  using namespace discr;

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.3F);
    thicknesses.push_back(1.4F);
    VoxelStack stack(thicknesses.begin(), thicknesses.end());
    BOOST_CHECK(dal::comparable(stack.topElevation(0),
         VoxelStack::value_type(1.2)));
    BOOST_CHECK(dal::comparable(stack.topElevation(1),
         VoxelStack::value_type(2.5)));
    BOOST_CHECK(dal::comparable(stack.topElevation(2),
         VoxelStack::value_type(3.9)));
  }
}

BOOST_AUTO_TEST_CASE(equals)
{
  using namespace discr;

  {
    VoxelStack stack1;
    VoxelStack stack2;
    BOOST_CHECK(stack1 == stack1);
    BOOST_CHECK(stack1 == stack2);
    BOOST_CHECK(stack2 == stack1);

    stack1.setBaseElevation(1.0);
    BOOST_CHECK(stack1 == stack1);
    BOOST_CHECK(stack1 != stack2);
    BOOST_CHECK(stack2 != stack1);
  }

  {
    std::vector<REAL4> thicknesses;
    thicknesses.push_back(1.2F);
    thicknesses.push_back(1.3F);
    thicknesses.push_back(1.4F);
    VoxelStack stack1(thicknesses.begin(), thicknesses.end());
    VoxelStack stack2(thicknesses.begin(), thicknesses.end());

    BOOST_CHECK(stack1 == stack1);
    BOOST_CHECK(stack1 == stack2);
    BOOST_CHECK(stack2 == stack1);

    stack1.setBaseElevation(5.0);
    BOOST_CHECK(stack1 == stack1);
    BOOST_CHECK(stack1 != stack2);
    BOOST_CHECK(stack2 != stack1);

    stack2.setBaseElevation(5.0);
    BOOST_CHECK(stack2 == stack2);
    BOOST_CHECK(stack1 == stack2);
    BOOST_CHECK(stack2 == stack1);

    stack1[1] = 3.1F;
    BOOST_CHECK(stack1 == stack1);
    BOOST_CHECK(stack1 != stack2);
    BOOST_CHECK(stack2 != stack1);

    stack2[1] = 3.1F;
    BOOST_CHECK(stack2 == stack2);
    BOOST_CHECK(stack1 == stack2);
    BOOST_CHECK(stack2 == stack1);
  }
}


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MATH_FLOAT_BACKEND_HPP
#define BOOST_MATH_FLOAT_BACKEND_HPP

#include <iostream>
#include <iomanip>
#include <sstream>
#include <cstdint>
#include <boost/lexical_cast.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/multiprecision/number.hpp>
#include <boost/integer/common_factor_rt.hpp>
#include <boost/container_hash/hash.hpp>

namespace boost {
namespace multiprecision {
namespace backends {

#ifdef BOOST_MSVC
#pragma warning(push)
#pragma warning(disable : 4389 4244 4018 4244 4127)
#endif

template <class Arithmetic>
struct arithmetic_backend
{
   typedef std::tuple<short, int, long, long long>                                 signed_types;
   typedef std::tuple<unsigned short, unsigned, unsigned long, unsigned long long> unsigned_types;
   typedef std::tuple<float, double, long double>                                  float_types;
   typedef int                                                                    exponent_type;

   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend() : m_value(0) {}
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend& o) : m_value(o.m_value) {}
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const A& o, const typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value >::type* = 0) : m_value(o) {}
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend(const arithmetic_backend<A>& o) : m_value(o.data()) {}
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend& o)
   {
      m_value = o.m_value;
      return *this;
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value, arithmetic_backend&>::type operator=(A i)
   {
      m_value = static_cast<Arithmetic>(i);
      return *this;
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR arithmetic_backend& operator=(const arithmetic_backend<A>& i)
   {
      m_value = i.data();
      return *this;
   }
   arithmetic_backend& operator=(const char* s)
   {
#ifndef BOOST_NO_EXCEPTIONS
      try
      {
#endif
         m_value = boost::lexical_cast<Arithmetic>(s);
#ifndef BOOST_NO_EXCEPTIONS
      }
      catch (const bad_lexical_cast&)
      {
         throw std::runtime_error(std::string("Unable to interpret the string provided: \"") + s + std::string("\" as a compatible number type."));
      }
#endif
      return *this;
   }
   BOOST_MP_CXX14_CONSTEXPR void swap(arithmetic_backend& o)
   {
      std::swap(m_value, o.m_value);
   }
   std::string str(std::streamsize digits, std::ios_base::fmtflags f) const
   {
      std::stringstream ss;
      ss.flags(f);
      ss << std::setprecision(digits ? digits : std::numeric_limits<Arithmetic>::digits10 + 4) << m_value;
      return ss.str();
   }
   BOOST_MP_CXX14_CONSTEXPR void do_negate(const std::integral_constant<bool, true>&)
   {
      m_value = 1 + ~m_value;
   }
   BOOST_MP_CXX14_CONSTEXPR void do_negate(const std::integral_constant<bool, false>&)
   {
      m_value = -m_value;
   }
   BOOST_MP_CXX14_CONSTEXPR void negate()
   {
      do_negate(std::integral_constant<bool, boost::multiprecision::detail::is_unsigned<Arithmetic>::value>());
   }
   BOOST_MP_CXX14_CONSTEXPR int compare(const arithmetic_backend& o) const
   {
      return m_value > o.m_value ? 1 : (m_value < o.m_value ? -1 : 0);
   }
   template <class A>
   BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A>::value, int>::type compare(A i) const
   {
      return m_value > static_cast<Arithmetic>(i) ? 1 : (m_value < static_cast<Arithmetic>(i) ? -1 : 0);
   }
   BOOST_MP_CXX14_CONSTEXPR Arithmetic& data() { return m_value; }
   BOOST_MP_CXX14_CONSTEXPR const Arithmetic& data() const { return m_value; }

 private:
   Arithmetic m_value;
};

template <class R, class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_integral<R>::value>::type eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend)
{
   typedef typename std::common_type<R, Arithmetic>::type c_type;
   constexpr const c_type                             max = static_cast<c_type>((std::numeric_limits<R>::max)());
   constexpr const c_type                             min = static_cast<c_type>((std::numeric_limits<R>::min)());
   c_type                                                   ct  = static_cast<c_type>(backend.data());
   if ((backend.data() < 0) && !std::numeric_limits<R>::is_signed)
      BOOST_THROW_EXCEPTION(std::range_error("Attempt to convert negative number to unsigned type."));
   if (ct > max)
      *result = boost::multiprecision::detail::is_signed<R>::value ? (std::numeric_limits<R>::max)() : backend.data();
   else if (std::numeric_limits<Arithmetic>::is_signed && (ct < min))
      *result = (std::numeric_limits<R>::min)();
   else
      *result = backend.data();
}

template <class R, class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!boost::multiprecision::detail::is_integral<R>::value && !std::is_enum<R>::value>::type eval_convert_to(R* result, const arithmetic_backend<Arithmetic>& backend)
{
   *result = backend.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_eq(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() == b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_eq(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() == static_cast<Arithmetic>(b);
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_lt(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() < b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_lt(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() < static_cast<Arithmetic>(b);
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_gt(const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   return a.data() > b.data();
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value, bool>::type eval_gt(const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   return a.data() > static_cast<Arithmetic>(b);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() += o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() -= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() *= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() /= o.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   if (!o.data())
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() /= o.data();
}

template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_add(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() += o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_subtract(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() -= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_multiply(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() *= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && !std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   if (!o)
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() /= o;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const A2& o)
{
   result.data() /= o;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() + b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() - b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() * b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = a.data() / b.data();
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<!std::numeric_limits<Arithmetic>::has_infinity>::type eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   if (!b.data())
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() = a.data() / b.data();
}

template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_add(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() + b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_subtract(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() - b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_arithmetic<A2>::value>::type eval_multiply(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() * b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && !std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   if (!b)
      BOOST_THROW_EXCEPTION(std::overflow_error("Divide by zero"));
   result.data() = a.data() / b;
}
template <class Arithmetic, class A2>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<(boost::multiprecision::detail::is_arithmetic<A2>::value && std::numeric_limits<Arithmetic>::has_infinity)>::type
eval_divide(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const A2& b)
{
   result.data() = a.data() / b;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR bool eval_is_zero(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<
    (!std::numeric_limits<Arithmetic>::is_specialized || std::numeric_limits<Arithmetic>::is_signed), int>::type
eval_get_sign(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0 ? 0 : val.data() < 0 ? -1 : 1;
}
template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<
    !(std::numeric_limits<Arithmetic>::is_specialized || std::numeric_limits<Arithmetic>::is_signed), int>::type
eval_get_sign(const arithmetic_backend<Arithmetic>& val)
{
   return val.data() == 0 ? 0 : 1;
}

template <class T>
inline BOOST_MP_CXX14_CONSTEXPR typename std::enable_if<boost::multiprecision::detail::is_unsigned<T>::value, T>::type abs(T v) { return v; }

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_abs(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   using boost::multiprecision::backends::abs;
   using std::abs;
   result.data() = abs(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_fabs(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   result.data() = std::abs(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_floor(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = floor(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_ceil(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = ceil(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sqrt(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sqrt(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR int eval_fpclassify(const arithmetic_backend<Arithmetic>& o)
{
   return (boost::math::fpclassify)(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_trunc(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = trunc(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_round(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = round(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_frexp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, int* v)
{
   BOOST_MATH_STD_USING
   result.data() = frexp(a.data(), v);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_ldexp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, int v)
{
   BOOST_MATH_STD_USING
   result.data() = ldexp(a.data(), v);
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_exp(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = exp(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_log(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = log(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_log10(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = log10(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sin(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sin(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_cos(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = cos(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_tan(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = tan(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_acos(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = acos(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_asin(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = asin(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_atan(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = atan(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_sinh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = sinh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_cosh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = cosh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_tanh(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& o)
{
   BOOST_MATH_STD_USING
   result.data() = tanh(o.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_fmod(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = fmod(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_pow(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = pow(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_atan2(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   BOOST_MATH_STD_USING
   result.data() = atan2(a.data(), b.data());
}

template <class Arithmetic, class I>
inline BOOST_MP_CXX14_CONSTEXPR void eval_left_shift(arithmetic_backend<Arithmetic>& result, I val)
{
   result.data() <<= val;
}

template <class Arithmetic, class I>
inline BOOST_MP_CXX14_CONSTEXPR void eval_right_shift(arithmetic_backend<Arithmetic>& result, I val)
{
   result.data() >>= val;
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_modulus(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() %= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_and(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() &= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_or(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() |= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_bitwise_xor(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() ^= a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_complement(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a)
{
   result.data() = ~a.data();
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_gcd(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = boost::integer::gcd(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR void eval_lcm(arithmetic_backend<Arithmetic>& result, const arithmetic_backend<Arithmetic>& a, const arithmetic_backend<Arithmetic>& b)
{
   result.data() = boost::integer::lcm(a.data(), b.data());
}

template <class Arithmetic>
inline BOOST_MP_CXX14_CONSTEXPR std::size_t hash_value(const arithmetic_backend<Arithmetic>& a)
{
   boost::hash<Arithmetic> hasher;
   return hasher(a.data());
}

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

} // namespace backends

using boost::multiprecision::backends::arithmetic_backend;

template <class Arithmetic>
struct number_category<arithmetic_backend<Arithmetic> > : public std::integral_constant<int, boost::multiprecision::detail::is_integral<Arithmetic>::value ? number_kind_integer : number_kind_floating_point>
{};

namespace detail {

template <class Backend>
struct double_precision_type;

template <class Arithmetic, boost::multiprecision::expression_template_option ET>
struct double_precision_type<number<arithmetic_backend<Arithmetic>, ET> >
{
   typedef number<arithmetic_backend<typename double_precision_type<Arithmetic>::type>, ET> type;
};
template <>
struct double_precision_type<arithmetic_backend<std::int32_t> >
{
   typedef arithmetic_backend<std::int64_t> type;
};

} // namespace detail

}} // namespace boost::multiprecision
#if !(defined(__SGI_STL_PORT) || defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS))
//
// We shouldn't need these to get code to compile, however for the sake of
// "level playing field" performance comparisons they avoid the very slow
// lexical_cast's that would otherwise take place.  Definition has to be guarded
// by the inverse of pp-logic in real_concept.hpp which defines these as a workaround
// for STLPort plus some other old/broken standartd libraries.
//
namespace boost { namespace math { namespace tools {

template <>
inline unsigned int real_cast<unsigned int, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<unsigned int>(r.value());
}

template <>
inline int real_cast<int, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<int>(r.value());
}

template <>
inline long real_cast<long, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<long>(r.value());
}

// Converts from T to narrower floating-point types, float, double & long double.

template <>
inline float real_cast<float, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<float>(r.value());
}
template <>
inline double real_cast<double, concepts::real_concept>(concepts::real_concept r)
{
   return static_cast<double>(r.value());
}
template <>
inline long double real_cast<long double, concepts::real_concept>(concepts::real_concept r)
{
   return r.value();
}

}}} // namespace boost::math::tools
#endif

namespace std {

template <class Arithmetic, boost::multiprecision::expression_template_option ExpressionTemplates>
class numeric_limits<boost::multiprecision::number<boost::multiprecision::arithmetic_backend<Arithmetic>, ExpressionTemplates> > : public std::numeric_limits<Arithmetic>
{
   typedef std::numeric_limits<Arithmetic>                                                                           base_type;
   typedef boost::multiprecision::number<boost::multiprecision::arithmetic_backend<Arithmetic>, ExpressionTemplates> number_type;

 public:
   static constexpr number_type(min)() noexcept { return (base_type::min)(); }
   static constexpr number_type(max)() noexcept { return (base_type::max)(); }
   static constexpr number_type lowest() noexcept { return -(max)(); }
   static constexpr number_type epsilon() noexcept { return base_type::epsilon(); }
   static constexpr number_type round_error() noexcept { return epsilon() / 2; }
   static constexpr number_type infinity() noexcept { return base_type::infinity(); }
   static constexpr number_type quiet_NaN() noexcept { return base_type::quiet_NaN(); }
   static constexpr number_type signaling_NaN() noexcept { return base_type::signaling_NaN(); }
   static constexpr number_type denorm_min() noexcept { return base_type::denorm_min(); }
};

template <>
class numeric_limits<boost::math::concepts::real_concept> : public std::numeric_limits<long double>
{
   typedef std::numeric_limits<long double>    base_type;
   typedef boost::math::concepts::real_concept number_type;

 public:
   static const number_type(min)() noexcept { return (base_type::min)(); }
   static const number_type(max)() noexcept { return (base_type::max)(); }
   static const number_type lowest() noexcept { return -(max)(); }
   static const number_type epsilon() noexcept { return base_type::epsilon(); }
   static const number_type round_error() noexcept { return epsilon() / 2; }
   static const number_type infinity() noexcept { return base_type::infinity(); }
   static const number_type quiet_NaN() noexcept { return base_type::quiet_NaN(); }
   static const number_type signaling_NaN() noexcept { return base_type::signaling_NaN(); }
   static const number_type denorm_min() noexcept { return base_type::denorm_min(); }
};

} // namespace std

#include <boost/multiprecision/detail/integer_ops.hpp>

#endif


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                      (C) 2006-2010 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include <sstream>
#include <boost/python.hpp>
#include "mccomponents/kernels/sample/diffraction/SimplePowderDiffractionData.h"
#include "mccomponents/kernels/sample/diffraction/SimplePowderDiffractionKernel.h"
#include "mccomponents/boostpython_binding/wrap_kernel.h"
#include "wrap_vector.h"

namespace wrap_mccomponents {

  void wrap_SimplePowderDiffractionKernel()
  {
    using namespace boost::python;
    using namespace mccomponents::boostpython_binding;

    typedef mccomponents::kernels::SimplePowderDiffractionKernel w_t;
    typedef mccomponents::kernels::SimplePowderDiffractionData Data;

    kernel_wrapper<w_t>::wrap
      ("SimplePowderDiffractionKernel",
       init<const Data &> ()
       )
      ;

    class_<Data>
      ("SimplePowderDiffractionData",
       init<>()
       )
      .def_readwrite("Dd_over_d", &Data::Dd_over_d)
      .def_readwrite("DebyeWaller_factor", &Data::DebyeWaller_factor)
      .def_readwrite("unitcell_volume", &Data::unitcell_volume)
      .def_readwrite("density", &Data::density)
      .def_readwrite("atomic_weight", &Data::atomic_weight)
      .def_readwrite("number_of_atoms", &Data::number_of_atoms)
      .def_readwrite("absorption_cross_section", &Data::absorption_cross_section)
      .def_readwrite("coherent_cross_section", &Data::coherent_cross_section)
      .def_readwrite("incoherent_cross_section", &Data::incoherent_cross_section)
      .def_readwrite("peaks", &Data::peaks)
      ;

    class_<Data::Peak>
      ("SimplePowderDiffractionData_Peak",
       init<> ()
       )
      .def_readwrite("q", &Data::Peak::q)
      .def_readwrite("F_squared", &Data::Peak::F_squared)
      .def_readwrite("multiplicity", &Data::Peak::multiplicity)
      .def_readwrite("intrinsic_line_width", &Data::Peak::intrinsic_line_width)
      .def_readwrite("DebyeWaller_factor", &Data::Peak::DebyeWaller_factor)
      ;

    wrap_vector<Data::Peak>("SimplePowderDiffractionData_Peak");

  }

}

// version
// $Id$

// End of file


#include <cradle/encoding.hpp>
#include <boost/scoped_array.hpp>
#include <cstdlib>
#include <boost/algorithm/string.hpp>
#include <set>

#define BOOST_TEST_MODULE base64
#include <cradle/test.hpp>

using namespace cradle;

static void check_mime_encoding(char const* original, char const* encoded)
{
    using namespace cradle;
    BOOST_CHECK_EQUAL(
        base64_encode(reinterpret_cast<cradle::uint8_t const*>(original),
            strlen(original), mime_base64_character_set),
        encoded);
}

static void check_base64_round_trip(std::vector<cradle::uint8_t> const& src,
    base64_character_set const& character_set)
{
    string encoded = base64_encode(&src[0], src.size(), character_set);
    std::vector<cradle::uint8_t> decoded(
        get_base64_decoded_length(encoded.length()));
    size_t decoded_length;
    base64_decode(&decoded[0], &decoded_length, encoded.c_str(),
        encoded.length(), character_set);
    decoded.resize(decoded_length);
    CRADLE_CHECK_RANGES_EQUAL(src, decoded);
}

static void test_random_base64_encoding(
    base64_character_set const& character_set)
{
    for (int i = 0; i != 100; ++i)
    {
        size_t length = rand() & 0xfff;
        std::vector<cradle::uint8_t> data(length);
        for (size_t j = 0; j != length; ++j)
            data[j] = cradle::uint8_t(rand() & 0xff);
        check_base64_round_trip(data, character_set);
    }
}

BOOST_AUTO_TEST_CASE(base64_encoding_test)
{
    check_mime_encoding(
        "Man is distinguished, not only by his reason, but by this singular passion from other animals, which is a lust of the mind, that by a perseverance of delight in the continued and indefatigable generation of knowledge, exceeds the short vehemence of any carnal pleasure.",
        "TWFuIGlzIGRpc3Rpbmd1aXNoZWQsIG5vdCBvbmx5IGJ5IGhpcyByZWFzb24sIGJ1dCBieSB0aGlzIHNpbmd1bGFyIHBhc3Npb24gZnJvbSBvdGhlciBhbmltYWxzLCB3aGljaCBpcyBhIGx1c3Qgb2YgdGhlIG1pbmQsIHRoYXQgYnkgYSBwZXJzZXZlcmFuY2Ugb2YgZGVsaWdodCBpbiB0aGUgY29udGludWVkIGFuZCBpbmRlZmF0aWdhYmxlIGdlbmVyYXRpb24gb2Yga25vd2xlZGdlLCBleGNlZWRzIHRoZSBzaG9ydCB2ZWhlbWVuY2Ugb2YgYW55IGNhcm5hbCBwbGVhc3VyZS4=");

    check_mime_encoding(
        "leasure.",
        "bGVhc3VyZS4=");

    check_mime_encoding(
        "easure.",
        "ZWFzdXJlLg==");

    check_mime_encoding(
        "asure.",
        "YXN1cmUu");

    check_mime_encoding(
        "sure.",
        "c3VyZS4=");

    check_mime_encoding(
        "sure",
        "c3VyZQ==");

    test_random_base64_encoding(url_friendly_base64_character_set);
    test_random_base64_encoding(mime_base64_character_set);
}

static void check_base36_round_trip(
    uint64_t id, unsigned minimum_length, string const& correct_encoding)
{
    string encoded = base36_encode(id, minimum_length);
    BOOST_CHECK_EQUAL(encoded, correct_encoding);
    uint64_t decoded = base36_decode(encoded);
    BOOST_CHECK_EQUAL(id, decoded);
    decoded = base36_decode(boost::to_upper_copy(encoded));
    BOOST_CHECK_EQUAL(id, decoded);
}

static void check_invalid_base36(string const& text)
{
    BOOST_CHECK_THROW(base36_decode(text), exception);
}

BOOST_AUTO_TEST_CASE(base36_encoding_test)
{
    check_base36_round_trip(   0, 1,   "0");
    check_base36_round_trip(   0, 2,  "00");
    check_base36_round_trip(   1, 2,  "01");
    check_base36_round_trip(  10, 1,   "a");
    check_base36_round_trip(  35, 1,   "z");
    check_base36_round_trip(  36, 2,  "10");
    check_base36_round_trip(  36, 1,  "10");
    check_base36_round_trip(  71, 1,  "1z");
    check_base36_round_trip(1000, 1,  "rs");
    check_base36_round_trip(2000, 1, "1jk");
    check_invalid_base36("");
    check_invalid_base36("-");
    check_invalid_base36("1-");
    check_invalid_base36("/");
    check_invalid_base36(".");
    check_invalid_base36("0.");
    check_invalid_base36("a111111111111");
    check_invalid_base36("11111111111111");
}

static void check_nonsequential_base36_round_trip(
    uint64_t id, unsigned minimum_length, string const& correct_encoding)
{
    string encoded = nonsequential_base36_encode(id, minimum_length);
    BOOST_CHECK_EQUAL(encoded, correct_encoding);
    uint64_t decoded = nonsequential_base36_decode(encoded);
    BOOST_CHECK_EQUAL(id, decoded);
    decoded = nonsequential_base36_decode(boost::to_upper_copy(encoded));
    BOOST_CHECK_EQUAL(id, decoded);
}

static void check_nonsequential_base36_series(
    uint64_t n_ids, unsigned minimum_length)
{
    std::set<string> ids;
    for (uint64_t i = 0; i != n_ids; ++i)
    {
        string encoded = nonsequential_base36_encode(i, minimum_length);
        uint64_t decoded = nonsequential_base36_decode(encoded);
        BOOST_CHECK_EQUAL(i, decoded);
        ids.insert(encoded);
    }
    BOOST_CHECK_EQUAL(ids.size(), n_ids);
}

BOOST_AUTO_TEST_CASE(nonsequential_base36_encoding_test)
{
    // Check several IDs to make sure they indeed appear nonsequential.
    check_nonsequential_base36_round_trip(   0, 4, "0c4k");
    check_nonsequential_base36_round_trip(   1, 4, "4kqx");
    check_nonsequential_base36_round_trip(   2, 4, "8sda");
    check_nonsequential_base36_round_trip(   3, 4, "d0zn");
    check_nonsequential_base36_round_trip(   4, 4, "h8m0");
    check_nonsequential_base36_round_trip(   5, 4, "lg8d");
    check_nonsequential_base36_round_trip(   6, 4, "pouq");
    check_nonsequential_base36_round_trip(   7, 4, "twh3");
    check_nonsequential_base36_round_trip(   8, 4, "y43g");
    check_nonsequential_base36_round_trip(   9, 4, "2cpt");
    // Try a different area.
    check_nonsequential_base36_round_trip(1000, 3, "6qk");
    check_nonsequential_base36_round_trip(1001, 3, "w0x");
    check_nonsequential_base36_round_trip(1002, 3, "lha");
    check_nonsequential_base36_round_trip(1003, 3, "arn");
    check_nonsequential_base36_round_trip(1004, 3, "020");
    check_nonsequential_base36_round_trip(1005, 3, "pcd");

    // Check the first 200k IDs (with different minimum lengths for the IDs.
    // Ensure that all encodings decode back to the original ID and that no two
    // IDs map to the same encoding.
    check_nonsequential_base36_series(200000, 1);
    check_nonsequential_base36_series(200000, 3);
    check_nonsequential_base36_series(200000, 6);
}


#include <ThermalAnalysis/LinearCombination.hpp>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>
#include <yaml-cpp/yaml.h>
#define UNITCONVERT_NO_BACKWARD_COMPATIBLE_NAMESPACE
#include <UnitConvert.hpp>
#include <UnitConvert/GlobalUnitRegistry.hpp>
#include <BoostUnitDefinitions/Units.hpp>
#include <libField/HDF5.hpp>
#include <iostream>
#include <fstream>
#include <sstream>
#include <exception>
#include <gputils/io.hpp>

namespace po = boost::program_options;
using namespace boost::units;
/*
string operator>>(string &input, string name){
  string lineText;
  ifstream readFile(input);
  while(getline (readFile, lineText)){
    std::cout << lineText;
  }
  lineText << input;
  int b = 0;
  return lineText;
}
*/

struct ItemNotFound
{
  std::string item;
};

template<typename R, typename N, typename I>
auto get_(const N& node, I b, I e, boost::optional<R> def = boost::none)
{
  auto elem = *b;
  b++;
  if(node[elem])
  {
    if( b == e )
      return node[elem].template as<R>();

    return get_(node[elem],b,e,def);
  }

  if(def)
  {
    return def.value();
  }

  throw ItemNotFound{elem};

  return def.value();

}
template<typename R, typename N>
auto get(const N& node, std::string key, boost::optional<R> def = boost::none)
{
  std::vector<std::string> key_elements;
  boost::split(key_elements, key, boost::is_any_of("/"));
  try{
  return get_<R>(node, key_elements.begin(), key_elements.end(), def);
  }
  catch( ItemNotFound e)
  {
    std::stringstream out;
    out << "Could not find element '"<< e.item <<"' when looking for key '" << key <<"', which is required. Exiting now.";
    std::cout << out.str() << std::endl;
    exit(1);
  }
  return R();
}

int main(int argc, const char** argv)
{
  // quick shortcut to create an input file
  /*
  Field<double, 1> delete_later(100);
  delete_later.setCoordinateSystem(Geometric<double>(0, 0.001, 1.05));
  delete_later.set_f([](auto t){return sqrt(t[0]);});
  {
    ofstream output_stream;
    output_stream.open("Tvst.txt");
    output_stream << delete_later;
    output_stream.close();
  }
  hdf5write("Tvst.h5", delete_later);
  */

  // define command line options
  po::options_description po_opts("Options");
  po_opts.add_options()("help,h", "print help message.")(
      "verbose,v", po::value<int>()->implicit_value(0), "verbose level.")(
      "write-example-config,w", po::value<std::string>(), "Write an example configuration file and exit.");

  // now define our arguments.
  po::options_description po_args("Arguments");
  po_args.add_options()
    ("config-files"  , po::value<std::vector<std::string>>()->composing(), "Configuration files to process.")
    ;

  // combine the options and arguments into one option list.
  // this is what we will use to parse the command line, but
  // when we output a description of the options, we will just use
  // po_opts
  po::options_description all_options("Options and Arguments");
  all_options.add(po_opts).add(po_args);

  // tell boost how to translate positional options to named options
  po::positional_options_description args;
  args.add("config-files", -1);

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

  if (argc == 1 || vm.count("help")) {
    std::cout << R"EOL(A command line application for adding thermal profiles (Time-temperature history) to build complex profiles.
tempBuilder can be used to construct thermal profiles from complex temporal laser exposures from profiles generated from simple exposures. For
example, the thermal profile for a multiple-pulse exposure can be generated from the profile for a single-pulse exposure.
)EOL";
    std::cout << po_opts << std::endl;
    return 0;
  }

  if( vm.count("write-example-config") )
  {
    std::string text = R"EOL(
combinations:
  - input:
      file_name: Tvst.h5
      format: h5
    output:
      file_name: Tvst-1.txt
    discretization:
      dt: 1 ms
      t_min: 0 s
      t_max: 2 s
    terms:
      - time: 0.1 s
        scale: 1
      - time: 0.2 s
        scale: 3
      - time: 0.5 s
        scale: 0.5
)EOL";

    if( vm["write-example-config"].as<std::string>() == "-" )
    {
      std::cout << text;
    }
    else{
      std::ofstream out(vm["write-example-config"].as<std::string>().c_str());
      out << text;
    }

    return 0;

  }

  auto &ureg = UnitConvert::getGlobalUnitRegistry();

  for( auto file : vm["config-files"].as<std::vector<std::string>>() )
  {

//    ReadFile >> a;

    if( boost::filesystem::exists(file) )
    {

      YAML::Node config = YAML::LoadFile(file);

      if(!config["combinations"])
      {
        std::cout << "WARNING: No \"combinations\" section found in configuration file. Nothing will be done." << std::endl;
      }

      for( auto combination : config["combinations"] )
      {
        auto input = get<std::string>( combination, "input/file_name" );
        auto input_format = get<std::string>(combination, "input/format" );
        auto output = get<std::string>(combination, "output/file_name");
        auto output_format = get<std::string>(combination, "output/format");
        auto disc_type = get<std::string>(combination, "output/discretization/type");

        // discretization initializations
        //needs organization
        //as of rn all discretizers have a dt and t_min
        quantity<t::s> dt = ureg.makeQuantity<double>( get<std::string>(combination, "output/discretization/dt") ).to<t::s>();
        quantity<t::s> t_min = ureg.makeQuantity<double>( get<std::string>(combination, "output/discretization/t_min") ).to<t::s>();
        quantity<t::s> t_max = ureg.makeQuantity<double>( get<std::string>(combination, "output/discretization/t_max") ).to<t::s>();

        Field<double, 1> T_output;

        if (disc_type == "uniform"){
          int N = (int)((t_max.value() - t_min.value()) / dt.value());
          T_output = Field<double, 1>(N);
          T_output.setCoordinateSystem( Uniform<double>(t_min.value(), t_max.value()) );
        } else if (disc_type == "geometric") {
          quantity<t::s> stretch = ureg.makeQuantity<double>( combination["output"]["discretization"]["stretch"].as<std::string>() ).to<t::s>();
          double s = stretch.value();
          int N = (int) (log(1.0 - (( (t_max.value() - t_min.value()) * (1.0 - s)) / dt.value())) / log(s));
        } else if (disc_type == "geometric periodic") {
          quantity<t::s> stretch = ureg.makeQuantity<double>( combination["output"]["discretization"]["stretch"].as<std::string>() ).to<t::s>();
          quantity<t::s> period = ureg.makeQuantity<double>( combination["output"]["discretization"]["period"].as<std::string>() ).to<t::s>();
          double s = stretch.value();
          double T = period.value();
          int N = (int) (((t_max.value() - t_min.value()) / T) * log(1.0 - (( (T - t_min.value()) * (1.0 - s)) / dt.value())) / log(s));
        } else {
          std::cout << "WARNING: No recognized type found in discretization. Nothing will be done." << std::endl;
          continue;
        }

        LinearCombination<_1D::MonotonicInterpolator<double>> tempBuilder;

        Field<double, 1> T_i;

        // reading Field from file
        std::cout << "Reading from " << input << "...";
        if("txt" == input_format ){
          GP2DData data;
          ReadGPASCII2DDataFile(input, data);

          T_i.reset(data.x.size());
          for(int i = 0; i < T_i.size(); ++i)
          {
            T_i.getAxis(0)[i] = data.x[i];
            T_i(i) = data.f[i];
          }

        } else if(input_format == std::string("h5")){
          hdf5read(input, T_i);
        } else {
          std::cout << "WARNING: No known input file format found in output. Nothing will be done." << std::endl;
          continue;
        }
        std::cout << "done" << std::endl;

        auto T0 = T_i[0];
        T_i -= T0;

        if(!combination["terms"])
        {
          std::cout << "WARNING: No \"terms \" section found in combination. Nothing will be done." << std::endl;
        }
        for( auto term : combination["terms"] )
        {
          // use UnitConvert library to allow users to specify quantities in any unit they want
          quantity<t::s> start_time = ureg.makeQuantity<double>( term["time"].as<std::string>() ).to<t::s>();
          auto scale = term["scale"].as<double>();
          tempBuilder.add(T_i, start_time.value(), scale);
        }

        tempBuilder.build(T_output);
        T_output += T0;

        // Writing generated Field data to file
        std::cout << "Writing to "<<output<<"...";
        if(output_format == std::string("txt")){
          ofstream output_stream;
          output_stream.open(output);
          output_stream << std::setprecision(10);
          output_stream << T_output;
          output_stream.close();
        } else if(output_format == std::string("h5")){
          hdf5write(output, T_i);
        } else {
          std::cout << "WARNING: No known output file format found in output. Nothing will be done." << std::endl;
          continue;
        }
        std::cout << "done" << std::endl;

      }

    }
    else{
      std::cerr << file << " does not appear to exists. Skipping" << std::endl;
    }

  }

  return 0;
}


// Boost.Geometry

// Copyright (c) 2019, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#include "common.hpp"

#include <boost/geometry/algorithms/is_simple.hpp>
#include <boost/geometry/algorithms/is_valid.hpp>

template <typename G, typename S>
inline void is(G const& g, S const& s)
{
    bg::is_simple(g, s);
    bg::is_valid(g, s);
}

template <typename G>
inline void is_x(G const& g)
{
    ::is(g, bg::strategy::intersection::cartesian_segments<>());
    ::is(g, bg::strategy::intersection::spherical_segments<>());
    ::is(g, bg::strategy::intersection::geographic_segments<>());
}

int test_main(int, char*[])
{
    geom g;

    ::is_x(g.pt);
    ::is_x(g.mpt);
    ::is_x(g.s);
    ::is_x(g.ls);
    ::is_x(g.mls);
    ::is_x(g.r);
    ::is_x(g.po);
    ::is_x(g.mpo);

    return 0;
}


#ifndef _maps_ViewBase_hpp_
#define _maps_ViewBase_hpp_

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

namespace maps {

class ViewBase {
public:
  enum Type {
    TypePointCloud,
    TypeOctree,
    TypeDepthImage,
    TypeVoxelGrid,
    TypeScanBundle,
    TypeMesh,
    TypeSurfels,
  };

  enum TimeMode {
    TimeModeAbsolute,
    TimeModeRelative,
    TimeModeRollAngleAbsolute,
    TimeModeRollAngleRelative,
  };

  enum AccumulationMethod {
    AccumulationMethodClosest,
    AccumulationMethodFurthest,
    AccumulationMethodMean,
    AccumulationMethodMedian,
    AccumulationMethodClosestPercentile,
    AccumulationMethodRobustBlend,
  };

  struct Spec {
    int64_t mMapId;
    int64_t mViewId;
    bool mActive;
    TimeMode mTimeMode;
    AccumulationMethod mAccumulationMethod;
    bool mRelativeLocation;
    Type mType;
    float mResolution;
    float mFrequency;
    float mQuantizationMax;
    std::string mChannel;
    int64_t mTimeMin;
    int64_t mTimeMax;
    int mWidth;
    int mHeight;
    std::vector<Eigen::Vector4f> mClipPlanes;
    Eigen::Projective3f mTransform;  // reference to view

    Spec();
    bool operator==(const Spec& iSpec) const;
    bool operator!=(const Spec& iSpec) const;
  };

  typedef std::shared_ptr<ViewBase> Ptr;

public:
  ViewBase();
  virtual ~ViewBase();

  virtual const Type getType() const = 0;

  void setId(const int64_t iId);
  const int64_t getId() const;

  void setTransform(const Eigen::Projective3f& iTransform);
  const Eigen::Projective3f getTransform() const;

  void setUpdateTime(const int64_t iTime);
  const int64_t getUpdateTime() const;

  virtual Ptr clone() const = 0;

  virtual bool getClosest(const Eigen::Vector3f& iPoint,
                          Eigen::Vector3f& oPoint,
                          Eigen::Vector3f& oNormal) const;

protected:
  int64_t mId;
  int64_t mUpdateTime;
  Eigen::Projective3f mTransform;  // from reference frame to this view
};

}

#endif


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_TRIGO_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_DETAIL_SIMD_TRIGO_HPP_INCLUDED

#include <boost/simd/arch/common/detail/scalar/trigo.hpp>
#include <boost/simd/arch/common/detail/simd/trig_base.hpp>
#include <boost/simd/arch/common/detail/simd/f_trig_evaluation.hpp>
#include <boost/simd/arch/common/detail/simd/d_trig_evaluation.hpp>

#endif


#include <byteswap.h>
#include <stdio.h>
#include <unistd.h>
#include <vector>
#include <Eigen/Dense>
#include "imu/imu.h"

using Eigen::Matrix4f;
using Eigen::Vector3f;
using Eigen::Vector4f;

namespace {
const uint8_t ADDR_ITG3200  = 0x68;
const uint8_t ADDR_HMC5883L = 0x1e;
const uint8_t ADDR_ADXL345  = 0x53;

const Vector3f gyrocal_b(-182.11, -179.32, -212.83);
const Vector3f gyrocal_m(-0.01169, -0.01123, -0.01164);
const float pointcloud_dist = 200.0f;

bool CheckPointCloud(const Vector3f& pt, std::vector<Vector3f> *pointcloud) {
  for (int i = 0; i < pointcloud->size(); i++) {
    if (((*pointcloud)[i] - pt).squaredNorm() <
        pointcloud_dist*pointcloud_dist) {
      return false;
    }
  }
  pointcloud->push_back(pt);
  return true;
}

}  // empty namespace

// our motion model should figure out whether the car is on the ground (1g down
// on the accelerometer) or flying off a ramp (0g); if it's on the ground and
// the motor is stopped (0 estimated velocity) then we can zero out the gyro,
// and calibrate the direction of the accelerometer Y direction.

bool IMU::Init() {
  mag_XTX_ = 0.01 * Matrix4f::Identity();
  mag_XTY_ = Vector4f::Zero();
  mag_bias_ = Vector3f::Zero();

  // config gyro
  i2c_.Write(ADDR_ITG3200, 0x3E, 0x01);  // use X gyro PLL oscillator
  i2c_.Write(ADDR_ITG3200, 0x15, 19);    // samplerate 50Hz (1000/(19+1))
  i2c_.Write(ADDR_ITG3200, 0x16, 0x18 + 4);  // enable, 20Hz bandwidth
  // config compass
  i2c_.Write(ADDR_HMC5883L, 0x00, 0x38);  // CRA: 75Hz rate w/ 2 averages
  i2c_.Write(ADDR_HMC5883L, 0x01, 0x20);  // CRB: set gain 1090 LSB/Gauss
  i2c_.Write(ADDR_HMC5883L, 0x02, 0x00);  // continuous measurement
  // config accelerometer
  i2c_.Write(ADDR_ADXL345, 0x2c, 0x09);  // 25Hz bw, 50Hz samplerate
  i2c_.Write(ADDR_ADXL345, 0x31, 0x08);  // FULL_RES
  i2c_.Write(ADDR_ADXL345, 0x38, 0x00);  // bypass FIFO, sample @ 50Hz
  i2c_.Write(ADDR_ADXL345, 0x2d, 0x08);  // turn on

  return true;
}

bool IMU::ReadRaw(IMURawState *s) {
  uint8_t axis_buf[8];
  if (!i2c_.Read(ADDR_ITG3200, 0x1b, axis_buf, 8))
    return false;
  // temperature is 280 LSB/deg C, -13200 LSB @35 C
  s->gyro_temp = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+0));
  s->gyro_x = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+2));  // roll
  s->gyro_y = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+4));  // pitch
  s->gyro_z = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+6));  // yaw

  if (!i2c_.Read(ADDR_HMC5883L, 0x03, axis_buf, 6))
    return false;
  s->mag_x = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+0));  // front?
  s->mag_z = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+2));  // up
  s->mag_y = bswap_16(*reinterpret_cast<uint16_t*>(axis_buf+4));  // side?

  if (!i2c_.Read(ADDR_ADXL345, 0x32, axis_buf, 6))
    return false;
  s->accel_x = (*reinterpret_cast<uint16_t*>(axis_buf+0));  // toward back
  s->accel_y = (*reinterpret_cast<uint16_t*>(axis_buf+2));  // toward right
  s->accel_z = (*reinterpret_cast<uint16_t*>(axis_buf+4));  // toward ground

  return true;
}

bool IMU::ReadCalibrated(IMUState *s) {
  IMURawState rawstate;
  if (!ReadRaw(&rawstate))
    return false;

  Calibrate(rawstate, s);
  return true;
}

// all devices are oriented such that X is to the front of the car, Y is to
// the left, and Z is up.
void IMU::Calibrate(const IMURawState &s, IMUState *state) {
  // gyro offset calibration (rough -- needs work)
  Vector3f w0(-s.gyro_y, -s.gyro_z, s.gyro_x);
  w0 -= gyrocal_b + s.gyro_temp * gyrocal_m;
  // scale to radians/second
  state->w = w0 * M_PI / (180.0 * 14.375);

  // magnetometer calibration via least-squares fit sphere
  // TODO: fix the axes (i think x and z are swapped and one is negated)
  Vector3f m(s.mag_x, s.mag_y, s.mag_z);
  // if there's room in the point cloud for this new point, update calibration
  if (CheckPointCloud(m, &mag_cal_points_)) {
    Vector4f b(s.mag_x, s.mag_y, s.mag_z, 1);
    mag_XTX_ += b * b.transpose();
    float bmag = b.squaredNorm();
    mag_XTY_ += bmag * b;
    Vector4f beta = mag_XTX_.ldlt().solve(mag_XTY_);
    mag_bias_ = beta.block<3, 1>(0, 0) / 2;
    bmag = sqrt(beta[3] + mag_bias_.squaredNorm());
    fprintf(stderr, "imu: updated mag calibration [%f %f %f] R=%f\n",
            beta[0] / 2, beta[1] / 2, beta[2] / 2, bmag);
  }
  state->N = m - mag_bias_;

  // TODO: verify axes
  state->g = Vector3f(-s.accel_y, -s.accel_z, -s.accel_x);
}


#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"MinimumJerkInterpolation.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"
#include "left_hand.h"
#include <sensor_msgs/JointState.h>
#include <cstdlib>
#include<termios.h>
#include <iostream>
#include <QTime>
using namespace  std;
using namespace  Eigen;

bool simulation=!true;

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 ;
ros::Publisher pub29 ;ros::Publisher pub30 ;ros::Publisher pub31 ;

void numplot(double num,double min,double max){
    //⬛

    QString str;
    int l=100;
    int n=int((num-min)/(max-min)*l);
    if (num<min){n=0;}
    if (num>max){n=100;}
    str+=QString::number(min);
    str+="|";
    if (n<=l/2){
        for (int i = 0; i < n; ++i) {
            str+=" ";
        }
        for (int i = 0; i < l/2-n; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
    }
    else {
        for (int i = 0; i < l/2; ++i) {
            str+=" ";
        }
        for (int i = 0; i < n-l/2; ++i) {
            str+="|";

        }
        str+="|";
        for (int i = 0; i < l-n; ++i) {
            str+=" ";
        }

    }

    str+="|";
    str+=QString::number(max);
    str+="=>";str+=QString::number(num);
    qDebug()<<str;
    qDebug()<<"";

}

void matrix_view(MatrixXd M){

    for (int i = 0; i <M.rows() ; ++i) {
        QString str;
        for (int j = 0; j <M.cols() ; ++j) {
            str+=QString::number(M(i,j));
            str+="   ";
        }
        qDebug()<<str;
    }
    qDebug()<<"";
}

void matrix_view(VectorXd M){
    QString str;
    for (int i = 0; i <M.rows() ; ++i) {str+=QString::number(M(i));str+="   ";}
    qDebug()<<str;
    qDebug()<<"";
}

double move2pose(double max,double t_local,double T_start ,double T_end){
    double T_move=T_end-T_start;
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_end){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else{theta=max;}
    return theta;
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

int qc_offset[12];
int qra_offset[4];
int qla_offset[4];
bool qc_initial_bool;
void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        for (int i = 12; i < 16; ++i){
            qra_offset[i-12]=int(msg.position[i+1]);
        }

        for (int i = 20; i < 24; ++i){
            qla_offset[i-20]=int(msg.position[i+1]);
        }

        qc_initial_bool=false;

        ROS_INFO("Offset_feet=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\n",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);
        ROS_INFO("Offset_righthand=%d\t%d\t%d\t%d",qra_offset[0],qra_offset[1], qra_offset[2],qra_offset[3]);
        ROS_INFO("Offset_lefthand=%d\t%d\t%d\t%d",qla_offset[0],qla_offset[1], qla_offset[2],qla_offset[3]);
        ROS_INFO("Initialized!");
    }
}

VectorXd absolute_q0_r(7);VectorXd absolute_q0_l(7);
bool abs_initial_bool;

void abs_read(const sensor_msgs::JointState & msg){
    if (abs_initial_bool){

        VectorXd absolute_sensor_r(7);VectorXd absolute_sensor_l(7);
        for (int i = 0; i < 7; ++i) {
            absolute_sensor_r(i)=msg.position[i+13];            absolute_q0_r(i)=0;
            absolute_sensor_l(i)=msg.position[i+13+8];            absolute_q0_l(i)=0;

        }
        //right hand
        absolute_q0_r(0)=double(absolute_sensor_r(0)+863)/8192*2*M_PI;
        absolute_q0_r(1)=double(absolute_sensor_r(1)-(-889+2275))/8192*2*M_PI;
        absolute_q0_r(2)=-double(absolute_sensor_r(2)-2881)/8192*2*M_PI;
        absolute_q0_r(3)=double(absolute_sensor_r(3)-2058)/8192*2*M_PI;
        //    //left hand
        absolute_q0_l(0)=double(absolute_sensor_l(0)-705)/8192*2*M_PI;
        absolute_q0_l(1)=double(absolute_sensor_l(1)-(3642-2275))/8192*2*M_PI;
        absolute_q0_l(2)=-double(absolute_sensor_l(2)+344)/8192*2*M_PI;
        absolute_q0_l(3)=-double(absolute_sensor_l(3)-47)/8192*2*M_PI;
        qDebug()<<"absolutes:";
        matrix_view(absolute_sensor_r);matrix_view(absolute_sensor_l);
        qDebug()<<"positions:";
        VectorXd absolute_q0_r_deg(7);
        absolute_q0_r_deg=180/M_PI*absolute_q0_r;
        matrix_view(absolute_q0_r_deg);
        VectorXd absolute_q0_l_deg(7);
        absolute_q0_l_deg=180/M_PI*absolute_q0_l;
        matrix_view(absolute_q0_l_deg);

        abs_initial_bool=false;
    }

}

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);
    data.data=q[29];
    pub29.publish(data);
    data.data=q[30];
    pub30.publish(data);
    data.data=q[31];
    pub31.publish(data);

}
// waist
double Waist2ArmZ=0.2694;
double Waist2RArmY=-0.239;
double Waist2LArmY=0.239;
//double WaistYaw=0; double WaistPitch=0;
//MatrixXd R_shoulder(3,3);
//VectorXd r_left_shoulder(3);
//VectorXd r_right_shoulder(3);
//    R_shoulder<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
//            cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
//            -sin(WaistPitch),              0,               cos(WaistPitch);
//    r_left_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
//                      Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
//                                                               Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
//    r_right_shoulder<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
//                       Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
//                                                                Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;
MatrixXd rightshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
        R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
         cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
                      -sin(WaistPitch),              0,               cos(WaistPitch);

               P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2RArmY*sin(WaistYaw),
                Waist2RArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2RArmY,
                Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;

    T<<R.transpose(),-R.transpose()*P,0,0,0,1;

    return T;
}
MatrixXd leftshoulder2waist(double WaistYaw, double WaistPitch){
    MatrixXd T(4,4);
    MatrixXd R(3,3);
    VectorXd P(3);
    R<<cos(WaistYaw)*cos(WaistPitch), -sin(WaistYaw), cos(WaistYaw)*sin(WaistPitch),
     cos(WaistPitch)*sin(WaistYaw),  cos(WaistYaw), sin(WaistYaw)*sin(WaistPitch),
                  -sin(WaistPitch),              0,               cos(WaistPitch);

           P<< Waist2ArmZ*cos(WaistYaw)*sin(WaistPitch) - Waist2LArmY*sin(WaistYaw),
            Waist2LArmY*cos(WaistYaw) + Waist2ArmZ*sin(WaistYaw)*sin(WaistPitch)-Waist2LArmY,
            Waist2ArmZ*cos(WaistPitch)-Waist2ArmZ;                                                                1;
T<<R.transpose(),-R.transpose()*P,0,0,0,1;
    return T;
}

VectorXd right2left(VectorXd qr){
    VectorXd ql(7);
    ql(0)=qr(0);
    ql(1)=-qr(1);
    ql(2)=-qr(2);
    ql(3)=qr(3);
    ql(4)=-qr(4);
    ql(5)=-qr(5);
    ql(6)=qr(6);
}

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

QByteArray joint_data;

    abs_initial_bool=!simulation;
    qc_initial_bool=!simulation;

    if (simulation){    ros::init(argc, argv, "rrbot");}
    else{ros::init(argc, argv, "jointdata");}

    ros::NodeHandle nh("~");

    std::string scenario_r;std::string scenario_l;
    //nh.getParam("scenario_r", scenario_r);

    ROS_INFO("scenario_r: %s",scenario_r.c_str());

    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("qc",1000);
    ros::Subscriber abs_sensor = nh.subscribe("/surena/abs_joint_state", 1000, abs_read);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);

    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    double t_r_offset=0;
    if(simulation){
        pub1  = nh.advertise<std_msgs::Float64>("joint1_position_controller/command",100);
        pub2  = nh.advertise<std_msgs::Float64>("joint2_position_controller/command",100);
        pub3  = nh.advertise<std_msgs::Float64>("joint3_position_controller/command",100);
        pub4  = nh.advertise<std_msgs::Float64>("joint4_position_controller/command",100);
        pub5  = nh.advertise<std_msgs::Float64>("joint5_position_controller/command",100);
        pub6  = nh.advertise<std_msgs::Float64>("joint6_position_controller/command",100);
        pub7  = nh.advertise<std_msgs::Float64>("joint7_position_controller/command",100);
        pub8  = nh.advertise<std_msgs::Float64>("joint8_position_controller/command",100);
        pub9  = nh.advertise<std_msgs::Float64>("joint9_position_controller/command",100);
        pub10 = nh.advertise<std_msgs::Float64>("joint10_position_controller/command",100);
        pub11 = nh.advertise<std_msgs::Float64>("joint11_position_controller/command",100);
        pub12 = nh.advertise<std_msgs::Float64>("joint12_position_controller/command",100);
        pub13 = nh.advertise<std_msgs::Float64>("joint13_position_controller/command",100);
        pub14 = nh.advertise<std_msgs::Float64>("joint14_position_controller/command",100);
        pub15 = nh.advertise<std_msgs::Float64>("joint15_position_controller/command",100);
        pub16 = nh.advertise<std_msgs::Float64>("joint16_position_controller/command",100);
        pub17 = nh.advertise<std_msgs::Float64>("joint17_position_controller/command",100);
        pub18 = nh.advertise<std_msgs::Float64>("joint18_position_controller/command",100);
        pub19 = nh.advertise<std_msgs::Float64>("joint19_position_controller/command",100);
        pub20 = nh.advertise<std_msgs::Float64>("joint20_position_controller/command",100);
        pub21 = nh.advertise<std_msgs::Float64>("joint21_position_controller/command",100);
        pub22 = nh.advertise<std_msgs::Float64>("joint22_position_controller/command",100);
        pub23 = nh.advertise<std_msgs::Float64>("joint23_position_controller/command",100);
        pub24 = nh.advertise<std_msgs::Float64>("joint24_position_controller/command",100);
        pub25 = nh.advertise<std_msgs::Float64>("joint25_position_controller/command",100);
        pub26 = nh.advertise<std_msgs::Float64>("joint26_position_controller/command",100);
        pub27 = nh.advertise<std_msgs::Float64>("joint27_position_controller/command",100);
        pub28 = nh.advertise<std_msgs::Float64>("joint28_position_controller/command",100);
        pub29 = nh.advertise<std_msgs::Float64>("joint29_position_controller/command",100);
        pub30 = nh.advertise<std_msgs::Float64>("joint30_position_controller/command",100);
        pub31 = nh.advertise<std_msgs::Float64>("joint31_position_controller/command",100);
    }

    right_hand hand_funcs;
    right_hand hand_r;

    VectorXd r_target_r(3);
    VectorXd r_force_r(3);
    VectorXd r_deliver_r(3);
    VectorXd r_middle_r(3);
    VectorXd r_release_r(3);
    MatrixXd R_target_r(3,3);
    VectorXd r_right_palm(3);
    int fingers_mode_r;
    VectorXd q0_r(7);

    double d0_r;
    double d_r ;
    double d_des_r;
    double theta_r;
    double theta_target_r;
    double sai_r;
    double sai_target_r;
    double phi_r;
    double phi_target_r;

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

    MinimumJerkInterpolation coef_generator;
    MatrixXd X_coef_r;
    MatrixXd Y_coef_r;
    MatrixXd Z_coef_r;

    MatrixXd   t_r(1,7);
    MatrixXd P_x_r(1,7);
    MatrixXd V_x_r(1,7);
    MatrixXd A_x_r(1,7);
    MatrixXd P_y_r(1,7);
    MatrixXd V_y_r(1,7);
    MatrixXd A_y_r(1,7);
    MatrixXd P_z_r(1,7);
    MatrixXd V_z_r(1,7);
    MatrixXd A_z_r(1,7);

    MatrixXd  fingers_r(1,7);
    VectorXd q_ra;
VectorXd q_la;
    int q_motor_r[8];
    int q_motor_l[8];
    for (int var = 0; var < 8; ++var) {
        q_motor_r[var]=0;
        q_motor_l[var]=0;
    }

    vector<double> q(31);

    ros::Rate loop_rate(200);
    int count = 0;
    double time=0.0;
    double time_r;
    bool initializing=true;
    VectorXd qr_end(7);
    double WaistYaw=0;
    double WaistPitch=0;
    double knee=0;
    double hip=0;
    double ankle=0;
//QTime chronometer;
//chronometer.start();

    vector<double> cntrl(13);
    QCgenerator QC;
    vector<int> qref(12);

    while (ros::ok())
    {

   // chronometer.restart();

        if (abs_initial_bool) {
            ROS_INFO_ONCE("abs is initializing!");
            ros::spinOnce();
            continue;
        }

        if (qc_initial_bool) {
            ROS_INFO_ONCE("qc is initializing!");
            ros::spinOnce();
            continue;
        }

        if(initializing){

            if(simulation){
                q0_r<<10*M_PI/180,
                        -10*M_PI/180,
                        0,
                        -20*M_PI/180,
                        0,
                        0,
                        0;
            }
            else{
                if (scenario_r=="h"){
                    q0_r=absolute_q0_r;
                }
                else{
                    q0_r<<10*M_PI/180,
                            -10*M_PI/180,
                            0,
                            -20*M_PI/180,
                            0,
                            0,
                            0;

                }

                qDebug("q0 init ok");

            }
            ROS_INFO("q0_r= %f, %f, %f, %f, %f, %f, %f",q0_r(0),q0_r(1),q0_r(2),q0_r(3),q0_r(4),q0_r(5),q0_r(6));

            // gripping

                r_target_r<<.45,
                        -0.05,
                        -0.35;

                r_middle_r<<.3,
                        -.2,
                        -.35;

                r_force_r<<.45,
                        -0.02,
                        -0.35;

                r_deliver_r<<.45,
                        -0.02,
                        -0.15;

                r_release_r<<.45,
                        -0.1,
                        -0.15;

                R_target_r=hand_funcs.rot(2,-90*M_PI/180,3);//*hand_funcs.rot(1,-10*M_PI/180+atan2(r_target_r(1),r_target_r(0)),3);

                //taghire R-target baraye target va deliver va taghire jabejayi
                //R_target_r=hand_funcs.rot(2,-70*M_PI/180,3);
                //R_deliver_r=hand_funcs.rot(2,-110*M_PI/180,3);

            double v0_r=0;
            double v_target_r =.4;
            right_hand hand0_r(q0_r,r_target_r,R_target_r,0,0);
            r_right_palm=hand0_r.r_right_palm;
            d0_r=hand0_r.dist;
            d_r=d0_r;
            d_des_r=hand0_r.d_des;
            theta_r=hand0_r.theta;
            theta_target_r=hand0_r.theta_target;
            sai_r=hand0_r.sai;
            sai_target_r=hand0_r.sai_target;
            phi_r=hand0_r.phi;
            phi_target_r=hand0_r.phi_target;
            hand0_r.HO_FK_right_palm(q0_r);

            q_ra=q0_r;

            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));

            if (simulation)
            {
                vector<double> q_init(31);
                for (int i = 0; i < 31; ++i) {
                    q_init[i]=0;
                }
                q_init[15]=qr1[0];   q_init[22]=qr1[0];
                q_init[16]=qr2[0];   q_init[23]=-qr2[0];
                q_init[17]=qr3[0];   q_init[24]=-qr3[0];
                q_init[18]=qr4[0];   q_init[25]=qr4[0];
                q_init[19]=qr5[0];   q_init[26]=-qr5[0];
                q_init[20]=qr6[0];   q_init[27]=-qr6[0];
                q_init[21]=qr7[0];   q_init[28]=qr7[0];

                SendGazebo(q_init);
            }

            //****path generation

            MatrixXd T_right_shoulder_transpose=rightshoulder2waist(0*WaistYaw,0*WaistPitch);

            VectorXd temp(4);
            temp<<r_target_r,1;
            r_target_r=(T_right_shoulder_transpose*temp).block(0,0,3,1);

            temp<<r_deliver_r,1;
            r_deliver_r=(T_right_shoulder_transpose*temp).block(0,0,3,1);

            temp<<r_release_r,1;
            r_release_r=(T_right_shoulder_transpose*temp).block(0,0,3,1);
            MatrixXd ord(1,6);
            ord<<5,5,5,5,5,5;

            t_r<<0,2,4,6,10,12,14;

            P_x_r<< hand0_r.r_right_palm(0),r_middle_r(0),r_target_r(0),r_force_r(0),r_deliver_r(0),r_deliver_r(0),r_release_r(0);
            P_y_r<< hand0_r.r_right_palm(1),r_middle_r(1),r_target_r(1),r_force_r(1),r_deliver_r(1),r_deliver_r(1),r_release_r(1);
            P_z_r<< hand0_r.r_right_palm(2),r_middle_r(2),r_target_r(2),r_force_r(2),r_deliver_r(2),r_deliver_r(2),r_release_r(2);

            R_target_r=T_right_shoulder_transpose.block(0,0,3,3)*R_target_r;

            V_x_r<<0,INFINITY,0,0,0,0,0;
            V_y_r<<0,INFINITY,0,0,0,0,0;
            V_z_r<<0,INFINITY,0,0,0,0,0;
            A_x_r<<0,INFINITY,0,0,0,0,0;
            A_y_r<<0,INFINITY,0,0,0,0,0;
            A_z_r<<0,INFINITY,0,0,0,0,0;

            MatrixXd conx(3,7); conx<<P_x_r,V_x_r,A_x_r;
            MatrixXd cony(3,7); cony<<P_y_r,V_y_r,A_y_r;
            MatrixXd conz(3,7); conz<<P_z_r,V_z_r,A_z_r;

//            X_coef_r=coef_generator.Coefficient(t_r,P_x_r,V_x_r,A_x_r);
//            Y_coef_r=coef_generator.Coefficient(t_r,P_y_r,V_y_r,A_y_r);
//            Z_coef_r=coef_generator.Coefficient(t_r,P_z_r,V_z_r,A_z_r);

            X_coef_r=coef_generator.Coefficient1(t_r,ord,conx,.1).transpose();
            Y_coef_r=coef_generator.Coefficient1(t_r,ord,cony,.1).transpose();
            Z_coef_r=coef_generator.Coefficient1(t_r,ord,conz,.1).transpose();

            fingers_r<<10,10,10,10,10,10,10;

            //ROS_INFO("theta_target=%f,sai_target=%f,phi_target=%f",theta_target,sai_target,phi_target);
            //ROS_INFO("\nr_target_r=\n%f\n%f\n%f",r_target_r(0),r_target_r(1),r_target_r(2));
            // ROS_INFO("\nR_target_r=\n%f\t%f\t%f\n%f\t%f\t%f\n%f\t%f\t%f\n",R_target_r(0,0),R_target_r(0,1),R_target_r(0,2),R_target_r(1,0),R_target_r(1,1),R_target_r(1,2),R_target_r(2,0),R_target_r(2,1),R_target_r(2,2));
            ROS_INFO("press any key to start!");
            getch();
            initializing=false;

        }

        else {

            time=double(count)*.005;
            time_r=time;

            VectorXd P_r(3); VectorXd V_r(3);

            if(time_r<=t_r(t_r.cols()-1)){
            for(int i=0;i<t_r.cols()-1;i++){

                if(time_r>=t_r(i)&&time_r<t_r(i+1)){
                    if(i==4){
                        P_r<<   coef_generator.GetAccVelPos(X_coef_r.row(i),t_r(i),0,5)(0,0),
                                coef_generator.GetAccVelPos(Y_coef_r.row(i),t_r(i),0,5)(0,0),
                                coef_generator.GetAccVelPos(Z_coef_r.row(i),t_r(i),0,5)(0,0);
                        V_r.fill(0);
                    }
                    else{
                        P_r<<   coef_generator.GetAccVelPos(X_coef_r.row(i),time_r,0,5)(0,0),
                                coef_generator.GetAccVelPos(Y_coef_r.row(i),time_r,0,5)(0,0),
                                coef_generator.GetAccVelPos(Z_coef_r.row(i),time_r,0,5)(0,0);
                        V_r<<   coef_generator.GetAccVelPos(X_coef_r.row(i),time_r,0,5)(0,1),
                                coef_generator.GetAccVelPos(Y_coef_r.row(i),time_r,0,5)(0,1),
                                coef_generator.GetAccVelPos(Z_coef_r.row(i),time_r,0,5)(0,1);
                              }

//VectorXd temp_r(3);
//switch (i) {
//case 0:
//   temp_r=r_target_r ;
//    break;
//case 1:
//   temp_r=r_target_r ;
//    break;
//case 2:
//   temp_r=r_deliver_r ;
//    break;
//case 3:
//   temp_r=r_deliver_r ;
//    break;
//default:
//    break;
//}
                   hand_r.update_right_hand(q_ra,V_r,r_target_r,R_target_r);
                   r_right_palm=hand_r.r_right_palm;
                   hand_r.doQP(q_ra);
                   q_ra=hand_r.q_next;

                   d_r=hand_r.dist;
                   theta_r=hand_r.theta;
                   sai_r=hand_r.sai;
                   phi_r=hand_r.phi;
                   qr_end=q_ra;

                  fingers_mode_r=fingers_r(i);
                }

            }
}

            else if (time_r<=t_r(t_r.cols()-1)+t_r(2)){

                            ROS_INFO_ONCE("right reached!");

                            MatrixXd t_h(1,2);
                            MatrixXd p_r(7,2); MatrixXd p_l(7,2);
                            MatrixXd zero(1,2); zero<<0,0;
                            MatrixXd r_coeff;  MatrixXd l_coeff;
                          //  t_h<<t_r(2),t_r(2)*2;

                            t_h<<t_r(t_r.cols()-1),t_r(t_r.cols()-1)+t_r(2);
                            p_r<<   qr_end(0),10*M_PI/180,
                                    qr_end(1),-10*M_PI/180,
                                    qr_end(2),0,
                                    qr_end(3),-20*M_PI/180,
                                    qr_end(4),0,
                                    qr_end(5),0,
                                    qr_end(6),0;

                            for (int i = 0; i < 7; ++i) {
                                r_coeff=coef_generator.Coefficient(t_h,p_r.row(i),zero,zero);
                                q_ra(i)=coef_generator.GetAccVelPos(r_coeff.row(0),time_r,t_h(0) ,5)(0,0);
                            }
                           fingers_mode_r=9;
                        }
            }

      double dh=move2pose(.2,time_r,t_r(1),t_r(2))-move2pose(.2,time_r,t_r(3),t_r(4));
      double l=.36+.37-dh;

      knee=acos((l*l-.37*.37-.36*.36)/2/.36/.37);
      hip=-atan(sin(knee)*.36/(cos(knee)*.36+.37));
      ankle=-knee-hip;
      WaistPitch=move2pose(.2,time_r,t_r(1),t_r(2))-move2pose(.2,time_r,t_r(3),t_r(4));
      WaistYaw=move2pose(.3,time_r,t_r(4),t_r(5)-1)-move2pose(.3,time_r,t_r(t_r.cols()-1),t_r(t_r.cols()-1)+t_r(2));

          if(time_r>t_r(t_r.cols()-1)+t_r(2)) {break;}

    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));

    for (int i = 0; i < 31; ++i) {
        q[i]=0;
    }

    //         q[15]=-M_PI/2;
    //         q[16]=-10*M_PI/180;
    //         q[17]=-10*M_PI/180;
    q[3]=hip;
    q[4]=knee;
    q[5]=ankle;
    q[9]=hip;
    q[10]=knee;
    q[11]=ankle;
    q[15]=qr1[count];  q[22]=qr1[count];
    q[16]=qr2[count];  q[23]=-qr2[count];
    q[17]=qr3[count];  q[24]=-qr3[count];
    q[18]=qr4[count];  q[25]=qr4[count];
    q[19]=qr5[count];  q[26]=-qr5[count];
    q[20]=qr6[count];  q[27]=-qr6[count];
    q[21]=qr7[count];  q[28]=qr7[count];
    q[14]=WaistPitch;
    q[13]=WaistYaw;

    if(simulation){SendGazebo(q);}

    q_motor_r[0]=-int(10*(qr1[count]-q0_r(0))*180/M_PI*120/60)+qra_offset[0];
    q_motor_r[1]=int(10*(qr2[count]-q0_r(1))*180/M_PI*120/60)+qra_offset[1];
    q_motor_r[2]=-int(7*(qr3[count]-q0_r(2))*180/M_PI*100/60)+qra_offset[2];
    q_motor_r[3]=int(7*(qr4[count]-q0_r(3))*180/M_PI*100/60)+qra_offset[3];
    q_motor_r[4]=int((qr5[count]-q0_r(4))*(2048)/M_PI);
    q_motor_r[5]=int((qr6[count]-q0_r(5))*(4000-2050)/(23*M_PI/180));
    q_motor_r[6]=int((qr7[count]-q0_r(6))*(4000-2050)/(23*M_PI/180));

    if(q_motor_r[7]!=fingers_mode_r&&q_motor_r[7]!=2){
        q_motor_r[7]=2;

    }
    else{
        q_motor_r[7]=fingers_mode_r;
    }

    q_motor_l[0]=int(10*(qr1[count]-q0_r(0))*180/M_PI*120/60)+qla_offset[0];
    q_motor_l[1]=-int(10*(qr2[count]-q0_r(1))*180/M_PI*120/60)+qla_offset[1];
    q_motor_l[2]=int(7*(qr3[count]-q0_r(2))*180/M_PI*100/60)+qla_offset[2];
    q_motor_l[3]=-int(7*(qr4[count]-q0_r(3))*180/M_PI*100/60)+qla_offset[3];

    q_motor_l[4]=-int((qr5[count]-q0_r(4))*(2048)/M_PI);
    q_motor_l[5]=int((qr6[count]-q0_r(5))*(4000-2050)/(23*M_PI/180));
    q_motor_l[6]=int((qr7[count]-q0_r(6))*(4000-2050)/(23*M_PI/180));
    q_motor_l[7]=q_motor_r[7];

    cntrl[0]=0.0;
    cntrl[1]=0;
    cntrl[2]=0;
    cntrl[3]=hip;
    cntrl[4]=knee;
    cntrl[5]=ankle;
    cntrl[6]=0;//roll
    cntrl[7]=0;
    cntrl[8]=0;
    cntrl[9]=hip;
    cntrl[10]=knee;
    cntrl[11]=ankle;
    cntrl[12]=0;

    // qDebug()<<"Pitch: hip:"<<cntrl[3]<<"\tknee:"<<cntrl[4]<<"\tankle:"<<cntrl[5];

    qref=QC.ctrldata2qc(cntrl);

    msg.data.clear();
    for(int  i = 0;i < 12;i++)
    {
        msg.data.push_back( qref[i]+qc_offset[i]);
    }
    //right hand epose
    msg.data.push_back(q_motor_r[0]);//12 -y  a,z
    msg.data.push_back(q_motor_r[1]);//13 +x
    msg.data.push_back(q_motor_r[2]);//14 -z
    msg.data.push_back(q_motor_r[3]);//15 +y
    //right hand dynamixel + fingers
    msg.data.push_back(q_motor_r[4]);//16
    msg.data.push_back(q_motor_r[5]);//17
    msg.data.push_back(q_motor_r[6]);//18
    msg.data.push_back(q_motor_r[7]);//19
    //left hand epose
    msg.data.push_back(q_motor_l[0]);//20 +y
    msg.data.push_back(q_motor_l[1]);//21 +x
    msg.data.push_back(q_motor_l[2]);//22 -z
    msg.data.push_back(q_motor_l[3]);//23 -y
    //left hand dynamixel + fingers
    msg.data.push_back(q_motor_l[4]);//24
    msg.data.push_back(q_motor_l[5]);//25
    msg.data.push_back(q_motor_l[6]);//26
    msg.data.push_back(q_motor_l[7]);//27

//    if(q_motor_r[7]==2){

//        for (int var = 0; var < 2; ++var) {

//        msg.data.clear();
//        for(int  i = 0;i < 12;i++)
//        {
//            msg.data.push_back(qc_offset[i]);
//        }
//        //right hand epose
//        msg.data.push_back(q_motor_r[0]);//12 -y  a,z
//        msg.data.push_back(q_motor_r[1]);//13 +x
//        msg.data.push_back(q_motor_r[2]);//14 -z
//        msg.data.push_back(q_motor_r[3]);//15 +y
//        //right hand dynamixel + fingers
//        msg.data.push_back(q_motor_r[4]);//16
//        msg.data.push_back(q_motor_r[5]);//17
//        msg.data.push_back(q_motor_r[6]);//18
//        msg.data.push_back(q_motor_r[7]);//19
//        //left hand epose
//        msg.data.push_back(q_motor_l[0]);//20 +y
//        msg.data.push_back(q_motor_l[1]);//21 +x
//        msg.data.push_back(q_motor_l[2]);//22 -z
//        msg.data.push_back(q_motor_l[3]);//23 -y
//        //left hand dynamixel + fingers
//        msg.data.push_back(q_motor_l[4]);//24
//        msg.data.push_back(q_motor_l[5]);//25
//        msg.data.push_back(q_motor_l[6]);//26
//        msg.data.push_back(q_motor_l[7]);//27

//        ros::spinOnce();
//        loop_rate.sleep();
//    qDebug()<<time_r<<"\t"<<q_motor_r[7];
//        }

//    }

    ++count;
    if(!simulation){chatter_pub.publish(msg);}
matrix_view(r_right_palm);

qDebug()<<time_r<<"\t"<<q_motor_r[7];

//    joint_data.append(":["+QString::number(q_motor_r[0])+","
//            +QString::number(q_motor_r[1])+","
//            +QString::number(q_motor_r[2])+","
//            +QString::number(q_motor_r[3])+","
//            +QString::number(q_motor_r[4])+","
//            +QString::number(q_motor_r[5])+","
//            +QString::number(q_motor_r[6])+","
//            +QString::number(q_motor_r[7])+","
//            +QString::number(q_motor_l[0])+","
//            +QString::number(q_motor_l[1])+","
//            +QString::number(q_motor_l[2])+","
//            +QString::number(q_motor_l[3])+","
//            +QString::number(q_motor_l[4])+","
//            +QString::number(q_motor_l[5])+","
//            +QString::number(q_motor_l[6])+","
//            +QString::number(q_motor_l[7])+"]\n");

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

//    QFile myfile("Desktop/hands_joint_data.txt");
//    myfile.remove();
//    myfile.open(QFile::ReadWrite);
//    myfile.write(joint_data);
//    myfile.close();

return 0;
}