#include "meetup.h"
#include <boost/date_time/gregorian/gregorian.hpp>
#ifdef EXERCISM_TEST_SUITE
#include <catch2/catch.hpp>
#else
#include "test/catch.hpp"
#endif

// This problem requires you to install and use the boost date_time library.
// CMake will try to find and configure it for you if it is installed on your
// system.
// See <http://www.boost.org/doc/libs/1_58_0/doc/html/date_time.html> for
// documentation on boost::gregorian::date

TEST_CASE("monteenth_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 13};
    REQUIRE(expected == meetup.monteenth());
}

#if defined(EXERCISM_RUN_ALL_TESTS)
TEST_CASE("monteenth_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 19};
    REQUIRE(expected == meetup.monteenth());
}

TEST_CASE("monteenth_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 16};
    REQUIRE(expected == meetup.monteenth());
}

TEST_CASE("tuesteenth_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 19};
    REQUIRE(expected == meetup.tuesteenth());
}

TEST_CASE("tuesteenth_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 16};
    REQUIRE(expected == meetup.tuesteenth());
}

TEST_CASE("tuesteenth_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 13};
    REQUIRE(expected == meetup.tuesteenth());
}

TEST_CASE("wednesteenth_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 16};
    REQUIRE(expected == meetup.wednesteenth());
}

TEST_CASE("wednesteenth_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 13};
    REQUIRE(expected == meetup.wednesteenth());
}

TEST_CASE("wednesteenth_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 19};
    REQUIRE(expected == meetup.wednesteenth());
}

TEST_CASE("thursteenth_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 16};
    REQUIRE(expected == meetup.thursteenth());
}

TEST_CASE("thursteenth_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 13};
    REQUIRE(expected == meetup.thursteenth());
}

TEST_CASE("thursteenth_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 19};
    REQUIRE(expected == meetup.thursteenth());
}

TEST_CASE("friteenth_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 19};
    REQUIRE(expected == meetup.friteenth());
}

TEST_CASE("friteenth_of_Aug_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 16};
    REQUIRE(expected == meetup.friteenth());
}

TEST_CASE("friteenth_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 13};
    REQUIRE(expected == meetup.friteenth());
}

TEST_CASE("saturteenth_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 16};
    REQUIRE(expected == meetup.saturteenth());
}

TEST_CASE("saturteenth_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 19};
    REQUIRE(expected == meetup.saturteenth());
}

TEST_CASE("sunteenth_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 19};
    REQUIRE(expected == meetup.sunteenth());
}

TEST_CASE("sunteenth_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 16};
    REQUIRE(expected == meetup.sunteenth());
}

TEST_CASE("sunteenth_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 13};
    REQUIRE(expected == meetup.sunteenth());
}

TEST_CASE("first_Monday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 4};
    REQUIRE(expected == meetup.first_monday());
}

TEST_CASE("first_Monday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 1};
    REQUIRE(expected == meetup.first_monday());
}

TEST_CASE("first_Tuesday_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 7};
    REQUIRE(expected == meetup.first_tuesday());
}

TEST_CASE("first_Tuesday_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 4};
    REQUIRE(expected == meetup.first_tuesday());
}

TEST_CASE("first_Wednesday_of_July_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jul, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jul, 3};
    REQUIRE(expected == meetup.first_wednesday());
}

TEST_CASE("first_Wednesday_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 7};
    REQUIRE(expected == meetup.first_wednesday());
}

TEST_CASE("first_Thursday_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 5};
    REQUIRE(expected == meetup.first_thursday());
}

TEST_CASE("first_Thursday_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 3};
    REQUIRE(expected == meetup.first_thursday());
}

TEST_CASE("first_Friday_of_November_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Nov, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Nov, 1};
    REQUIRE(expected == meetup.first_friday());
}

TEST_CASE("first_Friday_of_December_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Dec, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Dec, 6};
    REQUIRE(expected == meetup.first_friday());
}

TEST_CASE("first_Saturday_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 5};
    REQUIRE(expected == meetup.first_saturday());
}

TEST_CASE("first_Saturday_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 2};
    REQUIRE(expected == meetup.first_saturday());
}

TEST_CASE("first_Sunday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 3};
    REQUIRE(expected == meetup.first_sunday());
}

TEST_CASE("first_Sunday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 7};
    REQUIRE(expected == meetup.first_sunday());
}

TEST_CASE("second_Monday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 11};
    REQUIRE(expected == meetup.second_monday());
}

TEST_CASE("second_Monday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 8};
    REQUIRE(expected == meetup.second_monday());
}

TEST_CASE("second_Tuesday_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 14};
    REQUIRE(expected == meetup.second_tuesday());
}

TEST_CASE("second_Tuesday_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 11};
    REQUIRE(expected == meetup.second_tuesday());
}

TEST_CASE("second_Wednesday_of_July_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jul, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jul, 10};
    REQUIRE(expected == meetup.second_wednesday());
}

TEST_CASE("second_Wednesday_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 14};
    REQUIRE(expected == meetup.second_wednesday());
}

TEST_CASE("second_Thursday_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 12};
    REQUIRE(expected == meetup.second_thursday());
}

TEST_CASE("second_Thursday_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 10};
    REQUIRE(expected == meetup.second_thursday());
}

TEST_CASE("second_Friday_of_November_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Nov, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Nov, 8};
    REQUIRE(expected == meetup.second_friday());
}

TEST_CASE("second_Friday_of_December_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Dec, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Dec, 13};
    REQUIRE(expected == meetup.second_friday());
}

TEST_CASE("second_Saturday_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 12};
    REQUIRE(expected == meetup.second_saturday());
}

TEST_CASE("second_Saturday_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 9};
    REQUIRE(expected == meetup.second_saturday());
}

TEST_CASE("second_Sunday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 10};
    REQUIRE(expected == meetup.second_sunday());
}

TEST_CASE("second_Sunday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 14};
    REQUIRE(expected == meetup.second_sunday());
}

TEST_CASE("third_Monday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 18};
    REQUIRE(expected == meetup.third_monday());
}

TEST_CASE("third_Monday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 15};
    REQUIRE(expected == meetup.third_monday());
}

TEST_CASE("third_Tuesday_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 21};
    REQUIRE(expected == meetup.third_tuesday());
}

TEST_CASE("third_Tuesday_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 18};
    REQUIRE(expected == meetup.third_tuesday());
}

TEST_CASE("third_Wednesday_of_July_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jul, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jul, 17};
    REQUIRE(expected == meetup.third_wednesday());
}

TEST_CASE("third_Wednesday_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 21};
    REQUIRE(expected == meetup.third_wednesday());
}

TEST_CASE("third_Thursday_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 19};
    REQUIRE(expected == meetup.third_thursday());
}

TEST_CASE("third_Thursday_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 17};
    REQUIRE(expected == meetup.third_thursday());
}

TEST_CASE("third_Friday_of_November_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Nov, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Nov, 15};
    REQUIRE(expected == meetup.third_friday());
}

TEST_CASE("third_Friday_of_December_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Dec, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Dec, 20};
    REQUIRE(expected == meetup.third_friday());
}

TEST_CASE("third_Saturday_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 19};
    REQUIRE(expected == meetup.third_saturday());
}

TEST_CASE("third_Saturday_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 16};
    REQUIRE(expected == meetup.third_saturday());
}

TEST_CASE("third_Sunday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 17};
    REQUIRE(expected == meetup.third_sunday());
}

TEST_CASE("third_Sunday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 21};
    REQUIRE(expected == meetup.third_sunday());
}

TEST_CASE("fourth_Monday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 25};
    REQUIRE(expected == meetup.fourth_monday());
}

TEST_CASE("fourth_Monday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 22};
    REQUIRE(expected == meetup.fourth_monday());
}

TEST_CASE("fourth_Tuesday_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 28};
    REQUIRE(expected == meetup.fourth_tuesday());
}

TEST_CASE("fourth_Tuesday_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 25};
    REQUIRE(expected == meetup.fourth_tuesday());
}

TEST_CASE("fourth_Wednesday_of_July_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jul, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jul, 24};
    REQUIRE(expected == meetup.fourth_wednesday());
}

TEST_CASE("fourth_Wednesday_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 28};
    REQUIRE(expected == meetup.fourth_wednesday());
}

TEST_CASE("fourth_Thursday_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 26};
    REQUIRE(expected == meetup.fourth_thursday());
}

TEST_CASE("fourth_Thursday_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 24};
    REQUIRE(expected == meetup.fourth_thursday());
}

TEST_CASE("fourth_Friday_of_November_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Nov, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Nov, 22};
    REQUIRE(expected == meetup.fourth_friday());
}

TEST_CASE("fourth_Friday_of_December_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Dec, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Dec, 27};
    REQUIRE(expected == meetup.fourth_friday());
}

TEST_CASE("fourth_Saturday_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 26};
    REQUIRE(expected == meetup.fourth_saturday());
}

TEST_CASE("fourth_Saturday_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 23};
    REQUIRE(expected == meetup.fourth_saturday());
}

TEST_CASE("fourth_Sunday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 24};
    REQUIRE(expected == meetup.fourth_sunday());
}

TEST_CASE("fourth_Sunday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 28};
    REQUIRE(expected == meetup.fourth_sunday());
}

TEST_CASE("last_Monday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 25};
    REQUIRE(expected == meetup.last_monday());
}

TEST_CASE("last_Monday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 29};
    REQUIRE(expected == meetup.last_monday());
}

TEST_CASE("last_Tuesday_of_May_2013")
{
    const meetup::scheduler meetup{boost::gregorian::May, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::May, 28};
    REQUIRE(expected == meetup.last_tuesday());
}

TEST_CASE("last_Tuesday_of_June_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jun, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jun, 25};
    REQUIRE(expected == meetup.last_tuesday());
}

TEST_CASE("last_Wednesday_of_July_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jul, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jul, 31};
    REQUIRE(expected == meetup.last_wednesday());
}

TEST_CASE("last_Wednesday_of_August_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Aug, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Aug, 28};
    REQUIRE(expected == meetup.last_wednesday());
}

TEST_CASE("last_Thursday_of_September_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Sep, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Sep, 26};
    REQUIRE(expected == meetup.last_thursday());
}

TEST_CASE("last_Thursday_of_October_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Oct, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Oct, 31};
    REQUIRE(expected == meetup.last_thursday());
}

TEST_CASE("last_Friday_of_November_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Nov, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Nov, 29};
    REQUIRE(expected == meetup.last_friday());
}

TEST_CASE("last_Friday_of_December_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Dec, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Dec, 27};
    REQUIRE(expected == meetup.last_friday());
}

TEST_CASE("last_Saturday_of_January_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Jan, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Jan, 26};
    REQUIRE(expected == meetup.last_saturday());
}

TEST_CASE("last_Saturday_of_February_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Feb, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Feb, 23};
    REQUIRE(expected == meetup.last_saturday());
}

TEST_CASE("last_Sunday_of_March_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Mar, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Mar, 31};
    REQUIRE(expected == meetup.last_sunday());
}

TEST_CASE("last_Sunday_of_April_2013")
{
    const meetup::scheduler meetup{boost::gregorian::Apr, 2013};

    const boost::gregorian::date expected{2013, boost::gregorian::Apr, 28};
    REQUIRE(expected == meetup.last_sunday());
}
#endif


//
// Copyright 2013 Christian Henning
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#include <boost/gil.hpp>
#include <boost/gil/extension/toolbox/color_converters/gray_to_rgba.hpp>

#include <boost/core/lightweight_test.hpp>

namespace gil = boost::gil;

void test_gray_to_rgba()
{
    gil::gray8_pixel_t a(45);
    gil::rgba8_pixel_t b;
    gil::color_convert(a, b);
    BOOST_TEST(b == gil::rgba8_pixel_t(45, 45, 45, 255));
}

int main()
{
    test_gray_to_rgba();

    return boost::report_errors();
}


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================
#include <nt2/include/functions/round2even.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/half.hpp>
#include <nt2/include/constants/mhalf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( round2even_real,  BOOST_SIMD_REAL_TYPES)
{
  using nt2::round2even;
  using nt2::tag::round2even_;
  typedef typename std::complex<T> cT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(round2even(nt2::Inf<cT>()), nt2::Inf<cT>());
  NT2_TEST_EQUAL(round2even(nt2::Minf<cT>()), nt2::Minf<cT>());
  NT2_TEST_EQUAL(round2even(nt2::Nan<cT>()), nt2::Nan<cT>());
#endif
  NT2_TEST_EQUAL(round2even(cT(T(-1.1))), cT(-1));
  NT2_TEST_EQUAL(round2even(cT(T(1.1))), cT(1));
  NT2_TEST_EQUAL(round2even(cT(T(-1.1), T(1.1))), cT(-1, 1));
  NT2_TEST_EQUAL(round2even(cT(T(1.4))), cT(1));
  NT2_TEST_EQUAL(round2even(cT(T(1.5))), cT(2));
  NT2_TEST_EQUAL(round2even(cT(T(1.6))), cT(2));
  NT2_TEST_EQUAL(round2even(cT(T(2.5))), cT(2));
  NT2_TEST_EQUAL(round2even(nt2::Half<cT>()), nt2::Zero<cT>());
  NT2_TEST_EQUAL(round2even(nt2::Mhalf<cT>()), nt2::Zero<cT>());
  NT2_TEST_EQUAL(round2even(nt2::Mone<cT>()), nt2::Mone<cT>());
  NT2_TEST_EQUAL(round2even(nt2::One<cT>()), nt2::One<cT>());
  NT2_TEST_EQUAL(round2even(nt2::Zero<cT>()), nt2::Zero<cT>());
}


// NOTE:   May 31, 2006
// gcc 4.1 has problem handling this function as it was part of interfaces.cpp;
// initial investigation points to some interplay among the header files;
// the current include-order works, while the following order generates lots
// of errors in compiling:
//       #include "RBGL.hpp"
//       #include <boost/graph/transitive_closure.hpp>
//       #include <boost/graph/graphviz.hpp>
// It's a work-around to separate this function from the rest and include header
// files differently.
// NOTE:   Jun 1, 2006
// The problem seems in
//       #include <boost/graph/graphviz.hpp>
// since we don't need it, we remove it from RBGL.hpp, everything seems ok.

#include "RBGL.hpp"
#include <boost/graph/transitive_closure.hpp>

extern "C"
{
    SEXP BGL_transitive_closure_D (SEXP num_verts_in,
    		SEXP num_edges_in, SEXP R_edges_in )
    {
    	using namespace boost;

        Graph_dd g(num_verts_in, num_edges_in, R_edges_in );
        Graph_dd TC;

    	transitive_closure(g, TC);

        SEXP ansList, eList, vList;
        PROTECT(ansList = allocVector(VECSXP,2));
        PROTECT(vList = allocMatrix(INTSXP, 1, num_vertices(TC)));
        PROTECT(eList = allocMatrix(INTSXP, 2, num_edges(TC)));

        Graph_dd::vertex_iterator vi, v_end;
        int i = 0;
        for (i = 0, tie(vi, v_end) = vertices(TC); vi != v_end; ++vi)
	{
		INTEGER(vList)[i++] = *vi;
	}

        Graph_dd::edge_iterator ei, e_end;
        for (i = 0, tie(ei, e_end) = edges(TC); ei != e_end; ++ei)
        {
            INTEGER(eList)[i++] = source(*ei, TC);
            INTEGER(eList)[i++] = target(*ei, TC);
        }

        SET_VECTOR_ELT(ansList,0,vList);
        SET_VECTOR_ELT(ansList,1,eList);
        UNPROTECT(3);
        return(ansList);

    }

}


#include <NTL/quad_float.h>

NTL_CLIENT

int main()
{
   quad_float a, b, c, d;

   quad_float::SetOutputPrecision(25);

   long pok;
   double one = 1.0;
   quad_float_PrecisionOK(pok, one);
   if (pok)
      cout << "Precision OK\n";
   else
      cout << "Precision not OK\n";

   cin >> a;
   cout << a << "\n";

   cin >> b;
   cout << b << "\n";

   c = a + b;
   d = a;
   d += b;
   cout << c << "\n";
   cout << d << "\n";

   c = a - b;
   d = a;
   d -= b;
   cout << c << "\n";
   cout << d << "\n";

   c = a * b;
   d = a;
   d *= b;
   cout << c << "\n";
   cout << d << "\n";

   c = a / b;
   d = a;
   d /= b;
   cout << c << "\n";
   cout << d << "\n";

   c = -a;
   cout << c << "\n";

   c = sqrt(a);
   cout << c << "\n";

   power(c, to_quad_float(10), 20);
   cout << c << "\n";

   {

   long n, n1;
   int shamt = min(NTL_BITS_PER_LONG,2*NTL_DOUBLE_PRECISION);

   n = to_long((1UL << (shamt-1)) - 1UL);
   c = to_quad_float(n);
   n1 = to_long(c);

   if (n1 == n)
      cout << "long conversion OK\n";
   else
      cout << "long conversion not OK\n";

   n = to_long(1UL << (shamt-1));
   c = to_quad_float(n);
   n1 = to_long(c);

   if (n1 == n)
      cout << "long conversion OK\n";
   else
      cout << "long conversion not OK\n";

   }

   {

   unsigned long n;
   ZZ n1;
   int shamt = min(NTL_BITS_PER_LONG,2*NTL_DOUBLE_PRECISION);

   n = (1UL << (shamt-1)) - 1UL;
   c = to_quad_float(n);
   n1 = to_ZZ(c);

   if (n1 == to_ZZ(n))
      cout << "ulong conversion OK\n";
   else
      cout << "ulong conversion not OK\n";

   n = 1UL << (shamt-1);
   c = to_quad_float(n);
   n1 = to_ZZ(c);

   if (n1 == to_ZZ(n))
      cout << "ulong conversion OK\n";
   else
      cout << "ulong conversion not OK\n";

   }

}


#include "PyInlineReductions.h"

#include <boost/python.hpp>
#include <string>

#include "Halide.h"

#include "PyExpr.h"

namespace h = Halide;
namespace p = boost::python;

h::Expr sum0(h::Expr e, const std::string name) {
    return h::sum(e, name);
}

h::Expr sum1(h::RDom r, h::Expr e, const std::string name) {
    return h::sum(r, e, name);
}

h::Expr product0(h::Expr e, const std::string name) {
    return h::product(e, name);
}

h::Expr product1(h::RDom r, h::Expr e, const std::string name) {
    return h::product(r, e, name);
}

h::Expr maximum0(h::Expr e, const std::string name) {
    return h::maximum(e, name);
}

h::Expr maximum1(h::RDom r, h::Expr e, const std::string name) {
    return h::maximum(r, e, name);
}

h::Expr minimum0(h::Expr e, const std::string name) {
    return h::minimum(e, name);
}

h::Expr minimum1(h::RDom r, h::Expr e, const std::string name) {
    return h::minimum(r, e, name);
}

p::object argmin0(h::Expr e, const std::string name) {
    return expr_vector_to_python_tuple(h::argmin(e, name).as_vector());
}

p::object argmin1(h::RDom r, h::Expr e, const std::string name) {
    return expr_vector_to_python_tuple(h::argmin(r, e, name).as_vector());
}

p::object argmax0(h::Expr e, const std::string name) {
    return expr_vector_to_python_tuple(h::argmin(e, name).as_vector());
}

p::object argmax1(h::RDom r, h::Expr e, const std::string name) {
    return expr_vector_to_python_tuple(h::argmax(r, e, name).as_vector());
}

void define_inline_reductions() {
    // Defines some inline reductions: sum, product, minimum, maximum.

    p::def("sum", &sum0, (p::arg("e"), p::arg("name") = "sum"),
           "An inline reduction.");
    p::def("sum", &sum1, (p::arg("r"), p::arg("e"), p::arg("name") = "sum"),
           "An inline reduction.");

    p::def("product", &product0, (p::arg("e"), p::arg("name") = "product"),
           "An inline reduction.");
    p::def("product", &product1, (p::arg("r"), p::arg("e"), p::arg("name") = "product"),
           "An inline reduction.");

    p::def("maximum", &maximum0, (p::arg("e"), p::arg("name") = "maximum"),
           "An inline reduction.");
    p::def("maximum", &maximum1, (p::arg("r"), p::arg("e"), p::arg("name") = "maximum"),
           "An inline reduction.");

    p::def("minimum", &minimum0, (p::arg("e"), p::arg("name") = "minimum"),
           "An inline reduction.");
    p::def("minimum", &minimum1, (p::arg("r"), p::arg("e"), p::arg("name") = "minimum"),
           "An inline reduction.");

    p::def("argmin", &argmin0, (p::arg("e"), p::arg("name") = "argmin"),
           "An inline reduction.");
    p::def("argmin", &argmin1, (p::arg("r"), p::arg("e"), p::arg("name") = "argmin"),
           "An inline reduction.");

    p::def("argmax", &argmax0, (p::arg("e"), p::arg("name") = "argmax"),
           "An inline reduction.");
    p::def("argmax", &argmax1, (p::arg("r"), p::arg("e"), p::arg("name") = "argmax"),
           "An inline reduction.");
}


/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2013, The University of Texas at Dallas
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of The University of Texas at Dallas nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 * Filename: connectivity.cpp
 *
 * Description: This file contains the ROS node that calculates the control variables
 * for a collection of iRobot Creates.
 *
 * Log
 * ----
 * 2013-09-15 File created by Hazen Eckert
 *
 */

// ROS includes
#include "ros/ros.h"
#include "ros/assert.h"
#include "dynamic_reconfigure/server.h"
#include "create_driver/vicon_driver.h"

// Library includes
#include <string>
#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/LU>

// Local Package includes
#include "connectivity_controller/ConnectivityMsgs.h"
#include "connectivity_controller/ConnectivityVariablesConfig.h"

using namespace std;
using namespace Eigen;

#define rho1 0.7
#define rho2 2.3

// Collision avoidance variables
double R = 0.7;
double r = 0.33;

double psi( double x )
{
	if ( x <= rho1 ) {
		return 1;
	} else if ( rho1 < x && x < rho2 ) {
		return ( exp(-1/(rho2-x)) / ( exp(-1/(rho2-x)) + exp(1/(rho1-x)) ) );
	} else if ( rho2 <= x ) {
		return 0;
	}
	return 0;
}

double psiPrime( double d )
{
	if ( d <= rho1 ) {
		return 0;
	} else if ( rho1 < d && d < rho2 ) {
		return (-0.5)*( pow((d-rho1),(-2))+pow((d-rho2),(-2)) ) / (1+  cosh(  (1/(-d+rho1))+(1/(-d+rho2)) )   );
	} else if ( rho2 <= d ) {
		return 0;
	}
	return 0;
}

void reconfigureCallback(connectivity_controller::ConnectivityVariablesConfig &config, uint32_t level) {
	R = config.r_max;
	r = config.r_min;

}

int main(int argc, char **argv)
{
	// ROS Initalization
	ros::init(argc, argv, "connectivity");

	ros::NodeHandle n;
	ros::NodeHandle private_n("~");

	dynamic_reconfigure::Server<connectivity_controller::ConnectivityVariablesConfig> server;
  	dynamic_reconfigure::Server<connectivity_controller::ConnectivityVariablesConfig>::CallbackType f;
  	f = boost::bind(&reconfigureCallback, _1, _2);
 	server.setCallback(f);

	// ROS Parameters

	// List of robot names
	vector<string> robot_names;
	XmlRpc::XmlRpcValue robot_list;
	private_n.getParam("robot_list", robot_list);
	ROS_ASSERT(robot_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < robot_list.size(); i++)
	{
		ROS_ASSERT(robot_list[i].getType() == XmlRpc::XmlRpcValue::TypeString);
		robot_names.push_back(static_cast<string>(robot_list[i]));
	}

	// List of formation x positions
	vector<double> formation_x;
	XmlRpc::XmlRpcValue x_list;
	private_n.getParam("formation_x_list", x_list);
	ROS_ASSERT(x_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < x_list.size(); i++)
	{
		ROS_ASSERT(x_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		formation_x.push_back(static_cast<double>(x_list[i]));
	}

	// List of formation y positions
	vector<double> formation_y;
	XmlRpc::XmlRpcValue y_list;
	private_n.getParam("formation_y_list", y_list);
	ROS_ASSERT(y_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for (int i = 0; i < y_list.size(); i++)
	{
		ROS_ASSERT(y_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
		formation_y.push_back(static_cast<double>(y_list[i]));
	}

	// Number of robots
	const int num_robots = robot_names.size();

	// ROS Subscribers
	map<string, create_driver::ViconStream> vicon;
	vector<string>::iterator name_it;
	vector<ros::Subscriber> sub;

	for (name_it = robot_names.begin(); name_it != robot_names.end(); name_it++)
	{
		vicon.insert(pair<string, create_driver::ViconStream>(*name_it, create_driver::ViconStream()));
		sub.push_back(n.subscribe(*name_it + "/tf", 10, &create_driver::ViconStream::callback, &vicon[*name_it]));
	}

	// ROS Publishers
	ros::Publisher pub = n.advertise<connectivity_controller::ConnectivityMsgs>("/connectivity", 10);

	// ROS loop
	ros::Rate loop_rate(250); // 250 Hz

	while (ros::ok())
	{
		// Retrieve Vicon Data
		ros::spinOnce();

		// Calculations
		MatrixXd adjacency_matrix = MatrixXd::Zero(num_robots, num_robots);
		MatrixXd P = MatrixXd::Zero(num_robots, num_robots - 1);
		MatrixXd L = MatrixXd::Zero(num_robots, num_robots);

		// Adjacency Matrix
		for (int i = 0; i < num_robots; i++)
		{
			for (int j = 0; j < num_robots; j++)
			{
				adjacency_matrix(i,j) = psi(sqrt(
				pow(vicon[robot_names[j]].x() - vicon[robot_names[i]].x(), 2) +
				pow(vicon[robot_names[j]].y() - vicon[robot_names[i]].y(), 2)
				));
			}
		}

		// L Matrix
		for (int i = 0; i < num_robots; i++)
		{
			for (int j = 0; j < num_robots; j++)
			{
				if ( i == j ) {
					double rowSum = 0;
					for (int k = 0; k < num_robots; k++) {
						rowSum += adjacency_matrix(i,k);
					}
					L(i,j) = rowSum - adjacency_matrix(i,j);
				} else {
					L(i,j) = -adjacency_matrix(i,j);
				}
			}
		}

		// P Matrix
		if (num_robots == 6)
		{
			P << 	1/sqrt(2), 		-1/sqrt(6.0),		-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30),
					0.0,       		sqrt(2.0/3), 		-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30),
					0.0, 		 	0.0, 				sqrt(3.0)/2,    	-1/(2*sqrt(5)),		-1/sqrt(30),
					0.0, 			0.0, 				0.0, 				2/sqrt(5), 			-1/sqrt(30),
					0.0, 			0.0, 				0.0,				0.0, 				sqrt(5.0/6),
					-1/sqrt(2), 	-1/sqrt(6),			-1/2.0/sqrt(3), 	-1/2.0/sqrt(5), 	-1/sqrt(30);
		} else if (num_robots == 4) {
			P << 	-0.5,        		-0.5,              	-0.5,
					 0.833333333333333, -0.166666666666667, -0.166666666666667,
					-0.166666666666667,  0.833333333333333, -0.166666666666667,
					-0.166666666666667, -0.166666666666667,  0.833333333333333;
		}
		// M Matrix
		MatrixXd M = P.transpose() * L * P;
		//std::cerr << "adjacency_matrix" << std::endl << adjacency_matrix << std::endl << std::endl;
		// dLdx and dLdy for each robot
		MatrixXd dLdx[num_robots];
		MatrixXd dLdy[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			dLdx[i] = MatrixXd::Zero(num_robots, num_robots);
			dLdy[i] = MatrixXd::Zero(num_robots, num_robots);
		}

		for(int k = 0; k < num_robots; k++)
		{
			for(int i = 0; i < num_robots; i++)
			{
				for (int j = i + 1; j < num_robots; j++)
				{
					double D = sqrt(
					pow(vicon[robot_names[i]].x() - vicon[robot_names[j]].x(), 2) +
					pow(vicon[robot_names[i]].y() - vicon[robot_names[j]].y(), 2)
					);

					double dAdD = psiPrime(D);

					if( k == i ) {
						double dDdx = (vicon[robot_names[i]].x()-vicon[robot_names[j]].x()) / D;
						double dDdy = (vicon[robot_names[i]].y()-vicon[robot_names[j]].y()) / D;

						dLdx[k](i,j) = dAdD*dDdx;
						dLdy[k](i,j) = dAdD*dDdy;

					}

					if( k == j ) {
						double dDdx = -(vicon[robot_names[i]].x()-vicon[robot_names[j]].x()) /D;
						double dDdy = -(vicon[robot_names[i]].y()-vicon[robot_names[j]].y()) / D;

						dLdx[k](i,j) = dAdD*dDdx;
						dLdy[k](i,j) = dAdD*dDdy;

					}

					dLdx[k](j,i) = dLdx[k](i,j);
					dLdy[k](j,i) = dLdy[k](i,j);

				}
			}

			for(int i = 0; i < num_robots; i++)
			{
				double sumx = 0;
				double sumy = 0;
				for(int j = 0; j<num_robots; j++)
				{
					sumx += dLdx[k](i,j);
					sumy += dLdy[k](i,j);
				}
				dLdx[k](i,i) = -sumx;
	            dLdy[k](i,i) = -sumy;
			}
		}

		// dMdx and dMdy for each robot
		MatrixXd dMdx[num_robots];
		MatrixXd dMdy[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			dMdx[i] = P.transpose() * dLdx[i] * P;
			dMdy[i] = P.transpose() * dLdy[i] * P;
			//std:cerr << dMdx[i] << std::endl << std::endl;
		}

		// Collision Avoidance for each robot starts here:
		MatrixXd CAMatrix_x = MatrixXd::Zero(num_robots,num_robots);
		MatrixXd CAMatrix_y = MatrixXd::Zero(num_robots,num_robots);

		for(int i = 0; i < num_robots; i++)
		{
			for (int j = i + 1; j < num_robots; j++)
			{
				double D = sqrt(
				pow(vicon[robot_names[i]].x() - vicon[robot_names[j]].x(), 2) +
				pow(vicon[robot_names[i]].y() - vicon[robot_names[j]].y(), 2)
				);
				if( D < R && D > r){
					CAMatrix_x(i,j) = -4*(pow(R,2)-pow(r,2))
										*(pow(D,2)-pow(R,2))
										/pow((pow(D,2)-pow(r,2)),3)
										*(vicon[robot_names[i]].x()
										- vicon[robot_names[j]].x());

		        	CAMatrix_y(i,j) = -4*(pow(R,2)-pow(r,2))
		        						*(pow(D,2)-pow(R,2))
		        						/pow((pow(D,2)-pow(r,2)),3)
		        						*(vicon[robot_names[i]].y()
		        						- vicon[robot_names[j]].y());
            	} else {
            		CAMatrix_x(i,j) = 0;
            		CAMatrix_y(i,j) = 0;
            	}

            	CAMatrix_x(j,i) = -CAMatrix_x(i,j);
	       		CAMatrix_y(j,i) = -CAMatrix_y(i,j);
			}
		}

		double control_input_CA_x[num_robots];
		double control_input_CA_y[num_robots];

		for(int i = 0; i < num_robots; i++)
		{
			double sumx = 0;
			double sumy = 0;
			for (int j = 0; j < num_robots; j++)
			{
				sumx = sumx + CAMatrix_x(i,j);
				sumy = sumy + CAMatrix_y(i,j);
			}
			control_input_CA_x[i] = sumx;
			control_input_CA_y[i] = sumy;
		}

		double control_input_FC_x[num_robots];
		double control_input_FC_y[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			control_input_FC_x[i] = formation_x[i] - vicon[robot_names[i]].x();
			control_input_FC_y[i] = formation_y[i] - vicon[robot_names[i]].y();
		}

		// Trace of Mx and My for each robot
		double trace_Mx[num_robots];
		double trace_My[num_robots];

		for (int i = 0; i < num_robots; i++)
		{
			//std::cerr << (M.inverse()*dMdx[i]) << std::endl << std::endl;
			trace_Mx[i] = (M.inverse()*dMdx[i]).trace();
			trace_My[i] = (M.inverse()*dMdy[i]).trace();
		}

		double determinant_M = M.determinant();

		// Send msg

		connectivity_controller::ConnectivityMsgs msg;

		msg.trMx.assign(trace_Mx, trace_Mx + num_robots);
		msg.trMy.assign(trace_My, trace_My + num_robots);
		msg.u_ca_x.assign(control_input_CA_x, control_input_CA_x + num_robots);
		msg.u_ca_y.assign(control_input_CA_y, control_input_CA_y + num_robots);
		msg.u_fc_x.assign(control_input_FC_x, control_input_FC_x + num_robots);
		msg.u_fc_y.assign(control_input_FC_y, control_input_FC_y + num_robots);
		msg.detM = determinant_M;

		pub.publish(msg);

		// Sleep
		loop_rate.sleep();
	}

	return 0;
}


#include <boost/test/unit_test.hpp>

#include <crave/ConstrainedRandom.hpp>

#include <boost/format.hpp>

#include <set>
#include <iostream>

// using namespace std;
using namespace crave;

using boost::format;

BOOST_FIXTURE_TEST_SUITE(Distribution_t, Context_Fixture)

BOOST_AUTO_TEST_CASE(variable_dist_t1) {
  Variable<int> v;
  Generator gen;
  gen(dist(v, distribution<int>::create(range<int>(0, 5))(range<int>(50, 65))(range<int>(100, 125))));

  std::map<int, int> s;
  int total = 10000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE((0 <= gen[v] && gen[v] <= 5) || (50 <= gen[v] && gen[v] <= 65) || (100 <= gen[v] && gen[v] <= 125));
    ++s[gen[v]];
  }
  for (int i = 0; i <= 200; i++)
    if ((0 <= i && i <= 5) || (50 <= i && i <= 65) || (100 <= i && i <= 125))
      BOOST_REQUIRE(s.find(i) != s.end());
}

BOOST_AUTO_TEST_CASE(variable_dist_t2) {
  Variable<int> v;
  Generator gen;
  BOOST_REQUIRE_THROW(gen(dist(v, distribution<int>::create(range<int>(0, 10))(range<int>(50, 75))(range<int>(30, 51)))),
                    std::runtime_error);
}

BOOST_AUTO_TEST_CASE(variable_dist_t3) {
  Variable<char> v;
  Generator gen;
  gen(dist(v, distribution<char>::create(weighted_range<char>(1, 5, 50))(weighted_range<char>(10, 20, 20))(
                  weighted_range<char>(-50, -50, 30))));
  int cnt1 = 0, cnt2 = 0, cnt3 = 0;
  int total = 50000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE((1 <= gen[v] && gen[v] <= 5) || (10 <= gen[v] && gen[v] <= 20) || (gen[v] == -50));
    if (1 <= gen[v] && gen[v] <= 5) cnt1++;
    if (10 <= gen[v] && gen[v] <= 20) cnt2++;
    if (gen[v] == -50) cnt3++;
  }
  BOOST_REQUIRE_LT(cnt2, cnt3);
  BOOST_REQUIRE_LT(cnt3, cnt1);
}

BOOST_AUTO_TEST_CASE(variable_dist_t4) {
  Variable<int> v;
  Generator gen;
  gen("x", dist(v, distribution<int>::create(range<int>(0, 10))(range<int>(50, 75))(range<int>(100, 200))));
  gen("y", dist(v, distribution<int>::simple_range(5000, 6000)));
  gen.disableConstraint("x");
  int total = 10000;
  for (int i = 0; i < total; i++) {
    BOOST_REQUIRE(gen.next());
    BOOST_REQUIRE(5000 <= gen[v] && gen[v] <= 6000);
  }
}

BOOST_AUTO_TEST_CASE(dist_of_boolean25) {
  Variable<bool> a;
  int counter = 0;

  Generator gen;
  gen(dist(a, distribution<bool>::create(0.25)));
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(gen.next());
    if (gen[a]) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_LT(counter, 0);
}

BOOST_AUTO_TEST_CASE(dist_of_boolean50) {
  Variable<bool> a;
  int counter = 0;

  Generator gen;
  gen(dist(a, distribution<bool>::create(0.5)));
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(gen.next());
    if (gen[a]) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_LT(counter, 280);
  BOOST_REQUIRE_GT(counter, -280);
}

BOOST_AUTO_TEST_CASE(dist_of_boolean75) {
  Variable<bool> a;
  int counter = 0;

  Generator gen;
  gen(dist(a, distribution<bool>::create(0.75)));
  for (unsigned i = 0; i < 1000; i++) {
    BOOST_REQUIRE(gen.next());
    if (gen[a]) {
      ++counter;
    } else {
      --counter;
    }
  }

  BOOST_REQUIRE_GT(counter, 0);
}

BOOST_AUTO_TEST_SUITE_END()  // Context

//  vim: ft=cpp:ts=2:sw=2:expandtab


#include <bsplines/BSplinePose.hpp>
#include <sm/assert_macros.hpp>
// boost::tie
#include <boost/tuple/tuple.hpp>
#include <sm/kinematics/transformations.hpp>

namespace bsplines {
  using namespace sm::kinematics;

    BSplinePose::BSplinePose(int splineOrder, const RotationalKinematics::Ptr & rotationalKinematics)
      : BSpline(splineOrder), rotation_(rotationalKinematics)
    {

    }

    BSplinePose::~BSplinePose()
    {

    }

    Eigen::Matrix4d BSplinePose::transformation(double tk) const
    {
      return curveValueToTransformation(eval(tk));
    }

    Eigen::Matrix4d BSplinePose::transformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );

      if(J)
	{
	  *J = JT * JS;
	}

      return T;
    }

    Eigen::Matrix3d BSplinePose::orientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
	{
	  *J = JO * JS;
	}

      return C;

    }

    Eigen::Matrix3d BSplinePose::inverseOrientationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Matrix3d C;
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::Matrix3d S;
      C = rotation_->parametersToRotationMatrix(p.tail<3>(), &S).transpose();

      Eigen::MatrixXd JO = Eigen::MatrixXd::Zero(3,6);
      JO.block(0,3,3,3) = S;
      if(J)
	{
	  *J = -C * JO * JS;
	}

      return C;

    }

    Eigen::Matrix4d BSplinePose::inverseTransformationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      //std::cout << __FUNCTION__ << "()\n";
      //ASRL_THROW(std::runtime_error,"Not Implemented");
      Eigen::MatrixXd JS;
      Eigen::VectorXd p;
      p = evalDAndJacobian(tk,0,&JS, coefficientIndices);

      Eigen::MatrixXd JT;
      Eigen::Matrix4d T = curveValueToTransformationAndJacobian( p, &JT );
      // Invert the transformation.
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();

      if(J)
	{
	  // The "box times" is the linearized transformation way of inverting the jacobian.
	  *J = -sm::kinematics::boxTimes(T) * JT * JS;
	}

      if(coefficientIndices)
	{
	  *coefficientIndices = localCoefficientVectorIndices(tk);
	}

      return T;
    }

    Eigen::Matrix4d BSplinePose::inverseTransformation(double tk) const
    {
      Eigen::Matrix4d T = curveValueToTransformation(eval(tk));
      T.topLeftCorner<3,3>().transposeInPlace();
      T.topRightCorner<3,1>() = (-T.topLeftCorner<3,3>() * T.topRightCorner<3,1>()).eval();
      return T;
    }

    Eigen::Vector4d BSplinePose::transformVectorAndJacobian(double tk, const Eigen::Vector4d & v_tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::MatrixXd JT;
      Eigen::Matrix4d T_n_vk = transformationAndJacobian(tk, &JT, coefficientIndices);
      Eigen::Vector4d v_n = T_n_vk * v_tk;

      if(J)
	{
	  *J = sm::kinematics::boxMinus(v_n) * JT;
	}

      return v_n;
    }

    Eigen::Vector3d BSplinePose::position(double tk) const
    {
      return eval(tk).head<3>();
    }

    Eigen::Matrix3d BSplinePose::orientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>());
    }

    Eigen::Matrix3d BSplinePose::inverseOrientation(double tk) const
    {
      return rotation_->parametersToRotationMatrix(eval(tk).tail<3>()).transpose();
    }

    Eigen::Vector3d BSplinePose::linearVelocity(double tk) const
    {
      return evalD(tk,1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearVelocityBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 1).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAcceleration(double tk) const
    {
      return evalD(tk,2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk, 0);
      Eigen::Matrix3d C_wb = rotation_->parametersToRotationMatrix(r.tail<3>());
      return C_wb.transpose() * evalD(tk, 2).head<3>();
    }

    Eigen::Vector3d BSplinePose::linearAccelerationAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d a = evalDAndJacobian(tk,2,J,coefficientIndices).head<3>();
      if(J)
	{
	  J->conservativeResize(3,J->cols());
	}
      return a;
    }

    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocity(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);

      // \omega = S(\bar \theta) \dot \theta
      omega = -rotation_->parametersToSMatrix(r.tail<3>()) * v.tail<3>();
      return omega;
    }

    // \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrame(double tk) const
    {
      Eigen::Vector3d omega;
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);
      Eigen::Matrix3d S;
      Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), &S);

      // \omega = S(\bar \theta) \dot \theta
      omega = -C_w_b.transpose() * S * v.tail<3>();
      return omega;

    }

    // \omega_b_{w,b} (angular velocity of the world frame as seen from the body frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularVelocityBodyFrameAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {
      Eigen::Vector3d omega;
      Eigen::Vector3d p;
      Eigen::Vector3d pdot;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jpdot;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      pdot = evalDAndJacobian(tk,1,&Jpdot,coefficientIndices).tail<3>();

      Eigen::MatrixXd Jr;
      Eigen::Matrix3d C_w_b = inverseOrientationAndJacobian(tk,&Jr,NULL);

      // Rearrange the spline jacobian matrices. Now Jpdot is the
      // jacobian of p wrt the spline coefficients stacked on top
      // of the jacobian of pdot wrt the spline coefficients.
      Jpdot.block(0,0,3,Jpdot.cols()) = Jp.block(3,0,3,Jp.cols());

      //std::cout << "Jpdot\n" << Jpdot << std::endl;

      Eigen::Matrix<double,3,6> Jo;
      omega = -C_w_b * rotation_->angularVelocityAndJacobian(p,pdot,&Jo);
      Jo = (-C_w_b * Jo).eval();
      //std::cout << "Jo:\n" << Jo << std::endl;
      if(J)
	{
	  *J = Jo * Jpdot + sm::kinematics::crossMx(omega) * Jr;
	}

      return omega;

    }

    // \omega_w_{b,w} (angular velocity of the body frame as seen from the world frame, expressed in the world frame)
    Eigen::Vector3d BSplinePose::angularVelocityAndJacobian(double tk, Eigen::MatrixXd * J, Eigen::VectorXi * coefficientIndices) const
    {

      Eigen::Vector3d omega;
      Eigen::Vector3d p;
      Eigen::Vector3d pdot;
      Eigen::MatrixXd Jp;
      Eigen::MatrixXd Jpdot;
      p = evalDAndJacobian(tk,0,&Jp,NULL).tail<3>();
      pdot = evalDAndJacobian(tk,1,&Jpdot,coefficientIndices).tail<3>();

      // Rearrange the spline jacobian matrices. Now Jpdot is the
      // jacobian of p wrt the spline coefficients stacked on top
      // of the jacobian of pdot wrt the spline coefficients.
      Jpdot.block(0,0,3,Jpdot.cols()) = Jp.block(3,0,3,Jp.cols());

      //std::cout << "Jpdot\n" << Jpdot << std::endl;

      Eigen::Matrix<double,3,6> Jo;
      omega = rotation_->angularVelocityAndJacobian(p,pdot,&Jo);

      //std::cout << "Jo:\n" << Jo << std::endl;
      if(J)
	{
	  *J = Jo * Jpdot;
	}

      return omega;
    }

    /*
    // \alpha_b_{w,b} (angular acceleration of the world frame as seen from the body frame, expressed in the body frame)
    Eigen::Vector3d BSplinePose::angularAccelerationBodyFrame(double tk) const
    {
      Eigen::VectorXd r = evalD(tk,0);
      Eigen::VectorXd v = evalD(tk,1);
      Eigen::VectorXd a = evalD(tk,2);
      Eigen::Matrix3d C_w_b = rotation_->parametersToRotationMatrix(r.tail<3>(), NULL);

      // \alpha_b_{w,b} = - C_{b,w} * \alpha_w_{b,w}
      Eigen::Vector3d alpha_w = - C_w_b.transpose() * rotation_->angularAccelerationAndJacobian(r.tail<3>(), v.tail<3>(), a.tail<3>(), NULL);

      return alpha;

    }
    */

    void BSplinePose::initPoseSpline(double t0, double t1, const Eigen::Matrix4d & T_n_t0, const Eigen::Matrix4d & T_n_t1)
    {
      Eigen::VectorXd v0 = transformationToCurveValue(T_n_t0);
      Eigen::VectorXd v1 = transformationToCurveValue(T_n_t1);

      initSpline(t0,t1,v0,v1);
    }

    void BSplinePose::addPoseSegment(double tk, const Eigen::Matrix4d & T_n_tk)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment(tk, vk);
    }

    void BSplinePose::addPoseSegment2(double tk, const Eigen::Matrix4d & T_n_tk, double lambda)
    {
      Eigen::VectorXd vk = transformationToCurveValue(T_n_tk);

      addCurveSegment2(tk, vk, lambda);
    }

    Eigen::Matrix4d BSplinePose::curveValueToTransformation( const Eigen::VectorXd & c ) const
    {
      SM_ASSERT_EQ_DBG(Exception, c.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(c.tail<3>());
      T.topRightCorner<3,1>() = c.head<3>();

      return T;
    }

    Eigen::Matrix4d BSplinePose::curveValueToTransformationAndJacobian( const Eigen::VectorXd & p, Eigen::MatrixXd * J ) const
    {
      SM_ASSERT_EQ_DBG(Exception, p.size(), 6, "The curve value is an unexpected size!");
      Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
      Eigen::Matrix3d S;
      T.topLeftCorner<3,3>() = rotation_->parametersToRotationMatrix(p.tail<3>(), &S);
      T.topRightCorner<3,1>() = p.head<3>();

      if(J)
	{
	  *J = Eigen::MatrixXd::Identity(6,6);
	  J->topRightCorner<3,3>() = -crossMx(p.head<3>()) * S;
	  J->bottomRightCorner<3,3>() = S;

	}

      return T;

    }

    Eigen::VectorXd BSplinePose::transformationToCurveValue( const Eigen::Matrix4d & T ) const
    {
      Eigen::VectorXd c(6);
      c.head<3>() = T.topRightCorner<3,1>();
      c.tail<3>() = rotation_->rotationMatrixToParameters(T.topLeftCorner<3,3>());

      return c;
    }

    void BSplinePose::initPoseSpline2(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline2(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSpline3(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSpline3(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparse(const Eigen::VectorXd & times, const Eigen::Matrix<double,6,Eigen::Dynamic> & poses, int numSegments, double lambda)
    {
      initSplineSparse(times, poses, numSegments, lambda);
    }

    void BSplinePose::initPoseSplineSparseKnots(const Eigen::VectorXd &times, const Eigen::MatrixXd &interpolationPoints, const Eigen::VectorXd knots, double lambda)
    {
    	initSplineSparseKnots(times, interpolationPoints, knots, lambda);
    }

    RotationalKinematics::Ptr BSplinePose::rotation() const
    {
      return rotation_;
    }
  } // namespace bsplines


//#include <igl/read_triangle_mesh.h>
//#include <igl/png/readPNG.h>
//#include <igl/opengl/glfw/Viewer.h>
//#include <igl/opengl/create_shader_program.h>
//#include <igl/opengl/destroy_shader_program.h>
//#include <Eigen/Core>
//
//int main(int argc, char *argv[])
//{
//	using namespace Eigen;
//	using namespace std;
//	Eigen::MatrixXd V;
//	Eigen::MatrixXi F;
//	igl::opengl::glfw::Viewer v;
//	igl::read_triangle_mesh(argv[1], V, F);
//	Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> R, G, B, A;
//	igl::png::readPNG(argv[2], R, G, B, A);
//
//	v.data().set_mesh(V, F);
//	v.data().set_texture(R, G, B, A);
//	v.data().set_face_based(false);
//	v.data().show_lines = false;
//	v.data().show_texture = true;
//	v.launch_init(true, false);
//
//	v.data().meshgl.init();
//	igl::opengl::destroy_shader_program(v.data().meshgl.shader_mesh);
//
//	{
//		std::string mesh_vertex_shader_string =
//			R"(#version 150
//uniform mat4 view;
//uniform mat4 proj;
//uniform mat4 normal_matrix;
//in vec3 position;
//in vec3 normal;
//out vec3 normal_eye;
//
//void main()
//{
//  normal_eye = normalize(vec3 (normal_matrix * vec4 (normal, 0.0)));
//  gl_Position = proj * view * vec4(position, 1.0);
//})";
//
//		std::string mesh_fragment_shader_string =
//			R"(#version 150
//in vec3 normal_eye;
//out vec4 outColor;
//uniform sampler2D tex;
//void main()
//{
//  vec2 uv = normalize(normal_eye).xy * 0.5 + 0.5;
//  outColor = texture(tex, uv);
//})";
//
//		igl::opengl::create_shader_program(
//			mesh_vertex_shader_string,
//			mesh_fragment_shader_string,
//			{},
//			v.data().meshgl.shader_mesh);
//	}
//
//	v.launch_rendering(true);
//	v.launch_shut();
//}