//==============================================================================
//         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 <boost/simd/ieee/include/functions/ulp.hpp>
#include <boost/simd/sdk/simd/logical.hpp>

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

#include <boost/simd/include/constants/eps.hpp>
#include <boost/simd/include/constants/mindenormal.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( ulp_real,  BOOST_SIMD_REAL_TYPES)
{
  using boost::simd::ulp;
  using boost::simd::tag::ulp_;
  typedef typename boost::dispatch::meta::call<ulp_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ulp(boost::simd::Inf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(ulp(boost::simd::Minf<T>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(ulp(boost::simd::Nan<T>()), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(ulp(boost::simd::Mone<T>()), boost::simd::Eps<r_t>()/2);
  NT2_TEST_EQUAL(ulp(boost::simd::One<T>()), boost::simd::Eps<r_t>()/2);
  NT2_TEST_EQUAL(ulp(boost::simd::Zero<T>()), boost::simd::Mindenormal<r_t>());
}

NT2_TEST_CASE_TPL ( ulp_unsigned_int,  BOOST_SIMD_UNSIGNED_TYPES)
{
  using boost::simd::ulp;
  using boost::simd::tag::ulp_;
  typedef typename boost::dispatch::meta::call<ulp_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(ulp(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(ulp(boost::simd::Zero<T>()), boost::simd::One<r_t>());
}

NT2_TEST_CASE_TPL ( ulp_signed_int,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{
  using boost::simd::ulp;
  using boost::simd::tag::ulp_;
  typedef typename boost::dispatch::meta::call<ulp_(T)>::type r_t;
  typedef T wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_EQUAL(ulp(boost::simd::Mone<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(ulp(boost::simd::One<T>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(ulp(boost::simd::Zero<T>()), boost::simd::One<r_t>());
}


$TEMPLATE[lapack.h]
//
// Copyright (c) 2003--2009
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DETAIL_LAPACK_H
#define BOOST_NUMERIC_BINDINGS_LAPACK_DETAIL_LAPACK_H

#include <boost/numeric/bindings/lapack/detail/lapack_names.h>

extern "C" {

$CONTENT//
// LAPACK auxiliary routines
//

$LIBRARY_INT_TYPE LAPACK_ILAENV(const $LIBRARY_INT_TYPE* ispec, const char* name,
        const char* opt, const $LIBRARY_INT_TYPE* n1, const $LIBRARY_INT_TYPE* n2,
        const $LIBRARY_INT_TYPE* n3, const $LIBRARY_INT_TYPE* n4, $LIBRARY_INT_TYPE,
        $LIBRARY_INT_TYPE);

}

#endif

$TEMPLATE[lapack.h_function]
void LAPACK_$SUBROUTINE( $ARGUMENTS );
$TEMPLATE[lapack_names.h]
//
// Copyright (c) 2003--2009
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_DETAIL_LAPACK_NAMES_H
#define BOOST_NUMERIC_BINDINGS_LAPACK_DETAIL_LAPACK_NAMES_H

#include <boost/numeric/bindings/detail/config/fortran.hpp>

$CONTENT//
// LAPACK auxiliary routines
//

#define LAPACK_ILAENV FORTRAN_ID2( ilaenv, ILAENV )

#endif

$TEMPLATE[lapack_names.h_function]
#define LAPACK_$SUBROUTINE FORTRAN_ID2( $subroutine, $SUBROUTINE )
$TEMPLATE[end]


#include "FeaturesComputer.hpp"

#include "itkImageRegionIteratorWithIndex.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkVectorImageToImageAdaptor.h"
#include "itkComposeImageFilter.h"

#include "itkScalarImageToHaralickTextureFeaturesImageFilter.h"

#include <boost/program_options.hpp>
#include <boost/regex.hpp>

#include <iostream>
#include <string>

namespace po = boost::program_options;

typedef itk::RescaleIntensityImageFilter< InputImageType, InputImageType > RescaleFilter;

typedef typename itk::Statistics::ScalarImageToHaralickTextureFeaturesImageFilter< InputImageType, typename OutputImageType::PixelType::ValueType > HaralickFilter;

typedef itk::VectorImageToImageAdaptor< typename OutputImageType::PixelType::ValueType, OutputImageType::ImageDimension > HaralickFeatureAdaptorType;

typedef typename itk::Image< typename OutputImageType::PixelType::ValueType, 3 > HaralickFeatureImageType;

typedef itk::RescaleIntensityImageFilter< HaralickFeatureAdaptorType, HaralickFeatureImageType > HaralickFeatureImageRescaleFilter;

class cli_offset {
public :
	cli_offset() : m_offset(3)
	{
		m_offset[0] = 0; m_offset[1] = 0; m_offset[2] = 0;
	}

	cli_offset(unsigned int o1, unsigned int o2, unsigned int o3) : m_offset(3)
	{
		m_offset[0] = o1; m_offset[1] = o2; m_offset[2] = o3;
	}

	std::vector< unsigned int > getOffset() { return m_offset; }

	const unsigned int operator[](int i) const { return m_offset[i]; }

private:
	std::vector< unsigned int > m_offset;
};

void validate(boost::any& v, const std::vector<std::string>& values, cli_offset* target_type, int)
{
	static boost::regex r("(\\d+),(\\d+),(\\d+)");

	using namespace boost::program_options;

	// Make sure no previous assignment to 'v' was made.
	validators::check_first_occurrence(v);
	// Extract the first string from 'values'. If there is more than
	// one string, it's an error, and exception will be thrown.
	const std::string& s = validators::get_single_string(values);

	// Do regex match and convert the interesting part to int.
	boost::smatch match;
	if (boost::regex_match(s, match, r)) {
		v = boost::any(cli_offset(boost::lexical_cast<unsigned int>(match[1]), boost::lexical_cast<unsigned int>(match[2]), boost::lexical_cast<unsigned int>(match[3])));
	} else {
		throw invalid_option_value(s);
	}
}

std::ostream &operator<<(std::ostream &out, cli_offset& t)
{
	std::vector<unsigned int> vec = t.getOffset();

	std::copy(vec.begin(), vec.end(), std::ostream_iterator<unsigned int>(out, ", ") );

	return out;
}

typedef itk::ImageRegionIteratorWithIndex< OutputImageType >  OutputImageIterator;

class HaralickComputer : public FeaturesComputer
{
private:
	boost::program_options::options_description options;
	unsigned int posterization_level;
	cli_offset window;
	std::vector< cli_offset > offsets;

public:
	HaralickComputer():
		options("HaralickComputer")
	{
		options.add_options()
			("posterization,p",
				po::value< unsigned int >(&this->posterization_level)->required(), "Posterization level (required)")
			("window,w",
				po::value< cli_offset >(&this->window)->required(), "Window radius (required)")
			("offset,o",
				po::value< std::vector< cli_offset > >(&this->offsets)->required()->multitoken(), "Offset (required)")
			;
	}

	virtual void print_usage(std::ostream &os)
	{
		os << this->options;
	}

	virtual OutputImageType::Pointer compute( InputImageType::Pointer input_image, std::vector< std::string > params )
	{
		po::variables_map vm;

		po::store(po::command_line_parser(params).options(this->options).run(), vm);
		vm.notify();

		// Posterize the input image
		typename RescaleFilter::Pointer rescaler = RescaleFilter::New();
		rescaler->SetInput(input_image);
		rescaler->SetOutputMinimum(0);
		rescaler->SetOutputMaximum(this->posterization_level - 1);
		rescaler->Update();

#ifdef USE_LOG4CXX
		LOG4CXX_INFO(m_Logger, "Posterization done.");
#endif

		typename HaralickFilter::Pointer haralickImageComputer = HaralickFilter::New();
		haralickImageComputer->SetInput(rescaler->GetOutput());
		haralickImageComputer->SetNumberOfBinsPerAxis(this->posterization_level);

		{
			typename HaralickFilter::RadiusType window_radius =
				{{this->window[0], this->window[1], this->window[2]}};
			haralickImageComputer->SetWindowRadius(window_radius);
		}

		{
			typename HaralickFilter::OffsetVectorType::Pointer offsetV =
				HaralickFilter::OffsetVectorType::New();
			typename HaralickFilter::OffsetType offset;
			std::vector< cli_offset >::const_iterator offsets_it;
			for( offsets_it = this->offsets.begin(); offsets_it < this->offsets.end(); ++offsets_it)
			{
				offset[0] = (*offsets_it)[0];
				offset[1] = (*offsets_it)[1];
				offset[2] = (*offsets_it)[2];

				offsetV->push_back(offset);
			}

			haralickImageComputer->SetOffsets(offsetV);
		}

		haralickImageComputer->Update();

#ifdef USE_LOG4CXX
		LOG4CXX_INFO(m_Logger, "Computation of Haralick features done.");
#endif

		return haralickImageComputer->GetOutput();
	}
};

extern "C" FeaturesComputer* create() {
	return new HaralickComputer;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2010-2015 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 <iostream>
#include <string>

// If defined, tests are run without rescaling-to-integer or robustness policy
// This multi_difference currently contains no tests for double which then fail
// #define BOOST_GEOMETRY_NO_ROBUSTNESS

//#define HAVE_TTMATH
//#define BOOST_GEOMETRY_DEBUG_ASSEMBLE
//#define BOOST_GEOMETRY_CHECK_WITH_SQLSERVER

//#define BOOST_GEOMETRY_DEBUG_SEGMENT_IDENTIFIER
//#define BOOST_GEOMETRY_DEBUG_FOLLOW
//#define BOOST_GEOMETRY_DEBUG_TRAVERSE

#include "test_difference.hpp"
#include <algorithms/test_overlay.hpp>
#include <algorithms/overlay/multi_overlay_cases.hpp>

#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/multi_linestring.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>

#include <boost/geometry/io/wkt/read.hpp>

template <typename MultiPolygon, typename MultiLineString>
void test_areal_linear()
{
    typedef typename boost::range_value<MultiPolygon>::type polygon;
    typedef typename boost::range_value<MultiLineString>::type linestring;
    typedef typename bg::point_type<polygon>::type point;
    typedef bg::model::ring<point> ring;

    test_one_lp<linestring, linestring, MultiPolygon>("case_mp_ls_1", "LINESTRING(2 0,2 5)", case_multi_simplex[0], 2, 4, 1.30);
    test_one_lp<linestring, MultiLineString, polygon>("case_p_mls_1", "MULTILINESTRING((2 0,2 5),(3 0,3 5))", case_single_simplex, 3, 6, 2.5);
    test_one_lp<linestring, MultiLineString, MultiPolygon>("case_mp_mls_1", "MULTILINESTRING((2 0,2 5),(3 0,3 5))", case_multi_simplex[0], 5, 10, 3.1666667);
    test_one_lp<linestring, MultiLineString, ring>("case_r_mls_1", "MULTILINESTRING((2 0,2 5),(3 0,3 5))", case_single_simplex, 3, 6, 2.5);

    // Collinear cases, with multiple turn points at the same location
    test_one_lp<linestring, linestring, MultiPolygon>("case_mp_ls_2a", "LINESTRING(1 0,1 1,2 1,2 0)", "MULTIPOLYGON(((0 0,0 1,1 1,1 0,0 0)),((1 1,1 2,2 2,2 1,1 1)))", 1, 2, 1.0);
    test_one_lp<linestring, linestring, MultiPolygon>("case_mp_ls_2b", "LINESTRING(1 0,1 1,2 1,2 0)", "MULTIPOLYGON(((1 1,1 2,2 2,2 1,1 1)),((0 0,0 1,1 1,1 0,0 0)))", 1, 2, 1.0);

    test_one_lp<linestring, linestring, MultiPolygon>("case_mp_ls_3",
            "LINESTRING(6 6,6 7,7 7,7 6,8 6,8 7,9 7,9 6)",
            "MULTIPOLYGON(((5 7,5 8,6 8,6 7,5 7)),((6 6,6 7,7 7,7 6,6 6)),((8 8,9 8,9 7,8 7,7 7,7 8,8 8)))", 2, 5, 3.0);

    return;

    // TODO: this case contains collinearities and should still be solved
    test_one_lp<linestring, linestring, MultiPolygon>("case_mp_ls_4",
            "LINESTRING(0 5,0 6,1 6,1 5,2 5,2 6,3 6,3 5,3 4,3 3,2 3,2 4,1 4,1 3,0 3,0 4)",
            "MULTIPOLYGON(((0 2,0 3,1 2,0 2)),((2 5,3 6,3 5,2 5)),((1 5,1 6,2 6,2 5,1 5)),((2 3,2 4,3 4,2 3)),((0 3,1 4,1 3,0 3)),((4 3,3 3,3 5,4 5,4 4,4 3)))", 5, 11, 6.0);
}

template <typename P>
void test_all()
{
    typedef bg::model::polygon<P> polygon;
    typedef bg::model::linestring<P> linestring;
    typedef bg::model::multi_polygon<polygon> multi_polygon;
    typedef bg::model::multi_linestring<linestring> multi_linestring;
    test_areal_linear<multi_polygon, multi_linestring>();
}

int test_main(int, char* [])
{
    test_all<bg::model::d2::point_xy<double> >();

#if ! defined(BOOST_GEOMETRY_TEST_ONLY_ONE_TYPE)
    test_all<bg::model::d2::point_xy<float> >();
#endif

    return 0;
}


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef DISCRETE_SOLVER_HH
#define DISCRETE_SOLVER_HH

/**
 * @file
 * @brief  Routines for the solution of (sparse) linear systems
 * @author Anton Schiela
 */

#include <iostream>

#include <boost/timer/timer.hpp>

#include "linalg/factorization.hh"
#include "linalg/linearsystem.hh"
#include "linalg/umfpack_solve.hh"
//#include "linalg/direct_solver_factory.hh"

namespace Kaskade
{
  /** \ingroup linalg
   * \brief Direct solver.Implements Direct Solver Interface of Algorithms
   *
   *
   *  Factorizations:
   * - UMF
   * - DirectType::MUMPS
   * - DirectType::SUPERLU
   */
  //template<class Factorization>
  template <class Scalar, class SparseInt=int>
  class DirectLinearSolver
  {
  public:
    /// needs a matrix
    static const bool needMatrix = true;
    /// default constructor
    explicit DirectLinearSolver(std::string const& solverName_, bool verbose = false) : report(false), solverName(solverName_)/*, factory(verbose) */{}

    ~DirectLinearSolver() { flushFactorization(); }

    /// solve a system, keep factorization
    void solve(std::vector<double>& sol, SparseLinearSystem& lin)
    {
      flushFactorization();
      boost::timer::cpu_timer timer;

      MatrixAsTriplet<double> mat;
      lin.getMatrix(mat);

      //   mat.print();
      factorization.reset(new UMFFactorization<Scalar,int>(mat.nrows(), 1, mat.ridx, mat.cidx, mat.data)/*factory.create(solverName, mat.nrows(), mat.ridx, mat.cidx, mat.data) */);
      //    factorization.reset(new Factorization(mat.nrows(),
      //                                          matrixtype,
      //                                          mat.ridx,
      //                                          mat.cidx,
      //                                          mat.data));
      if(report && (double)(timer.elapsed().user)/1e9 > 2.0) {
        std::cout << "Factorization: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;
      }
      resolve(sol,lin);
    }
    /// solve a system again
    void resolve(std::vector<double>& sol,
        SparseLinearSystem const& lin) const
    {
      boost::timer::cpu_timer timer;
      if(report >= 2) std::cout << "Starting Substitution...";
      std::vector<double> rhs;
      lin.getRHS(rhs);
      factorization->solve(rhs,sol);
      if(report >=2)  std::cout << "Finished: " << (double)(timer.elapsed().user)/1e9 << " sec." << std::endl;

    }

    /// Solves always exactly
    void setRelativeAccuracy(double) {}

    /// Always exact solution
    double getRelativeAccuracy() {return 0.0;}

    /// Always exact solution
    double getAbsoluteAccuracy() {return 0.0;}

    bool improvementPossible() { return false; }

    /// Do sth if linearization changed
    void onChangedLinearization() {flushFactorization(); }

    /// flush factorization
    void flushFactorization()
    {
      if(factorization.get()!=nullptr) factorization.reset();
    }
    /// Report of progress (0=no report, 1=brief, 2=verbose)
    int report;

  private:
    std::string const& solverName;
    //DirectSolverFactory<Scalar,SparseInt> factory;
    std::unique_ptr<Factorization<Scalar,SparseInt> > factorization;
  };
}
#endif


// 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.

#ifndef MTL_RANK_ONE_UPDATE_INCLUDE
#define MTL_RANK_ONE_UPDATE_INCLUDE

#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/property_map.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/matrix/inserter.hpp>
#include <boost/numeric/mtl/operation/update.hpp>
#include <boost/numeric/mtl/operation/conj.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl { namespace matrix {

/// Rank-one update: rank_one_update(A, x, y) computes A+= x * conj(y)^T
/** The current implementation works for column and row vectors (although
    the notation above refers to column vectors). **/
template <typename Matrix, typename VectorX, typename VectorY>
inline void rank_one_update(Matrix& matrix, const VectorX& x, const VectorY& y)
{
    using mtl::conj;
    vampir_trace<2022> tracer;
    MTL_THROW_IF(num_rows(matrix) != size(x) || num_cols(matrix) != size(y), incompatible_size());
    namespace traits = mtl::traits;
    typedef typename traits::range_generator<tag::nz, VectorX>::type x_cursor;
    typename traits::index<VectorX>::type             index_x(x);
    typename traits::const_value<VectorX>::type       value_x(x);

    typedef typename traits::range_generator<tag::nz, VectorY>::type y_cursor;
    typename traits::index<VectorY>::type             index_y(y);
    typename traits::const_value<VectorY>::type       value_y(y);

    matrix::inserter<Matrix, operations::update_plus<typename Collection<Matrix>::value_type> > ins(matrix);

    for (x_cursor xc= begin<tag::nz>(x), xend= end<tag::nz>(x); xc != xend; ++xc)
	for (y_cursor yc= begin<tag::nz>(y), yend= end<tag::nz>(y); yc != yend; ++yc)
	    ins(index_x(*xc), index_y(*yc)) << value_x(*xc) * conj(value_y(*yc));
}

}} // namespace mtl::matrix

#endif // MTL_RANK_ONE_UPDATE_INCLUDE


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2014-2018, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#include <iostream>

#ifndef BOOST_TEST_MODULE
#define BOOST_TEST_MODULE test_distance_cartesian_areal_areal
#endif

#include <boost/test/included/unit_test.hpp>

#include "test_distance_common.hpp"

typedef bg::model::point<int,2,bg::cs::cartesian>     int_point_type;
typedef bg::model::point<double,2,bg::cs::cartesian>  point_type;
typedef bg::model::polygon<point_type, false>         polygon_type;
typedef bg::model::multi_polygon<polygon_type>        multi_polygon_type;
typedef bg::model::ring<point_type, false>            ring_type;
typedef bg::model::box<int_point_type>                int_box_type;
typedef bg::model::box<point_type>                    box_type;

namespace services = bg::strategy::distance::services;
typedef bg::default_distance_result<point_type>::type return_type;

typedef bg::strategy::distance::projected_point<> point_segment_strategy;
typedef bg::strategy::distance::pythagoras_box_box<> box_box_strategy;

//===========================================================================

template <typename Strategy>
void test_distance_polygon_polygon(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "polygon/polygon distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<polygon_type, polygon_type> tester;

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,5 20,5 25,-5 25,-5 20))",
                  10, 100, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 5,5 5,5 20,-5 20))",
                  0, 0, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 -20,5 -20,5 20,-5 20))",
                  0, 0, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10),\
                   (-5 -5,-5 5,5 5,5 -5,-5 -5))",
                  "polygon((-1 -1,0 0,-1 0,-1 -1))",
                  4, 16, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_polygon_multipolygon(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "polygon/multipolygon distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            polygon_type, multi_polygon_type
        > tester;

    tester::apply("polygon((12 0,14 0,19 0,19.9 -1,12 0))",
                  "multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  0.1, 0.01, strategy);

    tester::apply("polygon((19 0,19.9 -1,12 0,20.5 0.5,19 0))",
                  "multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_polygon_ring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "polygon/ring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<polygon_type, ring_type> tester;

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,5 20,5 25,-5 25,-5 20))",
                  10, 100, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 5,5 5,5 20,-5 20))",
                  0, 0, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 -20,5 -20,5 20,-5 20))",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipolygon_multipolygon(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipolygon/multipolygon distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            multi_polygon_type, multi_polygon_type
        > tester;

    tester::apply("multipolygon(((12 0,14 0,14 1,12 0)),\
                   ((18 0,19 0,19.9 -1,18 0)))",
                  "multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  0.1, 0.01, strategy);

    tester::apply("multipolygon(((18 0,19 0,19.9 -1,18 0)),\
                   ((12 0,14 0,20.5 0.5,12 0)))",
                  "multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipolygon_ring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipolygon/ring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries
        <
            multi_polygon_type, ring_type
        > tester;

    tester::apply("multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  "polygon((12 0,14 0,19 0,19.9 -1,12 0))",
                  0.1, 0.01, strategy);

    tester::apply("multipolygon(((-10 -10,10 -10,10 10,-10 10,-10 -10)),\
                   ((20 -1,21 2,30 -10,20 -1)))",
                  "polygon((19 0,19.9 -1,12 0,20.5 0.5,19 0))",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_ring_ring(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "ring/ring distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<ring_type, ring_type> tester;

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,5 20,5 25,-5 25,-5 20))",
                  10, 100, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 5,5 5,5 20,-5 20))",
                  0, 0, strategy);

    tester::apply("polygon((-10 -10,10 -10,10 10,-10 10,-10 -10))",
                  "polygon((-5 20,-5 -20,5 -20,5 20,-5 20))",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_box_box(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "box/box distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<int_box_type, int_box_type> int_tester;
    typedef test_distance_of_geometries<box_type, box_type> tester;

    int_tester::apply("box(5 5,10 10)",
                      "box(0 0,1 1)",
                      sqrt(32.0), 32, strategy);

    tester::apply("box(5 5,10 10)",
                  "box(0 0,1 1)",
                  sqrt(32.0), 32, strategy);

    tester::apply("box(3 8,13 18)",
                  "box(0 0,5 5)",
                  3, 9, strategy);

    tester::apply("box(5 5,10 10)",
                  "box(0 0,5 5)",
                  0, 0, strategy);

    tester::apply("box(5 5,10 10)",
                  "box(0 0,6 6)",
                  0, 0, strategy);

    tester::apply("box(3 5,13 15)",
                  "box(0 0,5 5)",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_polygon_box(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "polygon/box distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<polygon_type, box_type> tester;

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,1 1)",
                  sqrt(32.0), 32, strategy);

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,5 5)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,6 6)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,15 5,10 0,5 5,10 10))",
                  "box(5 0,7.5 2.5)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,15 5,10 0,5 5,10 10))",
                  "box(5 0,6 1)",
                  sqrt(4.5), 4.5, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_multipolygon_box(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "multipolygon/box distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<multi_polygon_type, box_type> tester;

    tester::apply("multipolygon(((-10 -10,-10 -9,-9 -9,-9 -10,-10 -10)),\
                   ((2 2,2 3,3 3,3 2,2 2)))",
                  "box(0 0,1 1)",
                  sqrt(2.0), 2, strategy);

    tester::apply("multipolygon(((-10 -10,-10 -9,-9 -9,-9 -10,-10 -10)),\
                   ((2 2,2 3,3 3,3 2,2 2)))",
                  "box(0 0,2 2)",
                  0, 0, strategy);

    tester::apply("multipolygon(((-10 -10,-10 -9,-9 -9,-9 -10,-10 -10)),\
                   ((2 2,2 3,3 3,3 2,2 2)))",
                  "box(0 0,2.5 2)",
                  0, 0, strategy);
}

//===========================================================================

template <typename Strategy>
void test_distance_ring_box(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "ring/box distance tests" << std::endl;
#endif
    typedef test_distance_of_geometries<ring_type, box_type> tester;

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,1 1)",
                  sqrt(32.0), 32, strategy);

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,5 5)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,10 5,5 5,5 10,10 10))",
                  "box(0 0,6 6)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,15 5,10 0,5 5,10 10))",
                  "box(5 0,7.5 2.5)",
                  0, 0, strategy);

    tester::apply("polygon((10 10,15 5,10 0,5 5,10 10))",
                  "box(5 0,6 1)",
                  sqrt(4.5), 4.5, strategy);
}

//===========================================================================

template <typename Point, typename Strategy>
void test_more_empty_input_areal_areal(Strategy const& strategy)
{
#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << std::endl;
    std::cout << "testing on empty inputs... " << std::flush;
#endif
    bg::model::polygon<Point> polygon_empty;
    bg::model::multi_polygon<bg::model::polygon<Point> > multipolygon_empty;
    bg::model::ring<Point> ring_empty;

    bg::model::polygon<Point> polygon =
        from_wkt<bg::model::polygon<Point> >("polygon((0 0,1 0,0 1))");

    bg::model::multi_polygon<bg::model::polygon<Point> > multipolygon =
        from_wkt
            <
                bg::model::multi_polygon<bg::model::polygon<Point> >
            >("multipolygon(((0 0,1 0,0 1)))");

    bg::model::ring<Point> ring =
        from_wkt<bg::model::ring<Point> >("polygon((0 0,1 0,0 1))");

    // 1st geometry is empty
    test_empty_input(polygon_empty, polygon, strategy);
    test_empty_input(polygon_empty, multipolygon, strategy);
    test_empty_input(polygon_empty, ring, strategy);
    test_empty_input(multipolygon_empty, polygon, strategy);
    test_empty_input(multipolygon_empty, multipolygon, strategy);
    test_empty_input(multipolygon_empty, ring, strategy);
    test_empty_input(ring_empty, polygon, strategy);
    test_empty_input(ring_empty, multipolygon, strategy);
    test_empty_input(ring_empty, ring, strategy);

    // 2nd geometry is empty
    test_empty_input(polygon, polygon_empty, strategy);
    test_empty_input(polygon, multipolygon_empty, strategy);
    test_empty_input(polygon, ring_empty, strategy);
    test_empty_input(multipolygon, polygon_empty, strategy);
    test_empty_input(multipolygon, multipolygon_empty, strategy);
    test_empty_input(multipolygon, ring_empty, strategy);
    test_empty_input(ring, polygon_empty, strategy);
    test_empty_input(ring, multipolygon_empty, strategy);
    test_empty_input(ring, ring_empty, strategy);

    // both geometries are empty
    test_empty_input(polygon_empty, polygon_empty, strategy);
    test_empty_input(polygon_empty, multipolygon_empty, strategy);
    test_empty_input(polygon_empty, ring_empty, strategy);
    test_empty_input(multipolygon_empty, polygon_empty, strategy);
    test_empty_input(multipolygon_empty, multipolygon_empty, strategy);
    test_empty_input(multipolygon_empty, ring_empty, strategy);
    test_empty_input(ring_empty, polygon_empty, strategy);
    test_empty_input(ring_empty, multipolygon_empty, strategy);
    test_empty_input(ring_empty, ring_empty, strategy);

#ifdef BOOST_GEOMETRY_TEST_DEBUG
    std::cout << "done!" << std::endl;
#endif
}

//===========================================================================

BOOST_AUTO_TEST_CASE( test_all_polygon_polygon )
{
    test_distance_polygon_polygon(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_polygon_multipolygon )
{
    test_distance_polygon_multipolygon(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_polygon_ring )
{
    test_distance_polygon_ring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipolygon_multipolygon )
{
    test_distance_multipolygon_multipolygon(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipolygon_ring )
{
    test_distance_multipolygon_ring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_ring_ring )
{
    test_distance_ring_ring(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_box_box )
{
    test_distance_box_box(box_box_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_polygon_box )
{
    test_distance_polygon_box(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_multipolygon_box )
{
    test_distance_multipolygon_box(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_ring_box )
{
    test_distance_ring_box(point_segment_strategy());
}

BOOST_AUTO_TEST_CASE( test_all_empty_input_areal_areal )
{
    test_more_empty_input_areal_areal<point_type>(point_segment_strategy());
}


#pragma once
#include <lp_lib.h>
#include <Eigen/Core>

struct ILP {
    int num_variables;
    lprec* ptr;

    ILP(int num_variables)
        : num_variables(num_variables)
        , ptr(::make_lp(0, num_variables))
    {
        ::set_verbose(ptr, SEVERE);
        // set all variables to be integer
        for (int i = 0; i < num_variables; ++i)
            if (!::set_int(ptr, i + 1, TRUE)) assert(false);
        // default ojective
        set_objective(Eigen::VectorXd::Ones(num_variables), true);
    }
    ~ILP() { ::delete_lp(ptr); }
    void add_constraint(const Eigen::VectorXd& row, int constr_type, double rhs) const {
        assert(row.size() == num_variables);
        Eigen::VectorXd tmp;
        tmp.resize(num_variables + 1);
        tmp[0] = 0;
        tmp.tail(num_variables) = row;
        if (!::add_constraint(ptr, &tmp[0], constr_type, rhs)) assert(false);
    }
    void add_constraint(const Eigen::MatrixXd& rows, int constr_type, const Eigen::VectorXd& rhs) const {
        for (int i = 0; i < rows.rows(); ++i)
            add_constraint(rows.row(i), constr_type, rhs[i]);
    }
    void set_objective(const Eigen::VectorXd& row, bool is_maxim) const {
        assert(row.size() == num_variables);
        Eigen::VectorXd tmp;
        tmp.resize(num_variables + 1);
        tmp[0] = 0;
        tmp.tail(num_variables) = row;
        if (!::set_obj_fn(ptr, &tmp[0])) assert(false);
        (is_maxim ? ::set_maxim : ::set_minim)(ptr);
    }
    bool solve() const { return ::solve(ptr) <= SUBOPTIMAL; }
    void refresh() {            // workaround for weird error when re-solving
        auto ptr_old = ptr;
        ptr = ::copy_lp(ptr);
        ::delete_lp(ptr_old);
    }
    Eigen::VectorXi get_variables() const {
        Eigen::VectorXd variables = Eigen::VectorXd::Zero(num_variables);
        if (!::get_variables(ptr, &variables[0])) assert(false);
        return variables.cast<int>();
    }
};


#define BOOST_TEST_MODULE duration generator test
#include <boost/test/data/monomorphic.hpp>
#include <boost/test/data/test_case.hpp>
#include <boost/test/included/unit_test.hpp>
#include <unordered_map>

#include "duration_generator.hpp"

namespace bdata = boost::unit_test::data;

BOOST_DATA_TEST_CASE(constant_sleep_duration_test, bdata::make({1, 2, 3, 4, 5}), sample) {
  ms_duration second(sample);
  constant_duration_generator cdg(second);
  duration_generator &gen = cdg;

  // Geneartor must always return same duration
  BOOST_TEST(gen.get().count() == sample);
  BOOST_TEST(gen.get().count() == sample);
  BOOST_TEST(gen.get().count() == sample);
  BOOST_TEST(gen.get().count() == sample);
}

BOOST_TEST_DONT_PRINT_LOG_VALUE(ms_duration)
BOOST_DATA_TEST_CASE(random_sleep_duration_test,
                     bdata::make({ms_duration(1000)}) ^ bdata::make({ms_duration(2000)}),
                     lower,
                     upper) {
  const uint32_t invokation_count = 1000;
  class ms_duration_hasher {
  public:
    std::size_t operator()(const ms_duration &key) const noexcept { return key.count(); }
  };

  std::unordered_map<ms_duration, uint32_t, ms_duration_hasher> map;

  random_duration_generator rdg(lower, upper);
  duration_generator &gen = rdg;

  for (uint32_t i = 0; i < invokation_count; i++) {
    ms_duration duration = gen.get();
    // duration must be in between requested range
    BOOST_TEST((duration <= upper && duration >= lower));
    map[duration] += 1;
  }
  // To check that there are limited repeatation. In case generator start to return same duration.
  BOOST_TEST(map.size() > invokation_count / 10);
}


// test_nc_beta.cpp

// Copyright John Maddock 2008.

// Use, modification and distribution are subject to 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 <pch.hpp>

#ifdef _MSC_VER
#pragma warning (disable:4127 4512)
#endif

#if !defined(TEST_FLOAT) && !defined(TEST_DOUBLE) && !defined(TEST_LDOUBLE) && !defined(TEST_REAL_CONCEPT)
#  define TEST_FLOAT
#  define TEST_DOUBLE
#  define TEST_LDOUBLE
#  define TEST_REAL_CONCEPT
#endif

#include <boost/math/concepts/real_concept.hpp> // for real_concept
#include <boost/math/distributions/non_central_f.hpp> // for chi_squared_distribution
#include <boost/test/test_exec_monitor.hpp> // for test_main
#include <boost/test/results_collector.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp> // for BOOST_CHECK_CLOSE

#include "functor.hpp"
#include "handle_test_result.hpp"

#include <iostream>
using std::cout;
using std::endl;
#include <limits>
using std::numeric_limits;

#define BOOST_CHECK_CLOSE_EX(a, b, prec, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK_CLOSE(a, b, prec); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " } " << std::endl;\
      }\
   }

#define BOOST_CHECK_EX(a, i) \
   {\
      unsigned int failures = boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed;\
      BOOST_CHECK(a); \
      if(failures != boost::unit_test::results_collector.results( boost::unit_test::framework::current_test_case().p_id ).p_assertions_failed)\
      {\
         std::cerr << "Failure was at row " << i << std::endl;\
         std::cerr << std::setprecision(35); \
         std::cerr << "{ " << data[i][0] << " , " << data[i][1] << " , " << data[i][2];\
         std::cerr << " , " << data[i][3] << " , " << data[i][4] << " } " << std::endl;\
      }\
   }

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double|real_concept";
   }
   else
   {
      largest_type = "long double|real_concept";
   }
#else
   largest_type = "(long\\s+)?double|real_concept";
#endif

   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

template <class RealType>
RealType naive_pdf(RealType v1, RealType v2, RealType l, RealType f)
{
   BOOST_MATH_STD_USING
   RealType sum = 0;
   for(int i = 0; ; ++i)
   {
      RealType term = -l/2 + log(l/2) * i;
      term += boost::math::lgamma(v2/2 + v1/2+i) - (boost::math::lgamma(v2/2) + boost::math::lgamma(v1/2+i));
      term -= boost::math::lgamma(RealType(i+1));
      term += log(v1/v2) * (v1/2+i) + log(v2 / (v2 + v1 * f)) * ((v1 + v2) / 2 + i);
      term += log(f) * (v1/2 - 1 + i);
      term = exp(term);
      sum += term;
      if((term/sum < boost::math::tools::epsilon<RealType>()) || (term == 0))
         break;
   }
   return sum;
}

template <class RealType>
void test_spot(
     RealType a,     // df1
     RealType b,     // df2
     RealType ncp,   // non-centrality param
     RealType x,     // F statistic
     RealType P,     // CDF
     RealType Q,     // Complement of CDF
     RealType D,     // PDF
     RealType tol)   // Test tolerance
{
   boost::math::non_central_f_distribution<RealType> dist(a, b, ncp);
   BOOST_CHECK_CLOSE(
      cdf(dist, x), P, tol);
   BOOST_CHECK_CLOSE(
      pdf(dist, x), D, tol);
   if(boost::math::tools::digits<RealType>() > 50)
   {
      //
      // The "naive" pdf calculation fails at float precision.
      //
      BOOST_CHECK_CLOSE(
         pdf(dist, x), naive_pdf(a, b, ncp, x), tol);
   }

   if((P < 0.99) && (Q < 0.99))
   {
      //
      // We can only check this if P is not too close to 1,
      // so that we can guarentee Q is reasonably free of error:
      //
      BOOST_CHECK_CLOSE(
         cdf(complement(dist, x)), Q, tol);
      BOOST_CHECK_CLOSE(
            quantile(dist, P), x, tol * 10);
      BOOST_CHECK_CLOSE(
            quantile(complement(dist, Q)), x, tol * 10);
   }
   if(boost::math::tools::digits<RealType>() > 50)
   {
      //
      // Sanity check mode:
      //
      RealType m = mode(dist);
      RealType p = pdf(dist, m);
      BOOST_CHECK(pdf(dist, m * (1 + sqrt(tol) * 10)) <= p);
      BOOST_CHECK(pdf(dist, m * (1 - sqrt(tol)) * 10) <= p);
   }
}

template <class RealType> // Any floating-point type RealType.
void test_spots(RealType)
{
   RealType tolerance = (std::max)(
      boost::math::tools::epsilon<RealType>() * 100,
      (RealType)1e-6) * 100;

   cout << "Tolerance = " << tolerance << "%." << endl;

   //
   // Spot tests use values computed by the R statistical
   // package and the pbeta and dbeta functions:
   //
   test_spot(
     RealType(2),                   // alpha
     RealType(5),                   // beta
     RealType(1),                   // non-centrality param
     RealType(2),                   // F statistic
     RealType(0.6493871),           // CDF
     RealType(1-0.6493871),         // Complement of CDF
     RealType(0.1551262),           // PDF
     RealType(tolerance));
   test_spot(
     RealType(100),                 // alpha
     RealType(5),                   // beta
     RealType(15),                  // non-centrality param
     RealType(105),                 // F statistic
     RealType(0.999962),            // CDF
     RealType(1-0.999962),          // Complement of CDF
     RealType(8.95623e-07),         // PDF
     RealType(tolerance));
   test_spot(
     RealType(100),                 // alpha
     RealType(5),                   // beta
     RealType(15),                  // non-centrality param
     RealType(1.5),                 // F statistic
     RealType(0.5759232),           // CDF
     RealType(1-0.5759232),         // Complement of CDF
     RealType(0.3674375),           // PDF
     RealType(tolerance));
   test_spot(
     RealType(5),                   // alpha
     RealType(100),                 // beta
     RealType(102),                 // non-centrality param
     RealType(25),                  // F statistic
     RealType(0.7499338),           // CDF
     RealType(1-0.7499338),         // Complement of CDF
     RealType(0.0544676),           // PDF
     RealType(tolerance));
   test_spot(
     RealType(85),                  // alpha
     RealType(100),                 // beta
     RealType(245),                 // non-centrality param
     RealType(3.5),                 // F statistic
     RealType(0.2697244),           // CDF
     RealType(1-0.2697244),         // Complement of CDF
     RealType(0.5435237),           // PDF
     RealType(tolerance));
   test_spot(
     RealType(85),                  // alpha
     RealType(100),                 // beta
     RealType(0.5),                 // non-centrality param
     RealType(1.25),                // F statistic
     RealType(0.8522862),           // CDF
     RealType(1-0.8522862),         // Complement of CDF
     RealType(0.8851028),           // PDF
     RealType(tolerance));

   BOOST_MATH_STD_USING

   //
   // 5 eps expressed as a persentage, otherwise the limit of the test data:
   //
   RealType tol2 = (std::max)(boost::math::tools::epsilon<RealType>() * 500, RealType(1e-25));
   RealType x = 2;

   boost::math::non_central_f_distribution<RealType> dist(20, 15, 30);
   // mean:
   BOOST_CHECK_CLOSE(
      mean(dist)
      , static_cast<RealType>(2.8846153846153846153846153846154L), tol2);
   // variance:
   BOOST_CHECK_CLOSE(
      variance(dist)
      , static_cast<RealType>(2.1422807961269499731038192576654L), tol2);
   // std deviation:
   BOOST_CHECK_CLOSE(
      standard_deviation(dist)
      , sqrt(variance(dist)), tol2);
   // hazard:
   BOOST_CHECK_CLOSE(
      hazard(dist, x)
      , pdf(dist, x) / cdf(complement(dist, x)), tol2);
   // cumulative hazard:
   BOOST_CHECK_CLOSE(
      chf(dist, x)
      , -log(cdf(complement(dist, x))), tol2);
   // coefficient_of_variation:
   BOOST_CHECK_CLOSE(
      coefficient_of_variation(dist)
      , standard_deviation(dist) / mean(dist), tol2);
   BOOST_CHECK_CLOSE(
      median(dist),
      quantile(
      dist,
      static_cast<RealType>(0.5)), static_cast<RealType>(tol2));
   // mode:
   BOOST_CHECK_CLOSE(
      mode(dist)
      , static_cast<RealType>(2.070019130232759428074835788815387743293972985542L), sqrt(tolerance));
   // skewness:
   BOOST_CHECK_CLOSE(
      skewness(dist)
      , static_cast<RealType>(2.1011821125804540669752380228510691320707051692719L), tol2);
   // kurtosis:
   BOOST_CHECK_CLOSE(
      kurtosis(dist)
      , 3 + kurtosis_excess(dist), tol2);
   // kurtosis excess:
   BOOST_CHECK_CLOSE(
      kurtosis_excess(dist)
      , static_cast<RealType>(13.225781681053154767604638331440974359675882226873L), tol2);
} // template <class RealType>void test_spots(RealType)

int test_main(int, char* [])
{
   BOOST_MATH_CONTROL_FP;
   // Basic sanity-check spot values.
   expected_results();
   // (Parameter value, arbitrarily zero, only communicates the floating point type).
   test_spots(0.0F); // Test float.
   test_spots(0.0); // Test double.
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_spots(0.0L); // Test long double.
#if !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x582))
   test_spots(boost::math::concepts::real_concept(0.)); // Test real concept.
#endif
#endif

   return 0;
} // int test_main(int, char* [])


#include "NeuralNetwork/NeuralLayer/OpenCLNeuralLayer.h"
#include <NeuralNetwork/LearningAlgorithm/BackPropagation/BepAlgorithm.h>
#include <NeuralNetwork/NeuralLayer/ConvolutionLayer.h>
#include <NeuralNetwork/NeuralLayer/NeuralLayer.h>
#include <NeuralNetwork/Neuron/ActivationFunction/BiopolarSigmoidFunction.h>
#include <NeuralNetwork/Neuron/ActivationFunction/LogScaleSoftmaxFunction.h>
#include <NeuralNetwork/Neuron/ActivationFunction/SigmoidFunction.h>
#include <NeuralNetwork/Neuron/ActivationFunction/SoftmaxFunction.h>
#include <NeuralNetwork/Neuron/ActivationFunction/TanhFunction.h>
#include <NeuralNetwork/Neuron/ActivationFunction/ReluFunction.h>
#include <NeuralNetwork/Neuron/Neuron.h>
#include <NeuralNetwork/Perceptron/Perceptron.h>
#include <NeuralNetwork/NeuralLayer/OpenCLNeuralLayer.h>
#include <NeuralNetwork/Config.h>

#include <MPL/Tuple.h>

#ifndef BOOST_SYSTEM_NO_DEPRECATED
#define BOOST_SYSTEM_NO_DEPRECATED 1
#include <boost/filesystem.hpp>
#undef BOOST_SYSTEM_NO_DEPRECATED
#endif

#define png_infopp_NULL (png_infopp) NULL
#define int_p_NULL (int*)NULL

#if defined(NN_CC_MSVC)
#pragma warning(push)
// This function or variable may be unsafe
#pragma warning(disable : 4996)
#endif

#include <boost/gil/channel_algorithm.hpp>
#include <boost/gil/channel.hpp>
#include <boost/gil.hpp>
#include <boost/gil/image.hpp>

#include <boost/gil/extension/io/png/old.hpp>
#include <boost/gil/extension/numeric/sampler.hpp>
#include <boost/gil/extension/numeric/resample.hpp>
#include <boost/gil/extension/dynamic_image/any_image.hpp>
#include <boost/gil/extension/dynamic_image/dynamic_image_all.hpp>

#include <cereal/archives/json.hpp>
#include <cereal/types/array.hpp>
#include <cereal/types/tuple.hpp>
#include <cereal/types/vector.hpp>

#include <tuple>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <set>

typedef float VarType;

namespace {
    using namespace boost::gil;
    using namespace boost::gil::detail;
    const std::string alphabet("0123456789");
    constexpr std::size_t width = 49;
    constexpr std::size_t height = 67;
    constexpr std::size_t inputsNumber = width * height;
    constexpr std::size_t margin = 15;
    constexpr std::size_t stride = 10;
} // namespace

using ConvolutionGrid =
 typename nn::ConvolutionGrid< width, height, stride, margin >::define;

using ConvolutionGrid2 =
 typename nn::ConvolutionGrid< width / stride, height / stride, 2, 5 >::define;

using Perceptron =
 nn::Perceptron< VarType,
                 nn::ConvolutionLayer< nn::OpenCLNeuralLayer, nn::Neuron, nn::ReluFunction, inputsNumber, ConvolutionGrid >,
                 nn::NeuralLayer< nn::Neuron, nn::SoftmaxFunction, 10, 1000 > >;

using Algo = nn::bp::BepAlgorithm< Perceptron, nn::bp::CrossEntropyError >;

template< typename Out >
struct halfdiff_cast_channels {
    template< typename T >
    Out operator()(const T& in1, const T& in2) const {
        return Out((in1 - in2) / 2);
    }
};

template< typename SrcView, typename DstView >
void convert_color(const SrcView& src, const DstView& dst) {
    typedef typename channel_type< DstView >::type d_channel_t;
    typedef typename channel_convert_to_unsigned< d_channel_t >::type channel_t;
    typedef pixel< channel_t, gray_layout_t > gray_pixel_t;

    copy_pixels(color_converted_view< gray_pixel_t >(src), dst);
}

template< typename Iterator >
void readImage(std::string fileName, Iterator out) {
    using namespace boost::gil;
    using namespace nn;
    using namespace nn::bp;

    rgb8_image_t srcImg;
    png_read_image(fileName.c_str(), srcImg);

    gray8_image_t dstImg(srcImg.dimensions());
    gray8_pixel_t white(255);
    fill_pixels(view(dstImg), white);

    gray8_image_t grayImage(srcImg.dimensions());
    convert_color(view(srcImg), view(grayImage));
    auto grayView = view(grayImage);

    gray8_image_t scaledImage(width, height);
    resize_view(grayView, view(scaledImage), bilinear_sampler());
    auto srcView = view(scaledImage);

    for(int y = 0; y < srcView.height(); ++y) {
        gray8c_view_t::x_iterator src_it(srcView.row_begin(y));
        for(int x = 0; x < srcView.width(); ++x) {
            *out = src_it[x];
            out++;
        }
    }
}

Perceptron readPerceptron(std::string fileName) {
    Perceptron perceptron;
    if(boost::filesystem::exists(fileName.c_str())) {
        std::ifstream file(fileName);
        if(file.good()) {
            Perceptron::Memento memento;
            cereal::JSONInputArchive ia(file);
            ia >> memento;

            perceptron.setMemento(memento);
        } else {
            throw std::logic_error("Invalid perceptron file name");
        }
    }

    return perceptron;
}

void recognize(std::string perceptron, std::string image) {
    try {
        std::array< VarType, inputsNumber > inputs = {0};
        readImage(image, inputs.begin());
        std::vector< VarType > result(alphabet.length(), VarType(0.f));
        readPerceptron(perceptron)
         .calculate(inputs.begin(), inputs.end(), result.begin());
        for(unsigned int i = 0; i < result.size(); i++) {
            std::cout << "Symbol: " << alphabet[i] << " " << result[i] << std::endl;
        }
    } catch(const std::exception& e) {
        std::cout << e.what() << std::endl;
    } catch(...) {
        std::cout << "Unknown error" << std::endl;
    }
}

template< typename Perc >
void save(const Perc& perc, std::string name) {
    typename Perc::Memento memento = perc.getMemento();
    std::ofstream strm(name);
    cereal::JSONOutputArchive oa(strm);
    oa << memento;
    strm.flush();
}

void calculateWeights(std::string imagesPath) {
    using namespace boost::filesystem;
    path directory(imagesPath);
    directory_iterator end_iter;

    std::vector< std::string > files;
    if(exists(directory) && is_directory(directory)) {
        for(directory_iterator dir_iter(directory); dir_iter != end_iter; ++dir_iter) {
            if(is_regular_file(dir_iter->status())) {
                files.push_back(dir_iter->path().string());
            }
        }
    }

    std::cout << "Perceptron calculation started" << std::endl;
    static Perceptron tmp = readPerceptron("perceptron.json");
    static Algo algorithm(0.05f);
    algorithm.setMemento(tmp.getMemento());

    std::vector< Algo::Prototype > prototypes;
    for(auto image : files) {
        if(!boost::filesystem::is_directory(image)) {
            try {
                Algo::Prototype proto;
                readImage(image, std::get< 0 >(proto).begin());
                std::fill(std::get< 1 >(proto).begin(), std::get< 1 >(proto).end(), 0.f);
                char ch = path(image).filename().string()[0];
                size_t pos = alphabet.find(ch);
                std::get< 1 >(proto)[pos] = 1.0f;
                prototypes.push_back(proto);
            } catch(const std::exception&) {
                std::cout << "Invalid image found :" << image << std::endl;
            }
        }
    }

    auto errorFunc = [](unsigned int epoch, VarType error) {
        // if(epoch % 3 == 0) {
        std::cout << "Epoch:" << epoch << " error:" << error << std::endl;
        //}

        return error > 0.001f;
    };

    static Perceptron perceptron =
     algorithm.calculate(prototypes.begin(), prototypes.end(), errorFunc);

    save(perceptron, "perceptron.json");
}

int main(int argc, char** argv) {
    int result = -1;
    if(argc == 3) {
        recognize(argv[1], argv[2]);
        result = 0;
    } else if(argc == 2) {
        calculateWeights(argv[1]);
        result = 0;
    } else {
        std::cout << std::endl << "Usage : " << std::endl << std::endl;
        std::cout
         << "./ocr [folder] where  [folder] is a directory with your "
            "samples, this command will generate a perceptron.json file"
         << std::endl
         << std::endl;
        std::cout << "./ocr perceptron.json [file] where [file] is a png image "
                     "which has to be recognized"
                  << std::endl
                  << std::endl;
    }

    return result;
}


#include <cmath>
#include <ctime>
#include <vector>
#include <string>
#include <thread>
#include <fstream>
#include <iostream>
#include <ros/ros.h>
#include <algorithm>
#include <Eigen/Dense>
#include <ros/package.h>
#include <ros/spinner.h>
#include <opencv2/opencv.hpp>
#include <geometry_msgs/Twist.h>
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Float32.h"

using namespace cv;
using namespace std;
using namespace Eigen;

int itr = 1;

class FormControl
{
private:
  vector<string> rob_names_, rob_ctrl_topic_names_,
                           rob_ang_vel_topic_names_;

  // node handle and Subscribe and Publish topics
  ros::NodeHandle nh_;
  ros::Subscriber pos_world_sub_, pos_image_sub_;
  vector<ros::Publisher> ctrl_cmd_pub_array_, ang_vel_pub_array_;

  // variables used for formation control and PID controller
  vector<double> ctrl_, vd_, thte_, thtc_, thtc_old_, thtr_, tht0_old_,
                      tht0_new_, a_hat_old_, a_hat_new_, b_hat_old_, b_hat_new_,
                      error_sum_, vm_new_, vm_old_, vel_, ang_;
  vector<MatrixXd> pos_world_history_, pos_image_history_;
  double t_old_, t_new_, v_est_;
  int mem_, mem_filled_, num_rob_, thresh_pix_;

  int num_rows, idx;
  // variables for robot position
  std_msgs::Float64MultiArray pos_world_new_, pos_image_new_;

  // GAINS and thresholds:
  double vd_gain_, v_gain_, vmax_, km_, ka_, kh_, kp_, ki_, a_bound_, b_bound_,
        gain_, speed_gain_,  delay_;
  bool update, first_time, use_adaptive_vel_ctrl, use_PID_ctrl;
  Matrix<double, 6, 6> lnr_mat; // 3 rob

  string window_name, window_name2, rob_name, rob_id;

  // file stream. To be removed for final version
  ofstream outfile;
  ros::AsyncSpinner spinner;

public:
FormControl()
:  window_name("heading command"), window_name2("reconstruction"), first_time(true),
  num_rows(-1), use_adaptive_vel_ctrl(false), use_PID_ctrl(true),
  spinner(2)
{
  double L [6][6];
}

~FormControl() {   // class destructor
    // file stream. To be removed for final version
    outfile.close();
}

  void run(){
    spawn();
    unspawn();
  }

  std_msgs::Float64MultiArray get_pos_world_new()   {
    return pos_world_new_;
  }

  std_msgs::Float64MultiArray get_pos_image_new() {
    return pos_image_new_;
  }

private:

  void spawn(){
    if(spinner.canStart())
      spinner.start();

    thread form_ctrl_thread = thread(&FormControl::form_ctrl, this);

    populateRobots();

    // std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    initParams();

    pos_image_sub_ = nh_.subscribe("/locs/ordered", 1, &FormControl::pos_image_callback, this);
    pos_world_sub_ = nh_.subscribe("/pos/world", 1, &FormControl::pos_world_callback, this);

  }

  void unspawn(){
    spinner.stop();
  }

  void populateRobots(){

    get_pos_world_new();
    get_pos_image_new();

    ros::param::get("/form_ctrl_node/Rob_Params/num_rob", num_rob_); // number of robots
    for (auto i = 0; i < num_rob_; ++i)  {
      std::string rob_id = "sph" + std::to_string(i);
      std::string rob_name;
      ros::param::get("/form_ctrl_node/Rob_Params/"+rob_id, rob_name);
      rob_names_.push_back(rob_name);
      rob_ctrl_topic_names_.push_back("/" + rob_names_[i] + "/cmd_vel");
      rob_ang_vel_topic_names_.push_back("/" + rob_names_[i] + "/set_angular_velocity");
      ctrl_cmd_pub_array_.push_back(nh_.advertise<geometry_msgs::Twist>(rob_ctrl_topic_names_[i], 1));
      ang_vel_pub_array_.push_back(nh_.advertise<std_msgs::Float32>(rob_ang_vel_topic_names_[i], 1));
    }
    pos_image_sub_ = nh_.subscribe("/locs/ordered", 1, &FormControl::pos_image_callback, this);
    pos_world_sub_ = nh_.subscribe("/pos/world", 1, &FormControl::pos_world_callback, this);
  }

  template <typename T>
  void retrieveParam(std::string base, std::string param, T value) const  {
    ros::param::get(base + param, value);
  }

  void form_ctrl() {
      int iitr = 0;
      for (int num_rob = 0; num_rob < num_rob_; ++num_rob)  {
        iitr = --itr * num_rob_ + (++num_rob);
        if (iitr == 1) {// initialize some variables in first iteration
          tht0_new_[num_rob] = 0;   tht0_old_[num_rob] = 0;
          thtc_[num_rob] = 0;       thtc_old_[num_rob] = 0;
        }

        t_new_ = static_cast<double>(clock())/CLOCKS_PER_SEC;

        if (iitr == 1) {// initialize some variables in first iteration
          for (int i = 0; i < num_rob_; ++i)      {
            double x_init_w, y_init_w;
            x_init_w = pos_world_new_.data[2*i];
            y_init_w = pos_world_new_.data[2*i+1];

            double x_init_i, y_init_i;
            x_init_i = pos_image_new_.data[2*i];
            y_init_i = pos_image_new_.data[2*i+1];

            for (int j = 0; j < mem_+1; ++j)        {
              pos_world_history_[i](0,j) = t_new_;
              pos_world_history_[i](1,j) = x_init_w;
              pos_world_history_[i](2,j) = y_init_w;

              pos_image_history_[i](0,j) = t_new_;
              pos_image_history_[i](1,j) = x_init_i;
              pos_image_history_[i](2,j) = y_init_i;
            }
          }
        }

        if (itr >= 2)    {
          estimate_heading(iitr, num_rob); // estimate the robot's heading (thte_)
        }

        form_ctrl_free_scale(); //apply free-scale formation control and find control commands (ctrl_)

        find_vd(); // find desired velocity (vd_) based on ctrl_

        double v [2];
        v[0] = ctrl_[2*num_rob];
        v[1] = ctrl_[2*num_rob + 1];

        if (idx != -1) // idx != -1 if sufficient motion exists
        {
            // find control angle thtc_
            thtc_[num_rob] = bound(atan2(v[1],v[0]) * 180 / M_PI); // angle in degrees
        }

        if (itr >= 2)
        {
          // estimate tht0: initial heading offset of the robot
          estimate_tht0(thtc_old_[num_rob], thte_[num_rob], num_rob); //TODO: write code of fxn
        }

        thtr_[num_rob] = thtc_[num_rob] - tht0_new_[num_rob]; // find robot velocity command angle in degrees
        thtr_[num_rob] = bound(thtr_[num_rob]);
        outfile << "##############Comparing thtc and thte#############" <<endl;
        outfile << "THTC = " << thtc_[num_rob] << endl;
        outfile << "THTE = " << thte_[num_rob] << endl;
        outfile << "Ideally, they should be equal" << endl;
        outfile << "##################################################" <<endl;

        double vel_temp; // Use PID controller
        if (itr == 1)
        {
          vm_new_ [num_rob] = 0;
          a_hat_new_ [num_rob] = 0;
          b_hat_new_ [num_rob] = 0;
        }
        else // itr >= 2
        {
          if (use_adaptive_vel_ctrl == true)
          {
            // apply_adaptive_vel_ctrl(num_rob, dt);

          }
          else
          {
              if (use_PID_ctrl == true)
                      vel_temp = apply_pid_ctrl(num_rob);
          }
        }

        // find speed command value
        double vel;
        if (use_adaptive_vel_ctrl == true) // adaptive_ctrl doesn't work yet
            vel = a_hat_new_[num_rob]*v_est_ + b_hat_new_[num_rob]*vd_[num_rob];
        else   {
          if (use_PID_ctrl == true)
            vel = vel_temp;
        }

        vel = min(vel,vmax_);
        vel = find_max(vel, 0.01);
        vel_[num_rob] = vel;

        if (idx != -1)    {
            ang_[num_rob] = thtr_[num_rob]; // update command velocity angle when sufficient motion exists
        }

        cout << "robot: " << num_rob << endl << " vel: " << vel_[num_rob] << "  ang: " << ang_[num_rob] << endl;//////////////////////////////////////////////////////////////////

        // find x and y velocity commands
        double vel_x = vel_[num_rob] * cos(ang_[num_rob]*M_PI/180);
        double vel_y = vel_[num_rob] * sin(ang_[num_rob]*M_PI/180);

        // issue commands;
        std_msgs::Float32 set_angular_velocity_command;
        set_angular_velocity_command.data = 255.0;

        geometry_msgs::Twist command;
        command.linear.x = static_cast<float>(vel_x);
        command.linear.y = static_cast<float>(vel_y);
        command.linear.z = 0;
        command.angular.x = 0;
        command.angular.y = 0;
        command.angular.z = 0;

        ang_vel_pub_array_ [num_rob].publish(set_angular_velocity_command);
        ctrl_cmd_pub_array_ [num_rob].publish(command);
        ros::Duration(delay_).sleep();

        outfile << "itr: " << itr << " iitr: " << iitr << "   rob num: " << num_rob;
        outfile << " estimated tht0: " << tht0_new_[num_rob];
        outfile << " command vel: " << vel_[num_rob] << " command heading: " << ang_[num_rob];
        outfile << endl << " pos world: " << endl;
        outfile << pos_world_new_;
        outfile << " x_vel: " << vel_x << " y_vel: " << vel_y << endl;
        for ( int i = 0; i < num_rob_; ++i)    {
          outfile << "pos_world_history[" << i <<"] = " << pos_world_history_[i] << endl;
          outfile << "pos_image_history[" << i <<"] = " << pos_image_history_[i] << endl <<endl;
        }

        if (num_rob == (--num_rob_))
          outfile << endl << endl << endl << endl << endl;

        ros::spinOnce();

        // Display visual feedback
        Mat frame(480, 640, CV_8UC3, Scalar(0)); //image frame and thtc, thte, thtr vectors
        Mat frame_w(400, 400, CV_8UC3, Scalar(0)); // 3d reconstruction frame
        for ( int i = 0; i < num_rob_; ++i )
        {
          Point center;
          center.x = pos_image_new_.data[2*i];
          center.y = pos_image_new_.data[2*i+1];
          int radius = 5;
          int thickness = 3;
          circle(frame, center, radius, Scalar(0, 255, 255), thickness); // display robot in image frame

          Point center_world, origin_world, x_dir_world, y_dir_world;
          origin_world.x = 200 * (0 + 1);
          origin_world.y = 200 * (-0 + 1);
          x_dir_world.x = 200 * (0.5 + 1);
          x_dir_world.y = 200 * (-0 + 1);
          y_dir_world.x = 200 * (0 + 1);
          y_dir_world.y = 200 * (-0.5 + 1);
          center_world.x = 200 * (pos_world_new_.data[2*i] +1);
          center_world.y = 200 * (-pos_world_new_.data[2*i+1] +1); // flip y to plot in frame with z pointing down.
          circle(frame_w, center_world, radius, Scalar(0, 255, 255), thickness); // display robot in 3d reconstruction frame
          line(frame_w, origin_world, x_dir_world, Scalar(255,0,0), thickness); // x-axis in 3d reconstruction
          line(frame_w, origin_world, y_dir_world, Scalar(0,0,255), thickness); // y-axis in 3d reconstruction

          if (i == num_rob)
          {
            Point end_pt;
            end_pt.x = (center.x + vel_x);
            end_pt.y = (center.y - vel_y);
            line(frame, center, end_pt, Scalar(0, 255, 0), thickness); //  command velocity vector (green)
          }

          Point end_pt_heading;
          end_pt_heading.x = (center.x + 20*cos(thte_[i]*M_PI/180));
          end_pt_heading.y = (center.y - 20*sin(thte_[i]*M_PI/180));
          line(frame, center, end_pt_heading, Scalar(255, 50, 0), thickness); // estimated heading (blue)

          Point end_pt_tht0;
          end_pt_tht0.x = (center.x + 20*cos(tht0_new_[i]*M_PI/180));
          end_pt_tht0.y = (center.y - 20*sin(tht0_new_[i]*M_PI/180));
          line(frame, center, end_pt_tht0, Scalar(0, 0, 255), thickness); // estimated tht0 (red)

          Point txt_pt;
          txt_pt.x = min(center.x + 5, 630);
          txt_pt.y = max(center.y - 5, 0);
          putText(frame, std::to_string(i+1), txt_pt, FONT_HERSHEY_PLAIN, 2, Scalar(255,255,255), 1); // robot tag number
        }
        // outputVideo << frame;
        imshow(window_name, frame);
        imshow(window_name2, frame_w);

        int key = waitKey(1);
        updata_history (num_rob);

        ros::spinOnce();
      }
      update_old_new_variables();
    }

  void initParams(){
    // initializing variables used for formation control and PID controller
    ctrl_.resize(2*num_rob_);
    vd_.resize(num_rob_);
    t_old_ = 0;
    t_new_ = 0;
    thte_.resize(num_rob_);
    thtc_.resize(num_rob_);
    thtc_old_.resize(num_rob_);
    thtr_.resize(num_rob_);
    tht0_old_.resize(num_rob_);
    tht0_new_.resize(num_rob_);
    v_est_ = 0;
    mem_ = 0; //5
    mem_filled_ = 0;
    a_hat_old_.resize(num_rob_);
    a_hat_new_.resize(num_rob_);
    b_hat_old_.resize(num_rob_);
    b_hat_new_.resize(num_rob_);
    vm_new_.resize(num_rob_);
    vm_old_.resize(num_rob_);
    vel_.resize(num_rob_);
    ang_.resize(num_rob_);
    for (int i = 0; i < num_rob_; ++i)  {
            ang_[i] = 0;
    }
    error_sum_.resize(num_rob_);

    // initialize robot position variables
    pos_world_new_.data.clear();
    pos_image_new_.data.clear();

    pos_world_history_.resize(num_rob_);
    pos_image_history_.resize(num_rob_);
    for (int i = 0; i < num_rob_; ++i)
    {
      pos_world_history_[i]= MatrixXd(3, mem_+1);
      pos_image_history_[i]= MatrixXd(3, mem_+1);
    }

    // initializing GAINS and thresholds
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "vd_gain", vd_gain_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "v_gain_", v_gain_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "kh_", kh_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "thresh_pix_", thresh_pix_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "vmax_", vmax_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "km_", km_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "ka_", ka_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "a_bound_", a_bound_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "b_bound_", b_bound_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "gain_", gain_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "speed_gain_", speed_gain_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "kp_", kp_);
    retrieveParam("/form_ctrl_node/Gains_Thresholds/", "ki_", ki_);
    retrieveParam("/form_ctrl_node/Delay/", "delay_", delay_);

    namedWindow(window_name, WINDOW_NORMAL);
    resizeWindow(window_name, 640, 480);
    namedWindow(window_name2, WINDOW_NORMAL);
    resizeWindow(window_name2, 400, 400);

    std::string package_name = ros::package::getPath("form_ctrl");
    outfile.open(package_name + "/" + "config" + "/" + "pub_data.txt");
  }

  void pos_world_callback(const std_msgs::Float64MultiArray& pos_world_new)  {
      this->pos_world_new_ = pos_world_new;
  }

  void pos_image_callback(const std_msgs::Float64MultiArray& pos_image_new)  {
      this->pos_image_new_ = pos_image_new;
  }

  void form_ctrl_free_scale()   // free scale formation control function
  {
      if (first_time)
      {
        first_time = false;
        // x,y pairs for the shape
        // N-gon formation (circle)
        double sep_ang = 360 / num_rob_;
        double q_ang [num_rob_];
        for (int i = 0; i < num_rob_; ++i)        {
          q_ang[i] = sep_ang*i;
        }
        double pos_des [2][this->num_rob_];
        for (int i = 0; i < this->num_rob_; ++i)        {
            pos_des[0][i] = cos(q_ang[i] * M_PI / 180.0);
            pos_des[1][i] = sin(q_ang[i] * M_PI / 180.0);
        }
        // adjacency matrix of all graphs with 4 agents
        int num_graphs = 1;
        double adj [this->num_rob_][this->num_rob_][num_graphs] = {0};
        for (int i = 0; i < this->num_rob_; ++i)
        {
          for (int j = 0; j < this->num_rob_; ++j)  {
              if (i==j)    {
                  adj[i][j][0] = 0;
              }
              else     {
                  adj[i][j][0] = 1;
              }
          }
        }

        // TODO: add rearrangment of robots to travel shorter distance.

        // TODO: find gains that stabilize all adjacency matrices, find laplacian matrix and normalize it

        // get real laplacian matrix data produced by Matlab, store it in L, and normalize it
        std::vector<double> lnr_3;
        // std::vector<double> lnr_3_temp;
        //
        // std::string temp_lnr_name,temp_lnr_name_base;
        // this->nh_.getParam("/form_ctrl_node/CVX_Optimization/rows", num_rows);
        // temp_lnr_name_base = "LnR_3_";
        // lnr_3.resize(num_rows,num_rows);
        // for (int i = 0; i < num_rows; ++i)
        // {
        //   temp_lnr_name = "/form_ctrl_node/CVX_Optimization/" + temp_lnr_name_base + to_string(i+1);
        //   this->nh_.getParam(temp_lnr_name, lnr_3_temp);
        //
        //   for(int j = 0; j < num_rows; ++j)
        //   {
        //     lnr_3[num_rows*i + j] = lnr_3_temp[j];
        //   }
        //
        // }
        //

        nh_.getParam("/form_ctrl_node/CVX_Optimization/rows", num_rows);
        nh_.getParam("/form_ctrl_node/CVX_Optimization/LnR_3", lnr_3);
        double max_lnr = 0;

        for (int i = 0; i < num_rows; ++i)        {
          for (int j = 0; j < num_rows; ++j)
          {
                  lnr_mat(i,j) = lnr_3[num_rows*i+j];
                  max_lnr = find_max (max_lnr, abs(lnr_mat(i,j)));
          }
        }
        if (max_lnr != 0)
          lnr_mat / max_lnr;
        outfile << "LnR matrix : " << endl;
        outfile << lnr_mat << endl;
      }

      MatrixXd q;
      q.resize(2*num_rob_,1);
      double q_max = 0;
      for (int i = 0; i < 2*num_rob_; ++i)
      {
        q(i) = pos_world_new_.data[i];
        q_max = find_max(q_max, abs(q(i)));
      }
      if (q_max != 0)
          q /= q_max;

      MatrixXd dq;
      dq.resize(2 * num_rob_,1);
      dq = lnr_mat*q;

      for (int i = 0; i < 2*num_rob_; ++i)
      {
        ctrl_[i] = dq(i);
      }
  }

  void find_vd()   {// find desired velocity
    bool scale = false;
    double vd_max = 0;
    for (int i = 0; i < this->num_rob_; ++i)
    {
      this->vd_[i] = this->vd_gain_ * find_norm(this->ctrl_[2*i], this->ctrl_[2*i+1]);
      if (this->vd_[i] > this->vmax_) // check if any vd value is larger than max velocity
      {
        scale = true;
        if (this->vd_[i]>vd_max)
        {
                vd_max = this->vd_[i];
        }
      }
    }

    if (scale) // if there exists a vd value > max vel, scale all vd values
    {
      double scale_factor = this->vmax_ / vd_max;
      for (int i = 0; i < this->num_rob_; ++i)
      {
        this->vd_[i] *= scale_factor;
      }
    }
  }

  void estimate_heading (int iitr, int num_rob)   // estimate heading of robot based on data from image
  {
    MatrixXd pos_w; pos_w.resize(3, mem_+1); // with time
    MatrixXd pos_i; pos_i.resize(3, mem_+1); // with time
    pos_w = this->pos_world_history_[num_rob];
    pos_i = this->pos_image_history_[num_rob];

    MatrixXd d_pos_w; d_pos_w.resize(2, mem_+1); // difference in xy position
    MatrixXd d_pos_i; d_pos_i.resize(2, mem_+1); // difference in xy position

    outfile << "~~~~~~~~~~~~~~~~~~~~~~Heading estimation~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << endl;
    outfile << "Robot Number = " << num_rob << endl;
    outfile << "History location:" <<endl;
    outfile << "      WORLD: " <<endl << pos_w << endl;
    outfile << "      IMAGE: " <<endl << pos_i << endl;
    outfile << "New location:" <<endl;
    outfile << "      WORLD: " <<endl << pos_world_new_.data[2*num_rob] << ", " << pos_world_new_.data[2*num_rob+1]  << endl;
    outfile << "      IMAGE: " <<endl << pos_image_new_.data[2*num_rob] << ", " << pos_image_new_.data[2*num_rob+1]  << endl;

    for (int i = 0; i < mem_+1; ++i)
    {
            for (int j = 0; j < 2; ++j)
            {
                    d_pos_w(j,i) = pos_world_new_.data[2*num_rob+j] - pos_w(j+1,i);
                    d_pos_i(j,i) = pos_image_new_.data[2*num_rob+j] - pos_i(j+1,i);
            }
    }

    outfile << "Difference between new and history x and y: " << endl;
    outfile << "      WORLD: " <<endl << d_pos_w << endl;
    outfile << "      IMAGE: " <<endl << d_pos_i << endl;

    double dist = find_norm( d_pos_w(0, mem_), d_pos_w(1, mem_) );

    outfile << "Distance: " << endl;

    double dist_pix [mem_ + 1];
    for (int i = 0; i < mem_ + 1; ++i)
    {
      dist_pix[i] = find_norm(d_pos_i(0,i), d_pos_i(1,i));
    }

    outfile << "      WORLD: " <<endl << dist << endl;
    outfile << "      IMAGE: " <<endl << dist_pix[0] << endl;

    idx = -1;
    for (int i = 0; i < mem_ + 1; ++i)
    {
      if (dist_pix[i] > this->thresh_pix_)
      {
        idx = i;
      }
    }

    if (idx != -1)
    {
      double hd [2];
      hd[0] = d_pos_w(0,idx);
      hd[1] = d_pos_w(1,idx);
      this->thte_[num_rob] = atan2(hd[1],hd[0]) * 180 / M_PI; // angle in degrees
      this->thte_[num_rob] = bound(this->thte_[num_rob]);
      // this->thte_[num_rob] = (this->thte_[num_rob]);

      double dt;
      dt = this->t_new_ - this->pos_world_history_[num_rob](0,idx);

      this->v_est_ = this->v_gain_ * dist / dt;
      outfile << " DISTANCE = " << dist <<endl;
      outfile << " DT = " << dt <<endl;
      outfile << " v_est_ = " << this->v_est_ << endl;
    }
    else
    {
      this->v_est_ = 0;
      outfile << " v_est_ = " << this->v_est_ << endl;
    }

    outfile << "Is pixel distance greater than threshold? :: " << endl;
    bool isit = dist_pix[0] > this->thresh_pix_;
    outfile << isit << endl;

    // cout << "vest  = " << v_est_ <<endl;
    outfile << "Estimated heading: " <<endl << this->thte_[num_rob] << endl;
    outfile << "~~~~~~~~~~~~~~~~~~~~~~Heading estimation DONE~~~~~~~~~~~~~~~~~~~~~~~" << endl;
  }

  void estimate_tht0(double thtc, double thte, int num_rob) // estimate tht0, the initial heading of the robot
  {
    // cout<<"Estimating tht 0"<<endl; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    outfile << "````````````````````````````````THT0 estimation````````````````````````````````" << endl;

    if (isnan(thte) || idx == -1)
    {
      this->tht0_new_[num_rob] = this->tht0_old_[num_rob];
      outfile << "keeping old value: " << tht0_new_[num_rob] << endl;
    }
    else
    {
      double tht_diff = ang_diff(thtc, thte); // thte-thtc angle in degrees

      this->tht0_new_[num_rob] = this->kh_ * (tht_diff - this->tht0_old_[num_rob]) + this->tht0_old_[num_rob];

      outfile << "calculating new value: " << endl;
      outfile << "thte = " << thte << endl;
      outfile << "thtc = " << thtc << endl;
      outfile << "tht_diff = " << tht_diff << endl;
      outfile << "kh_ * tht_diff = " << this->kh_ * tht_diff << endl;
      outfile << "tht0 old = " << this->tht0_old_[num_rob] << endl;
      outfile << "new value: " << tht0_new_[num_rob] << endl;
    }
    outfile << "````````````````````````````````THT0 estimation DONE```````````````````````" << endl;
  }

  void apply_adaptive_vel_ctrl(int num_rob, double dt) // adaptive velocity control
  {
    // from v 8.2 (Matlab)
    // this->vm_new_[num_rob] = dt * (-this->km_*(this->vm_old_[num_rob] - this->vm_new_[num_rob])) + this->vm_old_[num_rob];
    // double ev = this->v_est_ - this->vm_new_[num_rob];
    // this->a_hat_new_[num_rob] = dt * (-this->ka_ * ev * this->v_est_) + this->a_hat_old_[num_rob];
    // this->a_hat_new_[num_rob] = find_sign(this->a_hat_new_[num_rob]) * min(abs(this->a_hat_new_[num_rob]), this->a_bound_);
    // this->b_hat_new_[num_rob] = dt * (-this->ka_ * ev * this->v_est_) + this->b_hat_old_[num_rob];
    // this->b_hat_new_[num_rob] = find_sign(this->b_hat_new_[num_rob]) * min(abs(this->b_hat_new_[num_rob]), this->b_bound_);

    // from v 8.1 (Matlab)
    // cout<<"Applying Adaptive vel ctrl"<<endl; /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    this->vm_new_[num_rob] = dt * (-this->km_*(this->vm_old_[num_rob] - this->vd_[num_rob])) + this->vm_old_[num_rob];
    double ev = this->v_est_ - this->vm_new_[num_rob];
    this->a_hat_new_[num_rob] = dt * (-this->ka_ * ev * this->v_est_) + this->a_hat_old_[num_rob];
    this->a_hat_new_[num_rob] = find_max(this->a_hat_new_[num_rob], 0.0);
    this->b_hat_new_[num_rob] = dt * (-this->ka_ * ev * this->v_est_) + this->b_hat_old_[num_rob];
    this->b_hat_new_[num_rob] = find_max(this->b_hat_new_[num_rob], 0.0);
  }

  double apply_pid_ctrl(int num_rob) // PID controller
  {
    // cout << "dt = " << dt <<endl;

    double v_req = this->gain_ * find_norm(this->ctrl_[2*num_rob], this->ctrl_[2*num_rob +1]);
    outfile << endl << "v_req = " << v_req;
    outfile << endl << "v_est = " << v_est_;
    double error = v_req - this->speed_gain_* this->v_est_;
    outfile << endl << "error = " << error;

    this->error_sum_[num_rob] = this->error_sum_[num_rob] + error;
    outfile << endl << "error_sum[num_rob]= " << this->error_sum_[num_rob] <<endl;

    double v_temp = this->kp_ * error + this->ki_ * this->error_sum_[num_rob];
    return v_temp;
  }

  void update_old_new_variables()   // set store xxx_new_ varaibles in xxx_old_
  {
    for (int i = 0; i < this->num_rob_; ++i)
    {
      this->tht0_old_[i] = this->tht0_new_[i];
      this->a_hat_old_[i] = this->a_hat_new_[i];
      this->b_hat_old_[i] = this->b_hat_new_[i];
      this->vm_old_[i] = this->vm_new_[i];
      this->thtc_old_[i] = this->thtc_[i];
    }
  }

  void updata_history (int num_rob) // update position history
  {
    if (idx != -1)
    {

      push_back_history(num_rob);
      this->pos_world_history_[num_rob](0,this->mem_) = this->t_new_;
      this->pos_world_history_[num_rob](1,this->mem_) = this->pos_world_new_.data[2*num_rob];
      this->pos_world_history_[num_rob](2,this->mem_) = this->pos_world_new_.data[2*num_rob+1];

      this->pos_image_history_[num_rob](0,this->mem_) = this->t_new_;
      this->pos_image_history_[num_rob](1,this->mem_) = this->pos_image_new_.data[2*num_rob];
      this->pos_image_history_[num_rob](2,this->mem_) = this->pos_image_new_.data[2*num_rob+1];
    }
  }

  void push_back_history(int num_rob)
  {
    for (int i = 0; i < this->mem_; ++i)
    {
        pos_world_history_[num_rob](0,i) = pos_world_history_[num_rob](0,i+1);
        pos_world_history_[num_rob](1,i) = pos_world_history_[num_rob](1,i+1);
        pos_world_history_[num_rob](2,i) = pos_world_history_[num_rob](2,i+1);

        pos_image_history_[num_rob](0,i) = pos_image_history_[num_rob](0,i+1);
        pos_image_history_[num_rob](1,i) = pos_image_history_[num_rob](1,i+1);
        pos_image_history_[num_rob](2,i) = pos_image_history_[num_rob](2,i+1);
    }
  }

  // helper functions
  double find_norm(double val0, double val1)   // find length using euclidean distance rule
  {
    double norm_val = sqrt(pow(val0,2) + pow(val1,2));
    return norm_val;
  }

  double find_max(double val0, double val1) // find max of two numbers
  {
    if (val0 > val1)
      return val0;
    else
      return val1;
  }

  double ang_diff(double thtc, double thte) // find thte-thtc in interval [-pi, pi]
  {
    double thtc_r, thte_r;
    thtc_r = thtc /180 * M_PI; // angle in radian
    thte_r = thte /180 * M_PI; // angle in radian
    double diff = thte_r - thtc_r; // angle in radian
    while (diff > M_PI || diff < -M_PI)
    {
      if (diff < -M_PI)
      {
        diff +=2*M_PI; // angle in radian// angle in radian
      }
      else // (diff > M_PI)
      {
        diff -= 2*M_PI; // angle in radian
      }
    }
    diff = diff *180 / M_PI;  // angle becomes in degrees
    return diff; // angle in degrees
  }

  template <typename T>
  int find_sign(T val) // find sign of a number
  {
          return (T(0) <  val) - (val < T(0));
  }

  double bound (double val) // bound the angle (val) between 0 and 360 degrees
  {
    while (val > 360 || val < 0){
      if (val > 360)
        val -= 360;
      else // (val < 0)
        val +=  360;
    }

    return val;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "form_ctrl_node");

  ros::NodeHandle nh;
  FormControl fc;

  // continue spinning until world and image position data is fetched
  // while (fc.get_pos_world_new().data.empty() || fc.get_pos_image_new().data.empty())
  // {
  //         // ros::spinOnce();
  // }
  // at this point we have data in pos_world_new_ and pos_image_new_

  fc.run(); // apply formation control

  // ros::spin();

  if (!ros::ok())
    ros::shutdown();

  return EXIT_SUCCESS;
}


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*                                                                  */
/*                        AIrcraft Microphysics                     */
/*                              (AIM)                               */
/*                                                                  */
/* Aerosol Header File                                              */
/*                                                                  */
/* Author               : Thibaud M. Fritz                          */
/* Time                 : 9/28/2018                                 */
/* File                 : Aerosol.hpp                               */
/*                                                                  */
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#ifndef AEROSOL_H_INCLUDED
#define AEROSOL_H_INCLUDED

#include <iostream>
#include <cmath>
#include <vector>
#include <cstring>
#include <boost/math/special_functions/gamma.hpp>
#ifdef OMP
    #include "omp.h"
#endif /* OMP */

/* Include Parameters.hpp for multithreading option */
#include "Core/Parameters.hpp"

#include "Util/ForwardDecl.hpp"
#include "Util/PhysConstant.hpp"
#include "AIM/Coagulation.hpp"
#include "Core/Mesh.hpp"

namespace AIM
{

    class Aerosol;
    class Grid_Aerosol;

}

class AIM::Aerosol
{

    public:

        /* Default constructor */
        Aerosol( );

        /* Constructor */
        Aerosol( Vector_1D bin_Centers, Vector_1D bin_Edges, RealDouble nPart, RealDouble mu, RealDouble sigma, const char* distType = "lognormal", RealDouble alpha_ = -1.0, RealDouble gamma_ = -1.0, RealDouble b_ = 0.0 );

        /* Copy */
        Aerosol( const Aerosol &rhs );

        /* Destructor */
        ~Aerosol();

        /* Operators */
        Aerosol& operator=( const Aerosol &rhs );
        Aerosol operator+=( const Aerosol &rhs );
        Aerosol operator-=( const Aerosol &rhs );
        Aerosol operator+( const Aerosol &rhs ) const;
        Aerosol operator-( const Aerosol &rhs ) const;

        /* Coagulation */
        void Coagulate( const RealDouble dt, const Coagulation &kernel );
        void Coagulate( const RealDouble dt, const Vector_2D &beta, const Vector_3D &f );

        /* Moments */
        RealDouble Moment( UInt n = 0 ) const;
        RealDouble Radius( ) const;
        RealDouble EffRadius( ) const;
        RealDouble StdDev( ) const;

        /* utils */
        void scalePdf( RealDouble factor );
        void updatePdf( Vector_1D pdf_ );

        /* gets */
        Vector_1D getBinCenters() const;
        Vector_1D getBinVCenters() const;
        Vector_1D getBinEdges() const;
        Vector_1D getBinSizes() const;
        UInt getNBin() const;
        RealDouble getNPart() const;
        const char* getType() const;
        RealDouble getAlpha() const;
        Vector_1D getPDF() const;

    protected:

        Vector_1D bin_Centers;
        Vector_1D bin_VCenters;
        Vector_1D bin_Edges;
        Vector_1D bin_Sizes;
        UInt nBin;
        RealDouble nPart;
        const char* type;
        RealDouble mu;
        RealDouble sigma;
        RealDouble alpha;
        Vector_1D pdf;

    private:

};

class AIM::Grid_Aerosol
{

    public:

        /* Default constructor */
        Grid_Aerosol( );

        /* Constructor */
        Grid_Aerosol( UInt Nx_, UInt Ny_, Vector_1D bin_Centers, Vector_1D bin_Edges, RealDouble nPart, RealDouble mu, RealDouble sigma, const char* distType = "lognormal", RealDouble alpha_ = -1.0, RealDouble gamma_ = -1.0, RealDouble b_ = 0.0 );

        /* Copy */
        Grid_Aerosol( const Grid_Aerosol &rhs );

        /* Destructor */
        ~Grid_Aerosol();

        /* Operators */
        Grid_Aerosol& operator=( const Grid_Aerosol &rhs );
        Grid_Aerosol operator+=( const Grid_Aerosol &rhs );
        Grid_Aerosol operator-=( const Grid_Aerosol &rhs );
        Grid_Aerosol operator+( const Grid_Aerosol &rhs ) const;
        Grid_Aerosol operator-( const Grid_Aerosol &rhs ) const;

        /* Coagulation */
        void Coagulate( const RealDouble dt, Coagulation &kernel, const UInt N = 2, const UInt SYM = 0 );

        /* Ice crystal growth */
        void Grow( const RealDouble dt, Vector_2D &H2O, const Vector_2D &T, const Vector_1D &P, const UInt N = 2, const UInt SYM = 0 );

        /* Update bin centers - Used after aerosol transport */
        void UpdateCenters( const Vector_3D &iceV, const Vector_3D &PDF );

        /* Moments */
        Vector_2D Moment( UInt n ) const;
        RealDouble Moment( UInt n, Vector_1D PDF ) const;
        RealDouble Moment( UInt n, UInt iNx, UInt jNy ) const;

        /* Extra utils */
        Vector_3D Number( ) const;
        Vector_2D TotalNumber( ) const;
        RealDouble TotalNumber_sum( const Vector_2D cellAreas ) const;
        Vector_1D Overall_Size_Dist( const Vector_2D cellAreas ) const;
        Vector_3D Volume( ) const;
        Vector_2D TotalVolume( ) const;
        Vector_2D TotalArea( ) const;
        RealDouble TotalIceMass_sum( const Vector_2D cellAreas ) const;
        Vector_2D IWC( ) const;
        Vector_2D Extinction( ) const;
        Vector_1D PDF_Total( const Vector_2D &cellAreas ) const;
        Vector_1D PDF_Total( const Mesh &m ) const;
        Vector_1D xOD( const Vector_1D dx ) const;
        Vector_1D yOD( const Vector_1D dy ) const;
        Vector_2D Radius( ) const;
        RealDouble Radius( UInt iNx, UInt jNy ) const;
        Vector_2D EffRadius( ) const;
        RealDouble EffRadius( UInt iNx, UInt jNy ) const;
        Vector_2D StdDev( ) const;
        RealDouble StdDev( UInt iNx, UInt jNy ) const;

        /* utils */
        void updatePdf( Vector_3D pdf_ );
        Vector_1D Average( const Vector_2D &weights,   \
                           const RealDouble &totWeight ) const;
        void addPDF( const Aerosol &PDF, const Vector_2D &weights,  \
                     const Vector_2D &cellAreas, const RealDouble nCell );
        void addPDF( const Vector_1D &PDF, const Vector_2D &weights, \
                     const Vector_2D &cellAreas, const RealDouble nCell );

        /* gets */
        Vector_1D getBinCenters() const;
        Vector_1D binCenters() const { return bin_Centers; };
        Vector_3D getBinVCenters() const;
        Vector_1D getBinEdges() const;
        Vector_1D binEdges() const { return bin_Edges; };
        Vector_1D getBinSizes() const;
        Vector_1D binSizes() const { return bin_Sizes; };
        UInt getNBin() const;
        Vector_3D getPDF() const;

        Vector_3D pdf;
        Vector_3D bin_VCenters;

    protected:

        unsigned int Nx, Ny;
        Vector_1D bin_Centers;
        Vector_1D bin_Edges;
        Vector_1D bin_VEdges;
        Vector_1D bin_Sizes;
        UInt nBin;
        RealDouble nPart;
        const char* type;
        RealDouble mu;
        RealDouble sigma;
        RealDouble alpha;

    private:

};

#endif /* AEROSOL_H_INCLUDED */


#ifndef TRANSPORTCELLPROPERTY_HPP
#define TRANSPORTCELLPROPERTY_HPP

//general includes
#include <vector>
#include <boost/shared_ptr.hpp>

// chaste includes
#include "Cell.hpp"
#include "AbstractCellProperty.hpp"
#include "StateVariableRegister.hpp"
#include "AbstractTransportReactionSystem.hpp"
#include "AbstractTransportOdeSystem.hpp"
#include "AbstractIvpOdeSolver.hpp"
#include "EulerIvpOdeSolver.hpp"
#include "ChastePoint.hpp"

class TransportCellProperty : public AbstractCellProperty
{
protected:

    // CellPtr to a given cell for access to cell data and for properties that relate ot the transport ode
    CellPtr mThis_cellPtr;

    // register for variables that cross the membrane; either side
    StateVariableRegister* mpBulkStateVariableRegister;
    StateVariableRegister* mpCellStateVariableRegister;

    // transport reaction system for the passage of state variables across the boundary
    AbstractTransportReactionSystem* mpTransportReactionSystem;

    // transport reaction system Ode corresponding to the mpTransportReactionSystem
    AbstractTransportOdeSystem* mpTransportOdeSystem;

    // transport Ode solver
    boost::shared_ptr<AbstractIvpOdeSolver> mpSolver;

    // concentration of state variables at the internal boundary of the cell if this is to remain constant
    bool mIsConstantCellConcentration = false; // for use in averaged source for example?

    std::vector<double> mBulkBoundaryConcentrationVector;

    std::vector<double> mChangeBulkBoundaryConcentrationVector;

    std::vector<double> mCellBoundaryConcentrationVector;

    std::vector<double> mChangeCellBoundaryConcentrationVector;

    bool mIncludeOdeInterpolationOnBoundary=false;

    //15/10/2020
    std::vector<double> mInitBulkBoundaryConcentrationVector;

    unsigned mNumberOCalls_this_reaction_step =0;

public:

    TransportCellProperty();

    virtual ~TransportCellProperty();

    TransportCellProperty(const TransportCellProperty&);

    // virtual methods

    virtual void SetUp(AbstractTransportReactionSystem*, CellPtr);

    virtual void UpdateCellConcentrationVector(std::vector<double>);

    virtual void UpdateBulkConcentrationVector(std::vector<double>);

    virtual void PerformTransportSystem(const std::vector<double>&, const std::vector<double>&, std::vector<double>&, std::vector<double>&);

    virtual void UpdateTransportReactionSystem(AbstractTransportReactionSystem*);

    virtual void UpdateTransportOdeSystem(AbstractTransportOdeSystem*);

    virtual void PreparePostDivisionParent(double);

    virtual void PreparePostDivisionDaughter(const TransportCellProperty&, double);

    // concrete methods
    //double RetrieveBoundarySourceByStateName(std::string,ChastePoint<SPACE_DIM> ); // moved to extended cell property

    void SetUpCellConcentrationVector(unsigned);

    void SetUpBulkConcentrationVector(unsigned);

    void SetUpChangeCellConcentrationVector(unsigned);

    void SetUpChangeBulkConcentrationVector(unsigned);

    double RetrieveBoundarySourceByStateName(std::string); // overload for non-extended cell property case

    double RetrieveChangeBoundarySourceByStateName(std::string); // overload for non-extended cell property case

    void AppendInternalCellBoundaryConcentrations(std::vector<double>&);

    void ReplaceBoundaryStateVariables(std::vector<double>&);

    void ReplaceChangeBoundaryStateVariables(std::vector<double>&);

    void ResetReactionCalls();

    unsigned GetReactionCalls();

    // set methods

    void SetBulkStateVariableRegister(StateVariableRegister*);

    void SetCellStateVariableRegister(StateVariableRegister*);

    void SetConstantCellConcentrationBool(bool);

    void SetCellBoundaryConcentrationVector(std::vector<double>);

    void SetBulkBoundaryConcentrationVector(std::vector<double>);

    void SetInitBulkBoundaryConcentrationVector(std::vector<double>);

    void SetTransportOdeSolver(boost::shared_ptr<AbstractIvpOdeSolver>);

    void SetIncludeOdeInterpolationOnBoundary(bool);

    void SetCellPtr(CellPtr);

    // get methods

    StateVariableRegister* GetBulkStateVariableRegister();

    StateVariableRegister* GetCellStateVariableRegister();

    bool GetConstantCellConcentrationBool();

    std::vector<double> GetInternalCellBoundaryConcentrationVector();

    double GetInternalCellBoundaryConcentrationByIndex(unsigned);

    double GetInternalCellBoundaryConcentrationByName(std::string);

    double GetChangeInternalCellBoundaryConcentrationByName(std::string);

    std::vector<double> GetExternalCellBoundaryConcentrationVector();

    double GetExternalCellBoundaryConcentrationByIndex(unsigned);

    double GetChangeExternalCellBoundaryConcentrationByIndex(unsigned);

    double GetExternalCellBoundaryConcentrationByName(std::string);

    AbstractTransportReactionSystem* GetTransportReactionSystem();

    AbstractTransportOdeSystem* GetTransportOdeSystem();

    boost::shared_ptr<AbstractIvpOdeSolver> GetTransportOdeSolver();

    bool GetIncludeOdeInterpolationOnBoundary();

    CellPtr GetCellPtr();
};

#endif