// Copyright (C) 2016 Vicente J. Botet Escriba
//
//  Distributed under the Boost Software License, Version 1.0. (See accompanying
//  file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

// <experimental/monad.hpp>
// <experimental/expected.hpp>

#include <experimental/expected.hpp>

#include <boost/detail/lightweight_test.hpp>

namespace stde = std::experimental;
template <class T>
using expected_sc = stde::expected<T, std::error_code>;

expected_sc<int> twice(int i) {
  if (i==0)
  {
    std::error_code ec = std::make_error_code(std::errc(1));
    return stde::make_unexpected(ec);
  }
  return stde::make_expected(2*i);
}

int main()
{

  static_assert(stde::is_type_constructible<expected_sc<int>>::value, "ERROR");
  static_assert(stde::is_monad<expected_sc<stde::_t>>::value, "ERROR");

  {
    std::error_code ec = std::make_error_code(std::errc(1));
    expected_sc<int> x = stde::make_unexpected(ec);
    expected_sc<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(! y);
  }
  {
    int v=1;
    expected_sc<int> x = stde::make<expected_sc<stde::_t>>(v);
    expected_sc<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(y);
    BOOST_TEST(*y == 2);
  }
  {
    int v=0;
    expected_sc<int> x = stde::make<expected_sc<stde::_t>>(v);
    expected_sc<int> y = stde::monad::bind(x,twice);
    BOOST_TEST(! y);
  }

  return ::boost::report_errors();
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Distributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#ifndef NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_COMMON_ANY_HPP_INCLUDED
#define NT2_TOOLBOX_REDUCTION_FUNCTION_SIMD_COMMON_ANY_HPP_INCLUDED
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/strip.hpp>
#include <nt2/include/functions/is_nez.hpp>
#include <nt2/include/functions/hmsb.hpp>
#include <boost/fusion/tuple.hpp>
#include <nt2/include/functions/split.hpp>

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is arithmetic_
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::any_, tag::cpu_,
                      (A0)(X),
                      ((simd_<arithmetic_<A0>,X>))
                     );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::any_(tag::simd_<tag::arithmetic_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>  { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      return hmsb(is_nez(a0)) != 0;
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is uint8_t
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::any_, tag::cpu_,
                      (A0)(X),
                      ((simd_<uint8_<A0>,X>))
                     );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::any_(tag::simd_<tag::uint8_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>  { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type                            stype;
      typedef typename meta::upgrade<stype>::type                           utype;
      typedef simd::native<utype,tag::sse_>                                 ttype;
      ttype a0h, a0l;
      boost::fusion::tie(a0h, a0l) = split(a0);
      return (hmsb(is_nez(a0h)) || hmsb(is_nez(a0l)));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is int8_t
/////////////////////////////////////////////////////////////////////////////
NT2_REGISTER_DISPATCH(tag::any_, tag::cpu_,
                      (A0)(X),
                      ((simd_<int8_<A0>,X>))
                     );

namespace nt2 { namespace ext
{
  template<class X, class Dummy>
  struct call<tag::any_(tag::simd_<tag::int8_, X> ),
              tag::cpu_, Dummy> : callable
  {
    template<class Sig> struct result;
    template<class This,class A0>
    struct result<This(A0)>  { typedef bool type; };

    NT2_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type                            stype;
      typedef typename meta::upgrade<stype>::type                           utype;
      typedef simd::native<utype,tag::sse_>                                 ttype;
      ttype a0h, a0l;
      boost::fusion::tie(a0h, a0l) = split(a0);
      return (hmsb(is_nez(a0h)) || hmsb(is_nez(a0l)));
    }
  };
} }

#endif
// modified by jt the 05/01/2011


/*
 * AscendingBase.cpp
 *
 *  Created on: Jul 14, 2014
 *      Author: sroehling
 */

#include <boost/test/unit_test.hpp>

#include "PeriodValSegment.h"
#include "AscendingBaseScanner.h"
#include "TestHelper.h"

using namespace boost::posix_time;
using namespace boost::gregorian;
using namespace testHelper;

BOOST_AUTO_TEST_CASE( AscendingBase_Synthesized )
{
	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4, 100.0,  88.0)); // pull-back 1: initial 12% down-trend
	ranges.push_back(TestPerValRange(3,  90.0, 104.0)); // pull-back 1: up-trend going a little higher than initial down-trend
	ranges.push_back(TestPerValRange(3, 103.5,  92.0)); // pull-back 2: next pull-back, with a higher low than pull-back 1
	ranges.push_back(TestPerValRange(4,  92.5, 107.0)); // pull-back 2: up-trend with higher high than pull-back 1
	ranges.push_back(TestPerValRange(4, 106.5,  93.0)); // pull-back 3: up-trend with higher high than pull-back 2
	ranges.push_back(TestPerValRange(4,  93.5,  107.5)); // pull-back 3: up-trend with higher high than pull-back 2
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	AscendingBaseScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("AscendingBase_Synthesized",patternMatches,1);
	verifyPatternMatch("AscendingBase_Synthesized match",
			ptime(date(2014,1,1)),ptime(date(2014,1,22)),6,patternMatches->front());

}

BOOST_AUTO_TEST_CASE( AscendingBase_Synthesized_NotEnoughDepth )
{
	BOOST_TEST_MESSAGE("Same as AscendingBase_Synthesized, but first pull-back only 5% (not 10% minimum)");
	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4, 100.0,  95.0)); // pull-back 1: Initial down-trend only 5% (must be 10-20%)
	ranges.push_back(TestPerValRange(3,  95.5, 104.0)); // pull-back 1: up-trend going a little higher than initial down-trend
	ranges.push_back(TestPerValRange(3, 103.5,  92.0)); // pull-back 2: next pull-back, with a higher low than pull-back 1
	ranges.push_back(TestPerValRange(4,  92.5, 107.0)); // pull-back 2: up-trend with higher high than pull-back 1
	ranges.push_back(TestPerValRange(4, 106.5,  93.0)); // pull-back 3: up-trend with higher high than pull-back 2
	ranges.push_back(TestPerValRange(4,  93.5,  107.5)); // pull-back 3: up-trend with higher high than pull-back 2
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	AscendingBaseScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("AscendingBase_Synthesized",patternMatches,0);

}

BOOST_AUTO_TEST_CASE( AscendingBase_Synthesized_NotHigherLow )
{
	BOOST_TEST_MESSAGE("Same as AscendingBase_Synthesized, but 2nd pull-back has lower-low than 1st (must have higher low)");
	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4, 100.0,  88.0)); // pull-back 1: initial 12% down-trend
	ranges.push_back(TestPerValRange(3,  90.0, 104.0)); // pull-back 1: up-trend going a little higher than initial down-trend
	ranges.push_back(TestPerValRange(3, 103.5,  87.0)); // pull-back 2: next pull-back, lower low than first pull-back (match should thus fail)
	ranges.push_back(TestPerValRange(4,  90.5, 107.0)); // pull-back 2: up-trend with higher high than pull-back 1
	ranges.push_back(TestPerValRange(4, 106.5,  93.0)); // pull-back 3: up-trend with higher high than pull-back 2
	ranges.push_back(TestPerValRange(4,  93.5,  107.5)); // pull-back 3: up-trend with higher high than pull-back 2
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	AscendingBaseScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("AscendingBase_Synthesized",patternMatches,0);

}

BOOST_AUTO_TEST_CASE( AscendingBase_Synthesized_NotHigherHigh )
{
	BOOST_TEST_MESSAGE("Same as AscendingBase_Synthesized, but 2nd pull-back has lower-low than 1st (must have higher low)");
	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4, 100.0,  88.0)); // pull-back 1: initial 12% down-trend
	ranges.push_back(TestPerValRange(3,  90.0, 104.0)); // pull-back 1: up-trend going a little higher than initial down-trend
	ranges.push_back(TestPerValRange(3, 103.5,  92.0)); // pull-back 2: next pull-back, with a higher low than pull-back 1
	ranges.push_back(TestPerValRange(4,  92.5, 103.0)); // pull-back 2: up-trend with without a higher high than pull-back 1 (match should fail)
	ranges.push_back(TestPerValRange(4, 102.5,  93.0)); // pull-back 3: up-trend with higher high than pull-back 2
	ranges.push_back(TestPerValRange(4,  93.5,  107.5)); // pull-back 3: up-trend with higher high than pull-back 2
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	AscendingBaseScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("AscendingBase_Synthesized",patternMatches,0);

}

BOOST_AUTO_TEST_CASE( AscendingBase_Synthesized_TooDeep )
{
	TestPerValRangeList ranges;
	ranges.push_back(TestPerValRange(4, 100.0,  75.0)); // pull-back 1: initial 25% down-trend is too deep
	ranges.push_back(TestPerValRange(4,  76.0, 104.0)); // pull-back 1: up-trend going a little higher than initial down-trend
	ranges.push_back(TestPerValRange(3, 103.5,  92.0)); // pull-back 2: next pull-back, with a higher low than pull-back 1
	ranges.push_back(TestPerValRange(4,  92.5, 107.0)); // pull-back 2: up-trend with higher high than pull-back 1
	ranges.push_back(TestPerValRange(4, 106.5,  93.0)); // pull-back 3: up-trend with higher high than pull-back 2
	ranges.push_back(TestPerValRange(4,  93.5,  107.5)); // pull-back 3: up-trend with higher high than pull-back 2
	PeriodValSegmentPtr chartData = synthesizePeriodValSegment(date(2014,1,1),ranges);

	AscendingBaseScanner scanner;
	PatternMatchListPtr patternMatches = scanner.scanPatternMatches(chartData);

	verifyMatchList("AscendingBase_Synthesized",patternMatches,0);

}


/*******************************************************************************
Copyright(c) 2015-2018 LORD Corporation. All rights reserved.

MIT Licensed. See the included LICENSE.txt for a copy of the full MIT License.
*******************************************************************************/
#include "mscl/MicroStrain/ChecksumBuilder.h"

#include <boost/test/unit_test.hpp>

using namespace mscl;

BOOST_AUTO_TEST_SUITE(ChecksumBuilder_Test)

BOOST_AUTO_TEST_CASE(ChecksumBuilder_AppendByte)
{
    //append bytes to the checksum builder
    ChecksumBuilder b;
    b.append_uint8(0x01);
    b.append_uint8(0x02);
    b.append_uint8(0x04);

    //check the checksum is good
    BOOST_CHECK_EQUAL(b.simpleChecksum(), 0x07);
}

BOOST_AUTO_TEST_CASE(ChecksumBuilder_AppendWord)
{
    //append word to the checksum builder
    ChecksumBuilder b;
    b.append_uint16(0x1234);
    b.append_uint8(0x02);
    b.append_uint8(0x04);

    //check the checksum is good
    BOOST_CHECK_EQUAL(b.simpleChecksum(), 0x4C);
}

BOOST_AUTO_TEST_CASE(ChecksumBuilder_AppendDWord)
{
    //append dword to the checksum builder
    ChecksumBuilder b;
    b.append_uint32(0x12345678);
    b.append_uint8(0x02);
    b.append_uint8(0x04);

    //check the checksum is good
    BOOST_CHECK_EQUAL(b.simpleChecksum(), 0x11A);
}

BOOST_AUTO_TEST_CASE(ChecksumBuilder_AppendBytes)
{
    Bytes data;
    data.push_back(0x01);
    data.push_back(0x02);
    data.push_back(0x04);

    //append bytes to the checksum builder
    ChecksumBuilder b;
    b.appendBytes(data);

    //check the checksum is good
    BOOST_CHECK_EQUAL(b.simpleChecksum(), 0x07);
}

BOOST_AUTO_TEST_CASE(ChecksumBuilder_AppendByteStream)
{
    ByteStream bs;
    bs.append_uint8(0x01);
    bs.append_uint8(0x02);
    bs.append_uint8(0x04);

    //append bytes to the checksum builder
    ChecksumBuilder b;
    b.appendByteStream(bs);

    //check the checksum is good
    BOOST_CHECK_EQUAL(b.simpleChecksum(), 0x07);
}

BOOST_AUTO_TEST_CASE(ChecksumBuilder_FletcherChecksum)
{
    ChecksumBuilder b;
    b.append_uint32(0x12345678);

    //check that the simple checksum is what it should be
    BOOST_CHECK_EQUAL(b.fletcherChecksum(), 5128);
}

BOOST_AUTO_TEST_SUITE_END()


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

#include <nt2/core/functions/colon.hpp>
#include <nt2/core/utility/box.hpp>
#include <nt2/core/utility/of_size.hpp>
#include <nt2/core/container/dsl.hpp>
#include <nt2/include/constants/threeeps.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <nt2/core/functions/details/colon.hpp>
#include <nt2/include/functions/scalar/fuzzy_equal.hpp>
#include <nt2/include/functions/scalar/tolerant_floor.hpp>

namespace nt2 { namespace ext
{
  /// INTERNAL ONLY
  /// Generates colon from a pair of [low,up]
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::colon_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A1> >)
                            )
  {
    typedef typename boost::common_type<A0,A1>::type          base_t;
    typedef meta::constant_<nt2::tag::unity_colon_,base_t> constant_t;
    typedef typename  boost::proto::
                      result_of::make_expr< nt2::tag::colon_
                                          , container::domain
                                          , box< of_size_<1, -1> >
                                          , box< constant_t >
                                          , meta::as_<base_t>
                                          >::type             result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& l, A1 const& u) const
    {
      return  boost::proto::
              make_expr < nt2::tag::colon_
                        , container::domain
                        > ( boxify( of_size_<1, -1>
                                    (1,details::unity_colon_size(l,u))
                                  )
                          , boxify(constant_t(l))
                          , meta::as_<base_t>()
                          );
    }
  };

  /// INTERNAL ONLY
  /// Generates colon from a pair of [low,up] (static)
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::colon_, tag::cpu_
                            , (A0)(A1)
                            , (mpl_integral_< scalar_< arithmetic_<A0> > >)
                              (mpl_integral_< scalar_< arithmetic_<A1> > >)
                           )
  {
    static const std::size_t sz = (A1::value>=A0::value)
                                ? std::size_t(A1::value-A0::value+1) : 0u;

    typedef typename boost::common_type<typename A0::value_type, typename A1::value_type>::type base_t;
    typedef meta::constant_<nt2::tag::unity_colon_,base_t> constant_t;

    typedef typename  boost::proto::
                      result_of::make_expr< nt2::tag::colon_
                                          , container::domain
                                          , box< of_size_<1, sz> >
                                          , box< constant_t >
                                          , meta::as_<base_t>
                                          >::type             result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const& l, A1 const& u) const
    {
      return  boost::proto::
              make_expr < nt2::tag::colon_
                        , container::domain
                        > ( boxify(of_size_<1, sz>())
                          , boxify(constant_t(l))
                          , meta::as_<base_t>()
                          );
    }
  };

  /// INTERNAL ONLY
  ///  Generates colon from a pair of [low,up] and a step
  /// Easy case : everything is signed integral
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::colon_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_< integer_<A0> >)
                              (scalar_< integer_<A1> >)
                              (scalar_< integer_<A2> >)
                            )
  {
    typedef typename boost::common_type<A0,A1,A2>::type                 base_t;
    typedef meta::constant_<nt2::tag::colon_,base_t>                constant_t;
    typedef typename  boost::proto::
                      result_of::make_expr< nt2::tag::colon_
                                          , container::domain
                                          , box< of_size_<1, -1> >
                                          , box< constant_t >
                                          , meta::as_<base_t>
                                          >::type             result_type;

    BOOST_FORCEINLINE result_type
    operator()(A0 const& l, A1 const& s, A2 const& u) const
    {
      return  boost::proto::
              make_expr < nt2::tag::colon_
                        , container::domain
                        > ( boxify(of_size_<1,-1>(1,details::colon_size(l,s,u)))
                          , boxify(constant_t(l,s))
                          , meta::as_<A0>()
                          );
    }
  };

  //============================================================================
  // Generates colon from a pair of [low,up] and a number of elements
  // Not easy case : floating points :s
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::colon_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A1> >)
                              (scalar_< arithmetic_<A2> >)
                            )
  {
    typedef typename boost::dispatch::meta::as_floating<A0,A1,A2>::type base_t;
    typedef meta::constant_<nt2::tag::colon_,base_t>                constant_t;
    typedef typename  boost::proto::
                      result_of::make_expr< nt2::tag::colon_
                                          , container::domain
                                          , box< of_size_<1, -1> >
                                          , box< constant_t >
                                          , meta::as_<base_t>
                                          >::type             result_type;

    BOOST_FORCEINLINE result_type
    operator()(A0 const& l, A1 const& s, A2 const& u) const
    {
      std::size_t n = (s) ? (((u-l)*s >= 0) ? size(l,s,u) : 0)
                          : std::size_t(u);

      return boost::proto::make_expr< nt2::tag::colon_
                                    , container::domain
                                    > ( boxify(of_size_<1, -1>(1,n))
                                      , boxify(constant_t(l,s))
                                      , meta::as_<base_t>()
                                      );
    }

    BOOST_FORCEINLINE std::size_t
    size(A0 const& l, A1 const& s, A2 const& u) const
    {
      const std::size_t nelt  = std::size_t(tolerant_floor(base_t(u-l+s)/base_t(s)));
      const base_t      eps3  = Threeeps<base_t>();

      if(!fuzzy_equal(l+(nelt-1)*s, u, eps3))
      {
              if(fuzzy_equal(l+(nelt-2)*s, u, eps3))  return nelt-1;
        else  if(fuzzy_equal(l+nelt*s    , u, eps3))  return nelt+1;
      }
      return nelt;
    }
  };
} }

#endif


#include <memory>

#include <Eigen/Core>
#include <aslam/common/hash-id.h>
#include <maplab-common/pose_types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <maplab-common/unique-id.h>
#include <posegraph/unique-id.h>
#include <vi-map/test/vi-map-generator.h>
#include <vi-map/unique-id.h>

#include "localization-summary-map/localization-summary-map-creation.h"
#include "localization-summary-map/localization-summary-map.h"

class LocalizationSummaryMapTest : public ::testing::Test {
 protected:
  virtual void SetUp() {
    constructProblem();
  }
  void constructProblem();
  void constructVertexWithVisualNFrame();

  vi_map::VIMap map_;
  vi_map::LandmarkId landmark_1_id_;
  vi_map::LandmarkId landmark_2_id_;
  vi_map::LandmarkId landmark_3_id_;
};

void LocalizationSummaryMapTest::constructProblem() {
  vi_map::VIMapGenerator generator(map_, 42);

  vi_map::MissionId mission_id =
      generator.createMission(pose::Transformation());
  pose_graph::VertexId v1 =
      generator.createVertex(mission_id, pose::Transformation());
  pose_graph::VertexId v2 =
      generator.createVertex(mission_id, pose::Transformation());
  pose_graph::VertexId v3 =
      generator.createVertex(mission_id, pose::Transformation());
  landmark_1_id_ =
      generator.createLandmark(Eigen::Vector3d(0, 0, 1), v1, {v2, v3});
  landmark_2_id_ = generator.createLandmark(Eigen::Vector3d(0, 0, 2), v2, {v3});
  landmark_3_id_ = generator.createLandmark(Eigen::Vector3d(0, 0, 3), v1, {v2});

  generator.generateMap();
}

TEST_F(LocalizationSummaryMapTest, LocalizationSummaryCreationFromMapTest) {
  summary_map::LocalizationSummaryMap summary_map;
  summary_map::LocalizationSummaryMapId id;
  common::generateId(&id);
  summary_map.setId(id);

  vi_map::LandmarkIdList summary_landmark_ids;
  summary_landmark_ids.push_back(landmark_1_id_);
  summary_landmark_ids.push_back(landmark_2_id_);

  summary_map::createLocalizationSummaryMapFromLandmarkList(
      map_, summary_landmark_ids, &summary_map);

  const Eigen::Matrix3Xf& G_landmark_position = summary_map.GLandmarkPosition();

  ASSERT_EQ(G_landmark_position.cols(), 2);
  EXPECT_NEAR_EIGEN(G_landmark_position.col(0), Eigen::Vector3f(0, 0, 1), 1e-9);
  EXPECT_NEAR_EIGEN(G_landmark_position.col(1), Eigen::Vector3f(0, 0, 2), 1e-9);

  EXPECT_EQ(summary_map.projectedDescriptors().cols(), 5);
  const Eigen::Matrix<unsigned int, Eigen::Dynamic, 1>& obs_to_landmark =
      summary_map.observationToLandmarkIndex();
  EXPECT_EQ(5, obs_to_landmark.rows());
  EXPECT_EQ(0, obs_to_landmark(0, 0));
  EXPECT_EQ(0, obs_to_landmark(1, 0));
  EXPECT_EQ(0, obs_to_landmark(2, 0));
  EXPECT_EQ(1, obs_to_landmark(3, 0));
  EXPECT_EQ(1, obs_to_landmark(4, 0));

  const Eigen::Matrix<unsigned int, Eigen::Dynamic, 1>& landmark_observers =
      summary_map.observerIndices();
  EXPECT_EQ(5, landmark_observers.rows());
  EXPECT_EQ(0, landmark_observers(0, 0));
  EXPECT_EQ(1, landmark_observers(1, 0));
  EXPECT_EQ(2, landmark_observers(2, 0));
  EXPECT_EQ(1, landmark_observers(3, 0));
  EXPECT_EQ(2, landmark_observers(4, 0));
}

MAPLAB_UNITTEST_ENTRYPOINT


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

#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <boost/timer.hpp>

#include <boost/numeric/mtl/matrix/parameter.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/operation/print_matrix.hpp>
#include <boost/numeric/mtl/operation/matrix_mult.hpp>
#include <boost/numeric/mtl/matrix/hessian_setup.hpp>
#include <boost/numeric/mtl/operation/assign_mode.hpp>
// #include <boost/numeric/mtl/recursion/recursive_matrix_mult.hpp>

#ifdef __INTEL_COMPILER
#  define MTL_DECL16  __declspec(align(16))
#else
#  define MTL_DECL16
#endif

using namespace mtl;
using namespace mtl::recursion;
using namespace std;

// Maximum time for a single measurement
// is 20 min
const double max_time= 900;

    // Bitmasks:
    const unsigned long morton_mask= generate_mask<true, 0, row_major, 0>::value,
	morton_z_mask= generate_mask<false, 0, row_major, 0>::value,
	doppled_16_row_mask= generate_mask<true, 4, row_major, 0>::value,
	doppled_16_col_mask= generate_mask<true, 4, col_major, 0>::value,
	doppled_32_row_mask= generate_mask<true, 5, row_major, 0>::value,
	doppled_32_col_mask= generate_mask<true, 5, col_major, 0>::value,
	doppled_z_32_row_mask= generate_mask<false, 5, row_major, 0>::value,
	doppled_z_32_col_mask= generate_mask<false, 5, col_major, 0>::value,
	doppled_64_row_mask= generate_mask<true, 6, row_major, 0>::value,
	doppled_64_col_mask= generate_mask<true, 6, col_major, 0>::value,
	doppled_z_64_row_mask= generate_mask<false, 6, row_major, 0>::value,
	doppled_z_64_col_mask= generate_mask<false, 6, col_major, 0>::value,
	doppled_128_row_mask= generate_mask<true, 7, row_major, 0>::value,
	doppled_128_col_mask= generate_mask<true, 7, col_major, 0>::value,
	shark_32_row_mask= generate_mask<true, 5, row_major, 1>::value,
	shark_32_col_mask= generate_mask<true, 5, col_major, 1>::value,
	shark_z_32_row_mask= generate_mask<false, 5, row_major, 1>::value,
	shark_z_32_col_mask= generate_mask<false, 5, col_major, 1>::value,
	shark_64_row_mask= generate_mask<true, 6, row_major, 1>::value,
	shark_64_col_mask= generate_mask<true, 6, col_major, 1>::value,
	shark_z_64_row_mask= generate_mask<false, 6, row_major, 1>::value,
	shark_z_64_col_mask= generate_mask<false, 6, col_major, 1>::value;

typedef assign::plus_sum                            ama_t;

typedef recursion::bound_test_static<32>                    test32_t;
typedef recursion::bound_test_static<64>                    test64_t;

typedef gen_dense_mat_mat_mult_t<assign::plus_sum>  base_mult_t;
typedef gen_recursive_dense_mat_mat_mult_t<base_mult_t>     rec_mult_t;

typedef gen_tiling_22_dense_mat_mat_mult_t<assign::plus_sum>  tiling_22_base_mult_t;
typedef gen_tiling_44_dense_mat_mat_mult_t<assign::plus_sum>  tiling_44_base_mult_t;
typedef gen_tiling_dense_mat_mat_mult_t<2, 4, assign::plus_sum>  tiling_24_base_mult_t;

void print_time_and_mflops(double time, double size)
{
    // time and MFlops of single measure
    std::cout << time << ", " << 2.0 * size * size * size / time / 1e6f << ", ";
}

// Matrices are only placeholder to provide the type
template <typename MatrixA, typename MatrixB, typename MatrixC, typename Mult>
void single_measure(MatrixA&, MatrixB&, MatrixC&, Mult mult, unsigned size, std::vector<int>& enabled, int i)
{
    MatrixA a(size, size);
    MatrixB b(size, size);
    MatrixC c(size, size);
    hessian_setup(a, 1.0);
    hessian_setup(b, 1.0);

    if (enabled[i]) {
	int reps= 0;
	boost::timer start;
	for (; start.elapsed() < 1; reps++)
	    mult(a, b, c);
	double time= start.elapsed() / double(reps);
	print_time_and_mflops(time, a.num_rows());
	if (time > max_time)
	    enabled[i]= 0;
    } else
	std::cout << ", , ";
}

template <typename Functor, typename Result, typename Arg1, typename Arg2, typename Arg3>
struct no_inline3
{
    Result operator()(Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	static Functor* f= new(Functor);
	return apply(f, arg1, arg2, arg3);
    }

    Result apply(Functor* f, Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	return (*f)(arg1, arg2, arg3);
    }
};

#if 0
// Specialization not needed, at least not with g++
template <typename Functor, typename Arg1, typename Arg2, typename Arg3>
struct no_inline3<Functor, void, Arg1, Arg2, Arg3>
{

    void operator()(Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	Functor* f= new(Functor);
	apply(f, arg1, arg2, arg3);
	free(f);
    }

    void apply(Functor* f, Arg1& arg1, Arg2& arg2, Arg3& arg3)
    {
	(*f)(arg1, arg2, arg3);
    }
};
#endif

#if 0
void hybrid_ext_mult_44(const morton_dense<double,  doppled_64_row_mask>& a,
			const morton_dense<double,  doppled_64_col_mask>& b,
			morton_dense<double,  doppled_64_row_mask>& c);
#endif

void dense_ext_mult_44(const dense2D<double>& a,
		       const dense2D<double, mat::parameters<col_major> >& b,
		       dense2D<double>& c);

struct ext_mult_44
{
#if 0
    void operator()(const morton_dense<double,  doppled_64_row_mask>& a,
		    const morton_dense<double,  doppled_64_col_mask>& b,
		    morton_dense<double,  doppled_64_row_mask>& c)
    {
	hybrid_ext_mult_44(a, b, c);
    }
#endif

    void operator()(const dense2D<double>& a,
		    const dense2D<double, mat::parameters<col_major> >& b,
		    dense2D<double>& c)
    {
	dense_ext_mult_44(a, b, c);
    }
};

typedef dense2D<double> rmt;
typedef dense2D<double, mat::parameters<col_major> > cmt;

// C must have even dimensions
template <typename MatrixA, typename MatrixB, typename MatrixC>
void mult_simple_ptu22t(const MatrixA& a, const MatrixB& b, MatrixC& c)
{
    typedef typename MatrixC::value_type  value_type;
    const value_type z= math::zero(c[0][0]);    // if this are matrices we need their size

    set_to_zero(c);

    // Temporary solution; dense matrices need to return const referencens
    MatrixA& aref= const_cast<MatrixA&>(a);
    MatrixB& bref= const_cast<MatrixB&>(b);

    size_t ari= &aref(1, 0) - &aref(0, 0), // how much is the offset of A's entry increased by incrementing row
	aci= &aref(0, 1) - &aref(0, 0), bri= &bref(1, 0) - &bref(0, 0), bci= &bref(0, 1) - &bref(0, 0);

    for (unsigned i= 0; i < c.num_rows(); i+=2)
	for (unsigned k= 0; k < c.num_cols(); k+=2) {
	    int ld= b.num_rows();
	    value_type tmp00= z, tmp01= z, tmp10= z, tmp11= z;

	    MTL_DECL16 const value_type *begin_a= &aref[i][0], *end_a= &aref[i][a.num_cols()];
	    MTL_DECL16 const value_type *begin_b= &bref[0][k];
	    for (; begin_a != end_a; begin_a+= aci, begin_b+= bri) {
		tmp00+= *begin_a * *begin_b;
		tmp01+= *begin_a * *(begin_b+bci);
		tmp10+= *(begin_a+ari) * *begin_b;
		tmp11+= *(begin_a+ari) * *(begin_b+bci);
	    }
	    assign::assign_sum::update(c[i][k], tmp00);
	    assign::assign_sum::update(c[i][k+1], tmp01);
	    assign::assign_sum::update(c[i+1][k], tmp10);
	    assign::assign_sum::update(c[i+1][k+1], tmp11);

#if 0
	    c[i][k]= tmp00; c[i][k+1]= tmp01;
	    c[i+1][k]= tmp10; c[i+1][k+1]= tmp11;
#endif
	}
}

template <typename Matrix, typename MatrixB>
void measure_unrolling(unsigned size, std::vector<int>& enabled, Matrix& matrix, MatrixB& matrixb)
{
    std::cout << size << ", ";

    dense2D<double> dense(4, 4);
    dense2D<double, mat::parameters<col_major> >    denseb(4, 4);

    gen_recursive_dense_mat_mat_mult_t<base_mult_t>           mult;
    gen_recursive_dense_mat_mat_mult_t<tiling_22_base_mult_t> mult_22;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t> mult_44;
    gen_recursive_dense_mat_mat_mult_t<tiling_24_base_mult_t> mult_24;

    typedef gen_tiling_22_dense_mat_mat_mult_ft<Matrix, MatrixB, Matrix, assign::plus_sum>
      tiling_22_t;
    typedef no_inline3<tiling_22_t, void, const Matrix, const MatrixB, Matrix> tiling_22_no_inline_t;

    typedef gen_tiling_44_dense_mat_mat_mult_ft<Matrix, MatrixB, Matrix, assign::plus_sum>
      tiling_44_t;
    typedef no_inline3<tiling_44_t, void, const Matrix, const MatrixB, Matrix> tiling_44_no_inline_t;

    gen_recursive_dense_mat_mat_mult_t<ext_mult_44>           rec_ext_mult_44;

    typedef typename base_case_matrix<Matrix, test64_t>::type    BaseMatrix;
    typedef typename base_case_matrix<MatrixB, test64_t>::type   BaseMatrixB;
    typedef no_inline3<tiling_44_t, void, const BaseMatrix, const BaseMatrixB, BaseMatrix> tiling_44_base_t;
    gen_recursive_dense_mat_mat_mult_t<tiling_44_base_mult_t>    rec_no_inline_mult_44;

    single_measure(matrix, matrixb, matrix, mult, size, enabled, 0);
    single_measure(matrix, matrixb, matrix, mult_22, size, enabled, 1);
    single_measure(matrix, matrixb, matrix, mult_44, size, enabled, 2);
    single_measure(matrix, matrixb, matrix, tiling_22_no_inline_t(), size, enabled, 3);
    single_measure(matrix, matrixb, matrix, tiling_44_no_inline_t(), size, enabled, 4);
    single_measure(matrix, matrixb, matrix, rec_ext_mult_44, size, enabled, 5);
    single_measure(matrix, matrixb, matrix, rec_no_inline_mult_44, size, enabled, 6);
    single_measure(dense, denseb, dense, tiling_22_base_mult_t(), size, enabled, 7);
    single_measure(dense, denseb, dense, tiling_44_base_mult_t(), size, enabled, 8);
    single_measure(dense, denseb, dense, mult_simple_ptu22t<rmt, cmt, rmt>, size, enabled, 9);
    //single_measure(matrix, matrixb, matrix, mult_24, size, enabled, 10);
    std::cout << "0\n";  std::cout.flush();
}

void measure_unrolling_hybrid(unsigned size, std::vector<int>& enabled)
{
    morton_dense<double,  doppled_64_row_mask>     d64r(4, 4);
    morton_dense<double,  doppled_64_col_mask>     d64c(4, 4);
    measure_unrolling(size, enabled, d64r, d64c);
}

void measure_unrolling_dense(unsigned size, std::vector<int>& enabled)
{
    dense2D<double> dense(4, 4);
    dense2D<double, mat::parameters<col_major> >    b(4, 4);
    measure_unrolling(size, enabled, dense, b);
}

template <typename Measure>
void series(unsigned steps, unsigned max_size, Measure measure, const string& comment)
{
    std::cout << "# " << comment << '\n';
    std::cout << "# Gnu-Format size, time, MFlops\n"; std::cout.flush();

    std::vector<int> enabled(16, 1);
    for (unsigned i= steps; i <= max_size; i+= steps)
	measure(i, enabled);
}

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

    std::vector<std::string> scenarii;
    scenarii.push_back(string("Comparing different unrolling for hybrid row-major matrices"));

    using std::cout;
    if (argc < 3) {
	cerr << "usage: recursive_mult_timing  <steps> <max_size>\n";
	exit(1);
    }
    unsigned int steps= atoi(argv[1]), max_size= atoi(argv[2]), size= 32;
    series(steps, max_size, measure_unrolling_hybrid, scenarii[0]);

    return 0;

}


#include <trajopt_utils/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <Eigen/Core>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling_utils.hpp>
#include <trajopt_utils/eigen_conversions.hpp>

#include "trajopt/trajectory_costs.hpp"

namespace
{
/** @brief Returns the difference between each row of a matrixXd and the row before */
static Eigen::MatrixXd diffAxis0(const Eigen::MatrixXd& in)
{
  return in.middleRows(1, in.rows() - 1) - in.middleRows(0, in.rows() - 1);
}
}  // namespace

namespace trajopt
{
//////////// Joint cost functions /////////////////

//////////////////// Position /////////////////////
JointPosEqCost::JointPosEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                               int& first_step, int& last_step)
  : Cost("JointPosEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // pos = x1 - targ
      sco::AffExpr pos;
      sco::exprInc(pos, sco::exprMult(vars(i, j), 1));
      sco::exprDec(pos, targets_[j]);
      // expr_ = coeff * vel^2
      sco::exprInc(expr_, sco::exprMult(sco::exprSquare(pos), coeffs_[j]));
    }
  }
}
double JointPosEqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = (getTraj(xvec, vars_));
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose();
  // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output
  return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum();
}
sco::ConvexObjectivePtr JointPosEqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointPosIneqCost::JointPosIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                                   const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols,
                                   int& first_step, int& last_step)
  : Cost("JointPosIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      // pos = x1 - targ
      sco::AffExpr pos;
      sco::exprInc(pos, sco::exprMult(vars(i, j), 1));
      sco::exprDec(pos, targets_[j]);
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprDec(expr, pos);           // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);  // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprDec(expr_neg, pos);          // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);  // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

double JointPosIneqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols());
  // Subtract targets to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum();
}

sco::ConvexObjectivePtr JointPosIneqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addHinge(expr, 1);
  }
  return out;
}

JointPosEqConstraint::JointPosEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                           const Eigen::VectorXd& targets, int& first_step, int& last_step)
  : EqConstraint("JointPosEq")
  , vars_(vars)
  , coeffs_(coeffs)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // pos = x1 - targ
      sco::AffExpr pos;
      sco::exprInc(pos, sco::exprMult(vars(i, j), 1));
      sco::exprDec(pos, targets_[j]);
      // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO)
      expr_vec_.push_back(sco::exprMult(pos, coeffs_[j]));
    }
  }
}

DblVec JointPosEqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())).rowwise() - targets_.transpose();
  // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector
  return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal());
}
sco::ConvexConstraintsPtr JointPosEqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addEqCnt(expr);
  }
  return out;
}

JointPosIneqConstraint::JointPosIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                               const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols,
                                               const Eigen::VectorXd& lower_tols, int& first_step, int& last_step)
  : IneqConstraint("JointPosIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      // pos = x1 - targ
      sco::AffExpr pos;
      sco::exprInc(pos, sco::exprMult(vars(i, j), 1));
      sco::exprDec(pos, targets_[j]);
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprDec(expr, pos);           // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);  // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form lower limit expr = (upper_tol-(vel-targ))
      sco::exprDec(expr_neg, pos);          // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);  // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

DblVec JointPosIneqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd pos = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()));
  // Subtract targets to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols());
  out << diff1, diff2;
  return util::toDblVec(out);
}

sco::ConvexConstraintsPtr JointPosIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addIneqCnt(expr);
  }
  return out;
}

//////////////////// Velocity /////////////////////
JointVelEqCost::JointVelEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                               int& first_step, int& last_step)
  : Cost("JointVelEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 1; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // vel = (x2 - x1) - targ
      sco::AffExpr vel;
      sco::exprInc(vel, sco::exprMult(vars(i, j), -1));
      sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1));
      exprDec(vel, targets_[j]);
      // expr_ = coeff * vel^2
      exprInc(expr_, exprMult(exprSquare(vel), coeffs_[j]));
    }
  }
}
double JointVelEqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = (getTraj(xvec, vars_));
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() -
                         targets_.transpose();
  // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output
  return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum();
}
sco::ConvexObjectivePtr JointVelEqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointVelIneqCost::JointVelIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                                   const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols,
                                   int& first_step, int& last_step)
  : Cost("JointVelIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 1; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // vel = (x2 - x1) - targ
      sco::AffExpr vel;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(vel, sco::exprMult(vars(i, j), -1));
      sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1));
      sco::exprDec(vel, targets_[j]);  // offset to center about 0

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, vel);             // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form lower limit expr =  (upper_tol-(vel-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, vel);             // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

double JointVelIneqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum();
}

sco::ConvexObjectivePtr JointVelIneqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addHinge(expr, 1);
  }
  return out;
}

JointVelEqConstraint::JointVelEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                           const Eigen::VectorXd& targets, int& first_step, int& last_step)
  : EqConstraint("JointVelEq")
  , vars_(vars)
  , coeffs_(coeffs)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 1; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // vel = (x2 - x1) - targ
      sco::AffExpr vel;
      sco::exprInc(vel, sco::exprMult(vars(i, j), -1));
      sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1));
      sco::exprDec(vel, targets_[j]);
      // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO)
      expr_vec_.push_back(sco::exprMult(vel, coeffs_[j]));
    }
  }
}

DblVec JointVelEqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff = (diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))).rowwise() -
                         targets_.transpose();
  // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector
  return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal());
}
sco::ConvexConstraintsPtr JointVelEqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addEqCnt(expr);
  }
  return out;
}

JointVelIneqConstraint::JointVelIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                               const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols,
                                               const Eigen::VectorXd& lower_tols, int& first_step, int& last_step)
  : IneqConstraint("JointVelIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 1; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // vel = (x2 - x1) - targ
      sco::AffExpr vel;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(vel, sco::exprMult(vars(i, j), -1));
      sco::exprInc(vel, sco::exprMult(vars(i + 1, j), 1));
      sco::exprDec(vel, targets_[j]);  // offset to center about 0

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, vel);             // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form lower limit expr = (lower_tol-(vel-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, vel);             // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

DblVec JointVelIneqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd vel = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (vel.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols());
  out << diff1, diff2;
  return util::toDblVec(out.cwiseMax(0));
}

sco::ConvexConstraintsPtr JointVelIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addIneqCnt(expr);
  }
  return out;
}

//////////////////// Acceleration /////////////////////
JointAccEqCost::JointAccEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                               int& first_step, int& last_step)
  : Cost("JointAccEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 2; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // acc = (x3 - 2*x2 + x1) - targ
      sco::AffExpr acc;
      sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0));

      sco::exprDec(acc, targets_[j]);
      // expr_ = coeff * acc^2
      sco::exprInc(expr_, sco::exprMult(sco::exprSquare(acc), coeffs_[j]));
    }
  }
}
double JointAccEqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = (getTraj(xvec, vars_));
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() -
      targets_.transpose();
  // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output
  return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum();
}
sco::ConvexObjectivePtr JointAccEqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointAccIneqCost::JointAccIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                                   const Eigen::VectorXd& upper_tols, const Eigen::VectorXd& lower_tols,
                                   int& first_step, int& last_step)
  : Cost("JointAccIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 2; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // acc = (x3 - 2*x2 + x1) - targ
      sco::AffExpr acc;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0));
      sco::exprDec(acc, targets_[j]);  // offset to center about 0

      // Form upper limit expr = - (upper_tol-(acc-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, acc);             // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form lower limit expr = (lower_tol-(acc-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, acc);             // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

double JointAccIneqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum();
}

sco::ConvexObjectivePtr JointAccIneqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addHinge(expr, 1);
  }
  return out;
}

JointAccEqConstraint::JointAccEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                           const Eigen::VectorXd& targets, int& first_step, int& last_step)
  : EqConstraint("JointAccEq")
  , vars_(vars)
  , coeffs_(coeffs)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 2; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // acc = (x3 - 2*x2 + x1) - targ
      sco::AffExpr acc;
      sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0));

      sco::exprDec(acc, targets_[j]);  // offset to center about 0
      // expr_ = coeff * vel - Not squared b/c QuadExpr cnt not yet supported (TODO)
      expr_vec_.push_back(sco::exprMult(acc, coeffs_[j]));
    }
  }
}

DblVec JointAccEqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))).rowwise() -
      targets_.transpose();
  // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector
  return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal());
}
sco::ConvexConstraintsPtr JointAccEqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addEqCnt(expr);
  }
  return out;
}

JointAccIneqConstraint::JointAccIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                               const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols,
                                               const Eigen::VectorXd& lower_tols, int& first_step, int& last_step)
  : IneqConstraint("JointAccIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  // Form upper limit expr = - (upper_tol-(vel-targ))
  for (int i = first_step_; i <= last_step_ - 2; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      // acc = (x3 - 2*x2 + x1) - targ
      sco::AffExpr acc;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(acc, sco::exprMult(vars(i, j), 1.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 1, j), -2.0));
      sco::exprInc(acc, sco::exprMult(vars(i + 2, j), 1.0));
      sco::exprDec(acc, targets_[j]);  // offset to center about 0

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, acc);             // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, acc);             // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

DblVec JointAccIneqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols());
  out << diff1, diff2;
  return util::toDblVec(out.cwiseMax(0));
}

sco::ConvexConstraintsPtr JointAccIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addIneqCnt(expr);
  }
  return out;
}

//////////////////// Jerk /////////////////////
JointJerkEqCost::JointJerkEqCost(const VarArray& vars, const Eigen::VectorXd& coeffs, const Eigen::VectorXd& targets,
                                 int& first_step, int& last_step)
  : Cost("JointJerkEq"), vars_(vars), coeffs_(coeffs), targets_(targets), first_step_(first_step), last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 4; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr jerk;
      sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0));

      sco::exprDec(jerk, targets_[j]);
      // expr_ = coeff * jerk^2
      sco::exprInc(expr_, sco::exprMult(sco::exprSquare(jerk), coeffs_[j]));
    }
  }
}
double JointJerkEqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = (getTraj(xvec, vars_));
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))))
          .rowwise() -
      targets_.transpose();
  // Element-wise square it, multiply it by a diagonal matrix of coefficients, and sums output
  return (diff.array().square().matrix() * coeffs_.asDiagonal()).sum();
}
sco::ConvexObjectivePtr JointJerkEqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  out->addQuadExpr(expr_);
  return out;
}

JointJerkIneqCost::JointJerkIneqCost(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                     const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols,
                                     const Eigen::VectorXd& lower_tols, int& first_step, int& last_step)
  : Cost("JointJerkIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 4; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr jerk;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0));
      sco::exprDec(jerk, targets_[j]);

      // Form upper limit expr = - (upper_tol-(jerk-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, jerk);            // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form lower limit expr = (lower_tol-(acc-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, jerk);            // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

double JointJerkIneqCost::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd jerk =
      diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()))));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (jerk.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  return diff1.cwiseMax(0).sum() + diff2.cwiseMax(0).sum();
}

sco::ConvexObjectivePtr JointJerkIneqCost::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexObjectivePtr out(new sco::ConvexObjective(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addHinge(expr, 1);
  }
  return out;
}

JointJerkEqConstraint::JointJerkEqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                             const Eigen::VectorXd& targets, int& first_step, int& last_step)
  : EqConstraint("JointJerkEq")
  , vars_(vars)
  , coeffs_(coeffs)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 4; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr jerk;
      sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0));

      sco::exprDec(jerk, targets_[j]);  // offset to center about 0
      // expr_ = coeff * jerk - Not squared b/c QuadExpr cnt not yet supported (TODO)
      expr_vec_.push_back(sco::exprMult(jerk, coeffs_[j]));
    }
  }
}

DblVec JointJerkEqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows and subtract each row by the targets_ vector
  Eigen::MatrixXd diff =
      (diffAxis0(diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())))))
          .rowwise() -
      targets_.transpose();
  // Squares it, multiplies it by a diagonal matrix of coefficients, and converts to vector
  return util::toDblVec((diff.array().square()).matrix() * coeffs_.asDiagonal());
}
sco::ConvexConstraintsPtr JointJerkEqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addEqCnt(expr);
  }
  return out;
}

JointJerkIneqConstraint::JointJerkIneqConstraint(const VarArray& vars, const Eigen::VectorXd& coeffs,
                                                 const Eigen::VectorXd& targets, const Eigen::VectorXd& upper_tols,
                                                 const Eigen::VectorXd& lower_tols, int& first_step, int& last_step)
  : IneqConstraint("JointJerkIneq")
  , vars_(vars)
  , coeffs_(coeffs)
  , upper_tols_(upper_tols)
  , lower_tols_(lower_tols)
  , targets_(targets)
  , first_step_(first_step)
  , last_step_(last_step)
{
  for (int i = first_step_; i <= last_step_ - 4; ++i)
  {
    for (int j = 0; j < vars.cols(); ++j)
    {
      sco::AffExpr jerk;
      sco::AffExpr expr;
      sco::AffExpr expr_neg;
      sco::exprInc(jerk, sco::exprMult(vars(i, j), -1.0 / 2.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 1, j), 1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 2, j), 0.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 3, j), -1.0));
      sco::exprInc(jerk, sco::exprMult(vars(i + 4, j), 1.0 / 2.0));
      sco::exprDec(jerk, targets_[j]);  // offset to center about 0

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
      sco::exprDec(expr, jerk);            // expr = upper_tol_- (vel - targets_)
      sco::exprScale(expr, -coeffs[j]);    // expr = - (upper_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr);

      // Form upper limit expr = - (upper_tol-(vel-targ))
      sco::exprInc(expr_neg, lower_tols_[j]);  // expr_ = lower_tol_
      sco::exprDec(expr_neg, jerk);            // expr = lower_tol_- (vel - targets_)
      sco::exprScale(expr_neg, coeffs[j]);     // expr = (lower_tol_- (vel - targets_)) * coeffs_
      expr_vec_.push_back(expr_neg);
    }
  }
}

DblVec JointJerkIneqConstraint::value(const DblVec& xvec)
{
  // Convert vector from optimization to trajectory
  Eigen::MatrixXd traj = getTraj(xvec, vars_);
  // Takes diff b/n the subsequent rows to get velocity
  Eigen::MatrixXd acc = diffAxis0(diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols())));
  // Subtract targets_ to center about 0 and then subtract from tolerance
  Eigen::MatrixXd diff0 = (acc.rowwise() - targets_.transpose());
  Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();
  Eigen::MatrixXd diff2 = ((diff0 * -1).rowwise() + lower_tols_.transpose()) * coeffs_.asDiagonal();
  // Applies hinge, multiplies it by a diagonal matrix of coefficients, sums each corresponding value, and converts to
  // vector
  Eigen::MatrixXd out(diff1.rows(), diff1.cols() + diff2.cols());
  out << diff1, diff2;
  return util::toDblVec(out.cwiseMax(0));
}

sco::ConvexConstraintsPtr JointJerkIneqConstraint::convex(const DblVec& /*x*/, sco::Model* model)
{
  sco::ConvexConstraintsPtr out(new sco::ConvexConstraints(model));
  // Add hinge cost. Set the coefficient to 1 here since we include it in the AffExpr already
  // This is necessary since we want a seperate coefficient per joint
  for (sco::AffExpr& expr : expr_vec_)
  {
    out->addIneqCnt(expr);
  }
  return out;
}

}  // namespace trajopt


// experimentalSetup.hpp
#ifndef experimentalSetup
#define experimentalSetup

#include <RcppEigen.h>

#include <ctime>

#include <boost/random/variate_generator.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>

struct InitialPopulationRandomVariates
{
    typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> > interval_uniform_generator;

    std::size_t M;
    boost::mt19937 rng;

    std::vector< interval_uniform_generator > generate_uniform_interval;

    InitialPopulationRandomVariates(const Eigen::VectorXd & numberOfAlleles, const std::size_t & seed);
};

struct RandomVariates
{
    typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> > interval_uniform_generator;

    std::size_t M;
    boost::mt19937 rng;

    boost::random::uniform_real_distribution<> uniform_real;
    boost::random::uniform_int_distribution<> uniform_01;
    boost::random::uniform_int_distribution<> uniform_marker;
    boost::random::uniform_int_distribution<> uniform_unknown_contributor;

    boost::variate_generator<boost::mt19937 &, boost::random::uniform_real_distribution<> > generate_uniform_real;
    boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> > generate_uniform_binary;
    boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> > generate_uniform_marker;
    boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> > generate_uniform_unknown_contributor;
    std::vector< interval_uniform_generator > generate_uniform_mutation;

    RandomVariates(const Eigen::VectorXd & numberOfAlleles, const std::size_t & numberOfUnknownContributors, const std::size_t & seed);
};

class ExperimentalSetup
{
    public:
        // Sample data
        std::size_t NumberOfMarkers;
        Eigen::VectorXd NumberOfAlleles;
        Eigen::VectorXd PartialSumAlleles;

        std::size_t NumberOfContributors;
        Eigen::MatrixXd KnownProfiles;
        std::size_t NumberOfKnownContributors;

        Eigen::MatrixXd AllKnownProfiles;

        Eigen::VectorXd Coverage;
        std::vector< std::vector< Eigen::MatrixXd > > PotentialParents;
        Eigen::VectorXd MarkerImbalances;
        Eigen::VectorXd NoiseParameters;

        // Reference population data
        double Theta;
        Eigen::VectorXd AlleleFrequencies;

        // Estimation input
        Eigen::VectorXd Tolerance;
        double ConvexMarkerImbalanceInterpolation;
        std::size_t LevelsOfStutterRecursion;
        bool DualEstimation;

        ExperimentalSetup(const std::size_t & numberOfMarkers,
                          const Eigen::VectorXd & numberOfAlleles,
                          const std::size_t & numberOfContributors,
                          const std::size_t & numberOfKnownContributors,
                          const Eigen::MatrixXd & knownProfiles,
                          const Eigen::MatrixXd & allKnownProfiles,
                          const Eigen::VectorXd & coverage,
                          const std::vector< std::vector < Eigen::MatrixXd > > & potentialParents,
                          const Eigen::VectorXd & markerImbalances,
                          const double & convexMarkerImbalanceInterpolation,
                          const Eigen::VectorXd & noiseParameters,
                          const Eigen::VectorXd & tolerance,
                          const double & theta,
                          const Eigen::VectorXd & alleleFrequencies,
                          const std::size_t & levelsOfStutterRecursion,
                          const bool & dualEstimation);

        Eigen::VectorXd GenerateUnknownGenotype(InitialPopulationRandomVariates & RV);
};

#endif


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

#include <nt2/linalg/functions/funm.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/dsl/size.hpp>
#include <nt2/sdk/complex/meta/is_complex.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/conj.hpp>
#include <nt2/include/functions/colon.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/divides.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/eyeminus.hpp>
#include <nt2/include/functions/factorial.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/isdiagonal.hpp>
#include <nt2/include/functions/is_finite.hpp>
#include <nt2/include/functions/length.hpp>
#include <nt2/include/functions/linsolve.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/globalmean.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/mnorminf.hpp>
#include <nt2/include/functions/oneplus.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/schur.hpp>
#include <nt2/include/functions/size.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/extent.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <boost/assert.hpp>

// there are many optimisations/ameliorations to be done
// -- passing parameters to functor
// -- fine tuning of types
// -- some code can be shared with logm

namespace nt2
{
  namespace ext
  {
    BOOST_DISPATCH_IMPLEMENT  ( funm_, tag::cpu_
                              , (A0)(A1)
                              , (unspecified_<A0>)
                                (scalar_<unspecified_<A1> >)
                              )
    {
      typedef A1 result_type;
      NT2_FUNCTOR_CALL(2)
      {
        return a0(a1, 0);
      }
    };

    // funm tag only used for functor/matrix
    template<class Domain, int N, class Expr>
    struct size_of<tag::funm_,Domain,N,Expr>
    {
      typedef _2D                                                      result_type;
      BOOST_FORCEINLINE result_type operator()(Expr& e) const
      {
        BOOST_ASSERT_MSG( issquare(boost::proto::child_c<1>(e)),
                          "funm needs a functor and scalar or a ");
        return nt2::extent(boost::proto::child_c<1>(e));
      }
    };

  BOOST_DISPATCH_IMPLEMENT  ( run_assign_, tag::cpu_
                            , (A0)(A1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_<A1, nt2::tag::funm_, boost::mpl::long_<2>, nt2::container::domain>))
                            )
  {
      typedef void                                                    result_type;
      typedef typename A0::value_type                                  value_type;
      typedef typename meta::as_real<value_type>::type                     r_type;
      typedef typename meta::as_complex<r_type>::type                   cplx_type;
      typedef typename meta::as_integer<r_type>::type                      i_type;
      typedef nt2::table<value_type >                                       tab_t;
      typedef nt2::table<r_type>                                           btab_t;
      typedef table<cplx_type>                                             ctab_t;
      typedef table<i_type>                                                itab_t;

      BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1& a1) const
      {
        compute_funm(boost::proto::value(boost::proto::child_c<0>(a1)), boost::proto::child_c<1>(a1), a0);
      }
    private:
      template <class F, class T >
      BOOST_FORCEINLINE static void compute_funm(const F& f, const T& a0, A0& res)
      {
         r_type tol = nt2::Eps<r_type>();
         uint32_t maxterms = 250;
         //u, t and r are complex arrays
         res.resize(extent(a0));
         ctab_t u, t;
         nt2::tie(u, t) = schur(a0, nt2::cmplx_);
         if (isdiagonal(t))
         {
           t = nt2::from_diag(f(nt2::diag_of(t), 0));
           BOOST_AUTO_TPL(r, nt2::mtimes(u, nt2::mtimes(t, nt2::trans(nt2::conj(u)))));
           transtype(res, r, typename nt2::meta::is_complex<value_type>::type());
           return;
         }
         else
         {
           ctab_t r = zeros(extent(a0), meta::as_<cplx_type>()); //is it necessary ?
           r_type delta = 0.1;
           itab_t  ord(nt2::of_size(2u, nt2::size(a0,1)));
           blocking(diag_of(t),delta, ord);
           uint32_t lord = nt2::size(ord, 2);
           ctab_t ca0 = a0;

           for(uint32_t col=1; col <= lord ; ++col)
           {
             BOOST_AUTO_TPL(j, nt2::_(ord(1, col), ord(2, col)));

             itab_t terms(nt2::of_size(1, lord));
             uint32_t nj =  length(j);
             ctab_t rj(nt2::of_size(nj, nj));
             terms(col) = funm_atom(f, a0(j, j), tol, maxterms, rj);
             r(j, j) = rj;
             for(uint32_t row=col-1; row >= 1; --row)
             {
               BOOST_AUTO_TPL(i, nt2::_(ord(1, row), ord(2, row)));
               if (length(i) == 1 && length(j) == 1)
               {
                 size_t ii = i(1), jj = j(1);
                 BOOST_AUTO_TPL(k, nt2::_(ii+1, jj-1));
                 cplx_type temp = ca0(ii,jj)*(r(ii,ii) - r(jj,jj));
                 if (!isempty(k)) temp += mtimes(r(ii,k), ca0(k,jj)) - mtimes(ca0(ii,k), r(k,jj));
                 r(ii,jj) = temp/(ca0(ii,ii)-ca0(jj,jj));
               }
               else
               {
                 itab_t k(nt2::of_size(1, 0));
                 for(uint32_t l = row+1; l < col; ++l)
                 {
                   itab_t k1 = horzcat(k, nt2::_(ord(1, l), ord(2, l)));
                   k = k1;
                 }
                 ctab_t rhs =  mtimes(r(i,i), ca0(i,j)) - mtimes(ca0(i,j), r(j,j));
                 if(!isempty(k)) rhs += mtimes(r(i,k), ca0(k,j)) -  mtimes(ca0(i,k), r(k,j));
                 r(i,j) = sylv_tri(ca0(i,i),-ca0(j,j),rhs);
               }
             }
             ctab_t z =  mtimes(mtimes(u, r), trans(conj(u)));
             transtype(res, z, typename nt2::meta::is_complex<value_type>::type());
           }
         }
       }

      template <class F, class X, class N> static inline
        cplx_type feval(const F & f, const X& x,  const N& k)
      {
        ctab_t xx = x;
        return f(xx, k);
      }

      template < class F, class D1, class D2> static inline
      int32_t funm_atom(const F& f, const D1& t,
                        const r_type& tol,
                        const uint32_t& maxterms,
                        D2& rj)
      {
        // Function of triangular matrix with nearly constant diagonal.
        //   n_terms = funm_atom(f, a0, tol, maxterms, fa0)
        //   evaluates function f at the upper triangular matrix t,
        //   where t has nearly constant diagonal.
        //   a taylor series is used, taking at most maxterms terms.
        //   the function represented by f must have a taylor series with an
        //   infinite radius of convergence.
        //   f(x,k) must return the k'th derivative of
        //   the function represented by fun evaluated at the vector x.
        //   tol is a convergence tolerance for the taylor series, defaulting to eps.
        //   if prnt ~= 0 information is printed on the convergence of the
        //   taylor series evaluation.
        //   n_terms is the number of terms taken in the taylor series.
        //   n_terms  = -1 signals lack of convergence.

        uint32_t n = length(t);
        if (n == 1)
        {
          rj = f(t, 0);
          return 1;
        }
        cplx_type lambda{nt2::globalmean(diag_of(t)),0};
        rj = feval(f, lambda,0)*eye(n, meta::as_<cplx_type>());
        btab_t f_deriv_max = zeros(maxterms+n-1,1, meta::as_<r_type>());
        ctab_t nn = t - lambda*eye(n, meta::as_<cplx_type>());
        r_type mu = mnorminf(linsolve(eyeminus(nt2::abs(triu(t,1))), nt2::ones(n,1, meta::as_<r_type>())));
        ctab_t p = nn;
        uint32_t max_d = 1;
        for(uint32_t k = 1; k <= maxterms; ++k)
        {
          cplx_type fval = feval(f, lambda,k);
          BOOST_ASSERT_MSG(is_finite(fval), "funm infinite derivative");
          ctab_t rj_old = rj;
          rj+= p*fval;
          r_type  rel_diff = nt2::mnorminf(rj - rj_old)/(tol+mnorminf(rj_old));
          p = mtimes(p, nn)/r_type(oneplus(k));
          if (rel_diff <= tol)
          {
            // Approximate the maximum of derivatives in convex set containing
            // eigenvalues by maximum of derivatives at eigenvalues.
            for(uint32_t j = max_d; j <=  k+n-1; ++j)
            {
              f_deriv_max(j) = nt2::mnorminf(f(nt2::diag_of(t),j));
            }
            max_d = k+n;
            r_type omega = nt2::Zero<r_type>();
            for(uint32_t j = 0; j <= n-1; ++j)
            {
              omega = nt2::max(omega,f_deriv_max(k+j)/nt2::factorial(j));
            }
            r_type trunc = nt2::mnorminf(p)*mu*omega;
            if (trunc <= tol*mnorminf(rj))
            {
              return k+1;
            }
          }
        }
        return -1;
      }

      template < class T1, class T2 >
      BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::false_ const &)
      {
        r =  real(z);
      }

      template < class T1, class T2 >
        BOOST_FORCEINLINE static void transtype(T1& r, T2& z, boost::mpl::true_ const &)
      {
        r =  z;
      }
      template < class D> static inline void blocking(const D& a0, const r_type& delta, itab_t & ord)
      {
        uint32_t n = nt2::size(a0, 1);
        uint32_t j = 1;
        ord(1, 1) = 1;
        for(uint32_t i=2; i <= n; ++i)
        {
          if (nt2::abs(a0(i-1)-a0(i)) >= delta)
          {
            ord(2, j) = i-1;
            ord(1, ++j) = i;
          }
        }
        ord(2, j) = n;
        ord.resize(nt2::of_size(2u, j));
      }
      template < class T, class U, class B>
        static inline ctab_t sylv_tri(const T& t,const U& u, const B& b)
      {

        // sylv_tri    solve triangular sylvester equation.
        //    x = sylv_tri(t,u,b) solves the sylvester equation
        //  t*x + x*u = b, where t and u are square upper triangular matrices.

        uint32_t m = length(t);
        uint32_t n = length(u);
        ctab_t x = zeros(m,n, nt2::meta::as_<cplx_type>());
        for(uint32_t i = 1;  i <= n; ++i)
        {
          ctab_t bb = b(nt2::_,i);
          BOOST_AUTO_TPL(ii, nt2::_(1u, i-1));
          if(!isempty(ii)) bb -= mtimes(x(nt2::_,ii),u(ii,i));
          x(nt2::_,i) = linsolve(t + u(i,i)*eye(m, meta::as_<cplx_type>()),
                                 bb);
        }
        return x;
     }
    };

  }
}

#endif


// HEADER
#include <path_follower/controller/robotcontroller_OFC.h>

// THIRD PARTY
#include <visualization_msgs/MarkerArray.h>

// PROJECT
#include <path_follower/parameters/path_follower_parameters.h>
#include <path_follower/utils/cubic_spline_interpolation.h>
#include <cslibs_navigation_utilities/MathHelper.h>
#include <path_follower/utils/pose_tracker.h>
#include <path_follower/utils/visualizer.h>
#include <path_follower/utils/obstacle_cloud.h>
#include <path_follower/collision_avoidance/collision_avoider.h>

// SYSTEM
#include <cmath>
#include <deque>
#include <boost/algorithm/clamp.hpp>
#include <pcl_ros/point_cloud.h>
#include <pluginlib/class_list_macros.h>

#include <path_follower/factory/controller_factory.h>

REGISTER_ROBOT_CONTROLLER(RobotController_OFC, OFC, default_collision_avoider);

using namespace Eigen;

RobotController_OFC::RobotController_OFC():
    RobotController(),
    cmd_(this),
    vn_(0.0),
    Ts_(0.02),
    e_prev_lin_(0.0),
    e_prev_ang_(0.0),
    e_sum_lin_(0.0),
    e_sum_ang_(0.0),
    counter_(1),
    mean_vel_(0.0)
{
    T_prev_ = ros::Time::now();
    previous_t_ = ros::Time::now();
    meas_velocities_ = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
    last_movement_ = ros::Time::now();
    target_vec_prev_ = tf::Vector3(0,0,0);

    goal_pub = nh_.advertise<geometry_msgs::PointStamped>("goal_position", 0);
    target_pub = nh_.advertise<geometry_msgs::PointStamped>("target_position", 0);
    err_vec_pub = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
}

void RobotController_OFC::stopMotion()
{

    cmd_.speed = 0;
    cmd_.direction_angle = 0;
    cmd_.rotation = 0;

    MoveCommand mcmd = cmd_;
    publishMoveCommand(mcmd);
}

void RobotController_OFC::initialize()
{
    RobotController::initialize();

    // desired velocity
    vn_ = std::min(PathFollowerParameters::getInstance()->max_velocity(), velocity_);
    ROS_DEBUG_STREAM("velocity_: " << velocity_ << ", vn: " << vn_);

    //reset the PID variables
    e_sum_lin_ = 0.0;
    e_sum_ang_ = 0.0;
    T_prev_ = ros::Time::now();

}

RobotController::MoveCommandStatus RobotController_OFC::computeMoveCommand(MoveCommand *cmd)
{
    // omni drive can rotate.
    *cmd = MoveCommand(true);

    ///get the person's velocity
    tf::Transform target_tf_robot = pose_tracker_->getTransform(pose_tracker_->getRobotFrameId(), "target", ros::Time::now(), ros::Duration(0.01));
    tf::Vector3 target_vec = target_tf_robot.getOrigin();

    if(std::abs(target_vec.length() - target_vec_prev_.length()) > 1e-1){
        ros::Time current_t = ros::Time::now();
        double dt = (current_t - previous_t_).toSec();
        previous_t_ = current_t;
        tf::Vector3 target_v_vec = (target_vec - target_vec_prev_)/std::max(1e-5,dt);
        target_vec_prev_ = target_vec;
        double target_v = target_v_vec.length()  <= vn_ ? target_v_vec.length() : vn_;

        //calculate mean value using the "meas_velocities.size()" last measured velocities
        //e.g. if meas_velocities_ = {0, 0}, the mean over the last two values is used, i.e. mean_vel_ = (i[t] + i[t-1])/2.0
        meas_velocities_[counter_] = target_v;
        mean_vel_ = 0.0;
        int meas_num = meas_velocities_.size();
        for(double i : meas_velocities_){
            mean_vel_ += i;
            if(i < 1e-5){
                meas_num--;
            }
        }
        mean_vel_ /= meas_num;

        if(counter_ == ((int) meas_velocities_.size()) - 1){
            counter_ = -1;
        }
        counter_++;

        last_movement_ = ros::Time::now();

    }
    ///***///

    Eigen::Vector2d rob2goal(opt_.goal_x(), opt_.goal_y());
    Eigen::Vector2d rob2target(target_vec.getX(), target_vec.getY());
    Eigen::Vector2d goal2target = rob2target - rob2goal;

    //publish the desired goal position
    geometry_msgs::PointStamped goal_pose;
    goal_pose.point.x = opt_.goal_x();
    goal_pose.point.y = opt_.goal_y();
    goal_pose.header.frame_id = pose_tracker_->getRobotFrameId();
    goal_pub.publish(goal_pose);
    //***//

    //publish the target (person) position
    geometry_msgs::PointStamped target_pose;
    target_pose.point.x = target_vec.getX();
    target_pose.point.y = target_vec.getY();
    target_pose.header.frame_id = pose_tracker_->getRobotFrameId();
    target_pub.publish(target_pose);
    //***//

    //publish the error vector
    visualization_msgs::Marker err_vec_marker;
    err_vec_marker.header.frame_id = pose_tracker_->getRobotFrameId();
    err_vec_marker.header.stamp = ros::Time();
    err_vec_marker.ns = "error_vector";
    err_vec_marker.id = 4;
    err_vec_marker.type = visualization_msgs::Marker::ARROW;
    err_vec_marker.action = visualization_msgs::Marker::ADD;

    err_vec_marker.pose.position.x = opt_.goal_x();
    err_vec_marker.pose.position.y = opt_.goal_y();
    err_vec_marker.pose.position.z = 0.0;
    tf::Quaternion quaternion = tf::createQuaternionFromYaw(std::atan2(goal2target[1], goal2target[0]));
    err_vec_marker.pose.orientation.x = quaternion.getX();
    err_vec_marker.pose.orientation.y = quaternion.getY();
    err_vec_marker.pose.orientation.z = quaternion.getZ();
    err_vec_marker.pose.orientation.w = quaternion.getW();
    err_vec_marker.scale.x = goal2target.norm();
    err_vec_marker.scale.y = 0.1f;
    err_vec_marker.scale.z = 0.1f;
    err_vec_marker.color.a = 1.0f;
    err_vec_marker.color.r = 0.0f;
    err_vec_marker.color.g = 1.0f;
    err_vec_marker.color.b = 0.0f;

    err_vec_pub.publish(err_vec_marker);
    //***//

    if(mean_vel_ < 1e-3){
        cmd_.speed = 0;
        cmd_.direction_angle = 0;
        cmd_.rotation = 0;

        *cmd = cmd_;

        double distance_to_goal_eucl = goal2target.norm();

        ROS_INFO_THROTTLE(1, "Final positioning error: %f m, %f deg", distance_to_goal_eucl, MathHelper::Angle(rob2goal, rob2target)*180./M_PI);

        return RobotController::MoveCommandStatus::REACHED_GOAL;
    }

    double Ta = std::max(1e-3, (ros::Time::now() - T_prev_).toSec());
    double e_lin = goal2target.norm();
    double e_ang = std::atan2(rob2target[1] - rob2goal[1], rob2target[0] - rob2goal[0]);
    e_sum_lin_ += e_lin*Ta;
    e_sum_ang_ += e_ang*Ta;
    double v = opt_.kp_lin()*e_lin + opt_.ki_lin()*e_sum_lin_ + opt_.kd_lin()*(e_lin - e_prev_lin_)/Ta;
    double w = opt_.kp_ang()*e_ang + opt_.ki_ang()*e_sum_ang_ + opt_.kd_ang()*(e_ang - e_prev_ang_)/Ta;
    T_prev_ = ros::Time::now();
    e_prev_lin_ = e_lin;
    e_prev_ang_ = e_ang;

    cmd_.speed = boost::algorithm::clamp(v, 0.0, PathFollowerParameters::getInstance()->max_velocity());
    cmd_.rotation = boost::algorithm::clamp(w, -opt_.max_angular_velocity(), opt_.max_angular_velocity());

    *cmd = cmd_;

    ///***///

    return MoveCommandStatus::OKAY;
}

void RobotController_OFC::publishMoveCommand(const MoveCommand &cmd) const
{
    geometry_msgs::Twist msg;
    msg.linear.x  = cmd.getVelocity();
    msg.linear.y  = 0;
    msg.angular.z = cmd.getRotationalVelocity();

    cmd_pub_.publish(msg);
}