// Boost.Geometry - gis-projections (based on PROJ4)

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

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

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

// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef BOOST_GEOMETRY_PROJECTIONS_BIPC_HPP
#define BOOST_GEOMETRY_PROJECTIONS_BIPC_HPP

#include <boost/geometry/util/math.hpp>
#include <boost/math/special_functions/hypot.hpp>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>

namespace boost { namespace geometry
{

namespace srs { namespace par4
{
    struct bipc {};

}} //namespace srs::par4

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace bipc
    {

            static const double epsilon = 1e-10;
            static const double epsilon10 = 1e-10;
            static const double one_plus_eps = 1.000000001;
            static const int n_iter = 10;
            static const double lamB = -.34894976726250681539;
            static const double n = .63055844881274687180;
            static const double F = 1.89724742567461030582;
            static const double Azab = .81650043674686363166;
            static const double Azba = 1.82261843856185925133;
            static const double const_T = 1.27246578267089012270;
            static const double rhoc = 1.20709121521568721927;
            static const double cAzc = .69691523038678375519;
            static const double sAzc = .71715351331143607555;
            static const double C45 = .70710678118654752469;
            static const double S45 = .70710678118654752410;
            static const double C20 = .93969262078590838411;
            static const double S20 = -.34202014332566873287;
            static const double R110 = 1.91986217719376253360;
            static const double R104 = 1.81514242207410275904;

            struct par_bipc
            {
                int    noskew;
            };

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_bipc_spheroid
                : public base_t_fi<base_bipc_spheroid<T, Parameters>, T, Parameters>
            {
                par_bipc m_proj_parm;

                inline base_bipc_spheroid(const Parameters& par)
                    : base_t_fi<base_bipc_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T& lp_lon, T& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T half_pi = detail::half_pi<T>();
                    static const T pi = detail::pi<T>();

                    T cphi, sphi, tphi, t, al, Az, z, Av, cdlam, sdlam, r;
                    int tag;

                    cphi = cos(lp_lat);
                    sphi = sin(lp_lat);
                    cdlam = cos(sdlam = lamB - lp_lon);
                    sdlam = sin(sdlam);
                    if (fabs(fabs(lp_lat) - half_pi) < epsilon10) {
                        Az = lp_lat < 0. ? pi : 0.;
                        tphi = HUGE_VAL;
                    } else {
                        tphi = sphi / cphi;
                        Az = atan2(sdlam , C45 * (tphi - cdlam));
                    }
                    if( (tag = (Az > Azba)) ) {
                        cdlam = cos(sdlam = lp_lon + R110);
                        sdlam = sin(sdlam);
                        z = S20 * sphi + C20 * cphi * cdlam;
                        if (fabs(z) > 1.) {
                            if (fabs(z) > one_plus_eps)
                                BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                            else
                                z = z < 0. ? -1. : 1.;
                        } else
                            z = acos(z);
                        if (tphi != HUGE_VAL)
                            Az = atan2(sdlam, (C20 * tphi - S20 * cdlam));
                        Av = Azab;
                        xy_y = rhoc;
                    } else {
                        z = S45 * (sphi + cphi * cdlam);
                        if (fabs(z) > 1.) {
                            if (fabs(z) > one_plus_eps)
                                BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                            else
                                z = z < 0. ? -1. : 1.;
                        } else
                            z = acos(z);
                        Av = Azba;
                        xy_y = -rhoc;
                    }
                    if (z < 0.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    r = F * (t = pow(tan(.5 * z), n));
                    if ((al = .5 * (R104 - z)) < 0.) {
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    }
                    al = (t + pow(al, n)) / const_T;
                    if (fabs(al) > 1.) {
                        if (fabs(al) > one_plus_eps)
                            BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                        else
                            al = al < 0. ? -1. : 1.;
                    } else
                        al = acos(al);
                    if (fabs(t = n * (Av - Az)) < al)
                        r /= cos(al + (tag ? t : -t));
                    xy_x = r * sin(t);
                    xy_y += (tag ? -r : r) * cos(t);
                    if (this->m_proj_parm.noskew) {
                        t = xy_x;
                        xy_x = -xy_x * cAzc - xy_y * sAzc;
                        xy_y = -xy_y * cAzc + t * sAzc;
                    }
                }

                // INVERSE(s_inverse)  spheroid
                // Project coordinates from cartesian (x, y) to geographic (lon, lat)
                inline void inv(T& xy_x, T& xy_y, T& lp_lon, T& lp_lat) const
                {
                    T t, r, rp, rl, al, z, fAz, Az, s, c, Av;
                    int neg, i;

                    if (this->m_proj_parm.noskew) {
                        t = xy_x;
                        xy_x = -xy_x * cAzc + xy_y * sAzc;
                        xy_y = -xy_y * cAzc - t * sAzc;
                    }
                    if( (neg = (xy_x < 0.)) ) {
                        xy_y = rhoc - xy_y;
                        s = S20;
                        c = C20;
                        Av = Azab;
                    } else {
                        xy_y += rhoc;
                        s = S45;
                        c = C45;
                        Av = Azba;
                    }
                    rl = rp = r = boost::math::hypot(xy_x, xy_y);
                    fAz = fabs(Az = atan2(xy_x, xy_y));
                    for (i = n_iter; i ; --i) {
                        z = 2. * atan(pow(r / F,1 / n));
                        al = acos((pow(tan(.5 * z), n) +
                           pow(tan(.5 * (R104 - z)), n)) / const_T);
                        if (fAz < al)
                            r = rp * cos(al + (neg ? Az : -Az));
                        if (fabs(rl - r) < epsilon)
                            break;
                        rl = r;
                    }
                    if (! i)
                        BOOST_THROW_EXCEPTION( projection_exception(error_tolerance_condition) );
                    Az = Av - Az / n;
                    lp_lat = asin(s * cos(z) + c * sin(z) * cos(Az));
                    lp_lon = atan2(sin(Az), c / tan(z) - s * cos(Az));
                    if (neg)
                        lp_lon -= R110;
                    else
                        lp_lon = lamB - lp_lon;
                }

                static inline std::string get_name()
                {
                    return "bipc_spheroid";
                }

            };

            // Bipolar conic of western hemisphere
            template <typename Parameters>
            inline void setup_bipc(Parameters& par, par_bipc& proj_parm)
            {
                proj_parm.noskew = pj_get_param_b(par.params, "ns");
                par.es = 0.;
            }

    }} // namespace detail::bipc
    #endif // doxygen

    /*!
        \brief Bipolar conic of western hemisphere projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Conic
         - Spheroid
        \par Projection parameters
         - ns (boolean)
        \par Example
        \image html ex_bipc.gif
    */
    template <typename T, typename Parameters>
    struct bipc_spheroid : public detail::bipc::base_bipc_spheroid<T, Parameters>
    {
        inline bipc_spheroid(const Parameters& par) : detail::bipc::base_bipc_spheroid<T, Parameters>(par)
        {
            detail::bipc::setup_bipc(this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::par4::bipc, bipc_spheroid, bipc_spheroid)

        // Factory entry(s)
        template <typename T, typename Parameters>
        class bipc_entry : public detail::factory_entry<T, Parameters>
        {
            public :
                virtual base_v<T, Parameters>* create_new(const Parameters& par) const
                {
                    return new base_v_fi<bipc_spheroid<T, Parameters>, T, Parameters>(par);
                }
        };

        template <typename T, typename Parameters>
        inline void bipc_init(detail::base_factory<T, Parameters>& factory)
        {
            factory.add_to_factory("bipc", new bipc_entry<T, Parameters>);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_BIPC_HPP


/**
 * \ file ChamberlinFilter.cpp
 */

#include <ATK/EQ/ChamberlinFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

constexpr gsl::index PROCESSSIZE = 1024*64;

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_frequency_test )
{
  ATK::ChamberlinFilter<double> filter;
  filter.set_cut_frequency(20);
  BOOST_CHECK_EQUAL(filter.get_cut_frequency(), 20);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_attenuation_test )
{
  ATK::ChamberlinFilter<double> filter;
  filter.set_attenuation(10);
  BOOST_CHECK_EQUAL(filter.get_attenuation(), 10);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_selected_test )
{
  ATK::ChamberlinFilter<double> filter;
  filter.select(1);
  BOOST_CHECK_EQUAL(filter.get_selected(), 1);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.set_cut_frequency(1000);
  filter.select(1);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.7070507157689713));
  frequency_checks.push_back(std::make_pair(2000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(1);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.3144771713283422));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_BandPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(1);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.22457718658433723));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_LowPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(0);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.10000664159706373));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_LowPassCoefficients_100_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(100);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(0);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0.7070507157689713));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_LowPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(0);
  filter.set_cut_frequency(100);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.05025431288806103));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_HighPassCoefficients_1k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(1000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(2);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0.7070613824910875));
  frequency_checks.push_back(std::make_pair(10000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_HighPassCoefficients_2k_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(2000);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(2);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(2000, 0.9159798927799332));
  frequency_checks.push_back(std::make_pair(1000, 0));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}

BOOST_AUTO_TEST_CASE( ChamberlinFilter_HighPassCoefficients_500_test )
{
  ATK::SimpleSinusGeneratorFilter<double> generator;
  generator.set_output_sampling_rate(1024*64);
  generator.set_amplitude(1);
  generator.set_frequency(500);

  ATK::ChamberlinFilter<double> filter;
  filter.set_input_sampling_rate(1024*64);
  filter.set_output_sampling_rate(1024*64);
  filter.select(2);
  filter.set_cut_frequency(1000);

  ATK::FFTCheckerFilter<double> checker;
  checker.set_input_sampling_rate(1024*64);
  std::vector<std::pair<int, double> > frequency_checks;
  frequency_checks.push_back(std::make_pair(100, 0));
  frequency_checks.push_back(std::make_pair(1000, 0));
  frequency_checks.push_back(std::make_pair(500, 0.4447622852890557));
  checker.set_checks(frequency_checks);

  checker.set_input_port(0, &filter, 0);
  filter.set_input_port(0, &generator, 0);

  filter.process(1024*64);

  checker.process(PROCESSSIZE);
}


/*
 * Author: Samuel I. Gunadi
 */

#include "mainwindow.h"
#include "content.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <iostream>
#include <string>
#include <stack>

namespace SameGame
{

MainWindow::MainWindow()
{
	/* Configuration */

	try
	{
		// Try to read the configuration file.
		readConfiguration();
	}
	catch (...)
	{
		// If something went wrong while reading configuration file,
		// create or overwrite the file with the default.
		writeDefaultConfiguration();
	}
	// Make sure configuration is valid.
	checkConfiguration();
	// Initialize the random number generator.
	m_uniform_dist = boost::random::uniform_int_distribution<>(0, static_cast<int>(m_block_colors.size() - 1));
	/* Load block image and texture. */
	try
	{
		if (!m_block_image.loadFromMemory(block_image, 52242) || !m_block_texture.loadFromImage(m_block_image))
			throw std::runtime_error("Error occured while loading image.");
		/* Load font. */
		if (!m_font.loadFromMemory(main_font, 58464))
			throw std::runtime_error("Error occured while loading font.");
	}
	catch (...)
	{
		exit(1);
	}
	/* Populate blocks. */
	m_blocks.resize(m_row_size);
	for (uint16_t row = 0; row < m_row_size; row++)
	{
		m_blocks[row].resize(m_column_size);
	}
	/*  Setup blocks. */
	startNewGame();
	m_window = new sf::RenderWindow(
		sf::VideoMode(m_block_width * m_column_size, m_block_height * (m_row_size + 1)),
		"Same Game",
		sf::Style::Titlebar | sf::Style::Close
	);
	m_window->setIcon(BLOCK_IMAGE_WIDTH, BLOCK_IMAGE_HEIGHT, m_block_image.getPixelsPtr());
	m_window->setVerticalSyncEnabled(true);
	sf::Clock clock;
	uint32_t frame_num = 0;
	/* Start Main loop. */
	while (m_window->isOpen())
	{
		if (clock.getElapsedTime().asSeconds() >= 1.f)
		{
			m_fps = frame_num;
			frame_num = 0;
			clock.restart();
			m_window->setTitle("Same Game | " + std::to_string(m_fps) + " FPS");
		}
		/* Handle events. */
		handleEvents();
		/* Clear the window. */
		m_window->clear(sf::Color::Black);
		/* Start drawing. */
		render();
		/* Display the window. */
		m_window->display();
		frame_num++;
	}
}

MainWindow::~MainWindow()
{
	delete m_window;
}

void MainWindow::startNewGame()
{
	for (uint16_t row = 0; row < m_row_size; row++)
	{
		for (uint16_t column = 0; column < m_column_size; column++)
		{
			m_blocks[row][column] = static_cast<uint8_t>(m_uniform_dist(m_rng)) % m_block_colors.size();
		}
	}
	bool found;
	for (;;)
	{
		found = false;
		uint16_t last_same_block_column;
		uint16_t last_same_block_row;
		uint16_t horizontal_count = 0;
		uint16_t vertical_count = 0;
		for (uint16_t row = 0; row < m_row_size; row++)
		{
			last_same_block_column = 0;
			horizontal_count = 0;
			for (uint16_t column = 1; column <= m_column_size; column++)
			{
				if (column < m_column_size && m_blocks[row][column] == m_blocks[row][last_same_block_column])
				{
					horizontal_count++;
				}
				else
				{
					horizontal_count++;
					if (horizontal_count >= m_block_needed)
					{
						for (uint16_t count = 0; count < horizontal_count; count++)
							m_blocks[row][last_same_block_column + count] = static_cast<uint8_t>(m_uniform_dist(m_rng)) % m_block_colors.size();
						found = true;
					}
					last_same_block_column = column;
					horizontal_count = 0;
				}
			}
		}
		for (uint16_t column = 0; column < m_column_size; column++)
		{
			last_same_block_row = 0;
			vertical_count = 0;
			for (uint16_t row = 1; row <= m_row_size; row++)
			{
				if (row < m_row_size && m_blocks[row][column] == m_blocks[last_same_block_row][column])
				{
					vertical_count++;
				}
				else
				{
					vertical_count++;
					if (vertical_count >= m_block_needed)
					{
						for (uint16_t count = 0; count < vertical_count; count++)
							m_blocks[last_same_block_row + count][column] = static_cast<uint8_t>(m_uniform_dist(m_rng)) % m_block_colors.size();
						found = true;
					}
					last_same_block_row = row;
					vertical_count = 0;
				}
			}
		}
		if (!found) break;
	}
}

bool MainWindow::removeBlocks()
{
	uint32_t score;
	bool found;
	bool return_val = false;
	for (;;)
	{
		score = 0;
		found = false;
		uint16_t last_same_block_column;
		uint16_t last_same_block_row;
		uint16_t horizontal_count = 0;
		uint16_t vertical_count = 0;
		for (uint16_t row = 0; row < m_row_size; row++)
		{
			last_same_block_column = 0;
			horizontal_count = 0;
			for (uint16_t column = 1; column <= m_column_size; column++)
			{
				if (column < m_column_size && m_blocks[row][column] == m_blocks[row][last_same_block_column])
				{
					horizontal_count++;
				}
				else
				{
					horizontal_count++;
					if (horizontal_count >= m_block_needed)
					{
						for (uint16_t count = 0; count < horizontal_count; count++)
						{
							m_blocks[row][last_same_block_column + count] = UINT8_MAX;
							score++;
						}
						found = true;
					}
					if (found) break;
					last_same_block_column = column;
					horizontal_count = 0;
				}
			}
			if (found) break;
		}
		for (uint16_t column = 0; column < m_column_size; column++)
		{
			last_same_block_row = 0;
			vertical_count = 0;
			for (uint16_t row = 1; row <= m_row_size; row++)
			{
				if (row < m_row_size && m_blocks[row][column] == m_blocks[last_same_block_row][column])
				{
					vertical_count++;
				}
				else
				{
					vertical_count++;
					if (vertical_count >= m_block_needed)
					{
						for (uint16_t count = 0; count < vertical_count; count++)
						{
							m_blocks[last_same_block_row + count][column] = UINT8_MAX;
							score++;
						}
						found = true;
					}
					if (found) break;
					last_same_block_row = row;
					vertical_count = 0;
				}
			}
			if (found) break;
		}
		if (!found) break;
		m_score += score * (score - 1);
		AnimationState state;
		state.m_duration1 = static_cast<uint32_t>(m_fps * 1.5f); /* 1.5 second */
		state.m_duration2 = m_fps; /* 1 second */
		state.m_score = score * (score - 1);
		state.m_x = -1;
		state.m_blocks_state.first = m_blocks;
		bool blank;
		do
		{
			blank = false;
			for (uint16_t row = 0; row < m_row_size; row++)
			{
				for (uint16_t column = 0; column < m_column_size; column++)
				{
					if (m_blocks[row][column] == UINT8_MAX)
					{
						if (!return_val) return_val = true;
						for (uint16_t shift = row; shift < m_row_size - 1; shift++)
						{
							m_blocks[shift][column] = m_blocks[shift + 1][column];
						}
						m_blocks[m_row_size - 1][column] = static_cast<uint8_t>(m_uniform_dist(m_rng)) % m_block_colors.size();
						blank = true;
						if (state.m_x == -1)
						{
							state.m_x = static_cast<float>(column * m_block_width);
							state.m_y = static_cast<float>(m_block_height * (m_row_size - 1) - row * m_block_height);
						}
					}
				}
			}
		} while (blank);
		state.m_blocks_state.second = m_blocks;
		m_anm_state.push(state);
	}
	while (found);
	return return_val;
}

void MainWindow::render()
{
	for (uint16_t row = 0; row < m_row_size; row++)
	{
		for (uint16_t column = 0; column < m_column_size; column++)
		{
			if (m_anm_state.size() &&
				m_anm_state.front().m_duration1 > 0 &&
				m_anm_state.front().m_blocks_state.first[row][column] == UINT8_MAX)
				continue;
			if (m_selected_column == column && m_selected_row == row) continue;
			sf::Sprite sprite;
			sprite.setTexture(m_block_texture);
			sprite.setScale(
				static_cast<float>(m_block_width) / BLOCK_IMAGE_WIDTH,
				static_cast<float>(m_block_height) / BLOCK_IMAGE_HEIGHT
				);
			if (m_anm_state.empty())
				sprite.setColor(m_block_colors[m_blocks[row][column]]);
			else if (m_anm_state.front().m_duration1 > 0)
				sprite.setColor(m_block_colors[m_anm_state.front().m_blocks_state.first[row][column]]);
			else
				sprite.setColor(m_block_colors[m_anm_state.front().m_blocks_state.second[row][column]]);
			sprite.setPosition(
				static_cast<float>(column * m_block_width),
				static_cast<float>(m_block_height * (m_row_size - 1) - m_block_height * row)
				);
			m_window->draw(sprite);
		}
	}
	if (m_anm_state.empty())
		m_anim_playing = false;
	else
	{
		m_anim_playing = true;
		if (m_anm_state.front().m_duration1 > 0)
		{
			m_anm_state.front().m_duration1--;
			sf::Text score_gained_text;
			score_gained_text.setFont(m_font);
			score_gained_text.setColor(sf::Color::White);
			score_gained_text.setCharacterSize(m_block_height);
			score_gained_text.setString("+" + std::to_string(m_anm_state.front().m_score));
			score_gained_text.setPosition(m_anm_state.front().m_x, m_anm_state.front().m_y - m_block_height / 10);
			m_window->draw(score_gained_text);
		}
		else
		{
			if (m_anm_state.front().m_duration2 > 0)
				m_anm_state.front().m_duration2--;
			else m_anm_state.pop();
		}
	}
	if (m_selected_column < m_column_size && m_selected_row < m_row_size)
	{
		sf::Sprite sprite;
		sprite.setTexture(m_block_texture);
		sprite.setColor(
			sf::Color(
				m_block_colors[m_blocks[m_selected_row][m_selected_column]].r,
				m_block_colors[m_blocks[m_selected_row][m_selected_column]].g,
				m_block_colors[m_blocks[m_selected_row][m_selected_column]].b,
				191
				)
			);
		sprite.setScale(
			static_cast<float>(m_block_width) / BLOCK_IMAGE_WIDTH,
			static_cast<float>(m_block_height) / BLOCK_IMAGE_HEIGHT
			);
		sprite.setPosition(
			static_cast<float>(sf::Mouse::getPosition(*m_window).x - m_block_width / 2),
			static_cast<float>(sf::Mouse::getPosition(*m_window).y - m_block_height / 2)
			);
		m_window->draw(sprite);
	}
	sf::Text score_text;
	score_text.setFont(m_font);
	score_text.setColor(sf::Color::White);
	score_text.setCharacterSize(m_block_height);
	score_text.setString("Score: " + std::to_string(m_score));
	score_text.setPosition(0, static_cast<float>(m_block_height * m_row_size - m_block_height / 10));
	m_window->draw(score_text);
}

void MainWindow::handleEvents()
{
	sf::Event event;
	while (m_window->pollEvent(event))
	{
		if (event.type == sf::Event::MouseButtonPressed)
		{
			if (!m_anim_playing)
			{
				if (event.mouseButton.button == sf::Mouse::Left)
				{
					m_press_x = event.mouseButton.x;
					m_press_y = event.mouseButton.y;
					if (m_press_x <= m_block_width * m_column_size && m_press_y <= m_block_height * m_row_size)
					{
						m_selected_column = static_cast<uint16_t>(m_press_x / m_block_width);
						m_selected_row = static_cast<uint16_t>((m_block_height * m_row_size - m_press_y) / m_block_height);
					}
				}
			}
		}
		else if (event.type == sf::Event::MouseButtonReleased)
		{
			m_selected_column = UINT16_MAX;
			m_selected_row = UINT16_MAX;
		}
		else if (event.type == sf::Event::MouseMoved)
		{
			if (!m_anim_playing)
			{
				if (m_selected_column != UINT16_MAX && m_selected_row != UINT16_MAX)
				{
					m_release_x = event.mouseMove.x;
					m_release_y = event.mouseMove.y;
					int32_t delta_x = m_press_x - m_release_x;
					int32_t delta_y = m_press_y - m_release_y;
					if (abs(delta_x) > m_block_height || abs(delta_y) > m_block_width)
					{
						Direction direction = Direction::None;
						/* Right */
						if (abs(delta_x) > abs(delta_y) && delta_x < 0)
						{
							direction = Direction::Right;
						}
						/* Up */
						else if (abs(delta_y) > abs(delta_x) && delta_y > 0)
						{
							direction = Direction::Up;
						}
						/* Left */
						else if (abs(delta_x) > abs(delta_y) && delta_x > 0)
						{
							direction = Direction::Left;
						}
						/* Down */
						else if (abs(delta_y) > abs(delta_x) && delta_y < 0)
						{
							direction = Direction::Down;
						}
						if (direction != Direction::None &&
							!(m_selected_row <= 0 && direction == Direction::Down) &&
							!(m_selected_column <= 0 && direction == Direction::Left) &&
							!(m_selected_row >= m_row_size && direction == Direction::Up) &&
							!(m_selected_column >= m_column_size && direction == Direction::Right))
						{
							uint16_t switched_column = m_selected_column;
							uint16_t switched_row = m_selected_row;
							switch (direction)
							{
							case Direction::Right:
								switched_column++;
								break;
							case Direction::Up:
								switched_row++;
								break;
							case Direction::Left:
								switched_column--;
								break;
							case Direction::Down:
								switched_row--;
							}
							if (switched_column < m_column_size && switched_row < m_row_size)
							{
								std::vector<std::vector<uint8_t>> temp_blocks(m_blocks);
								m_blocks[switched_row][switched_column] = temp_blocks[m_selected_row][m_selected_column];
								m_blocks[m_selected_row][m_selected_column] = temp_blocks[switched_row][switched_column];
								if (!removeBlocks()) m_blocks = temp_blocks;
							}
						}
						m_selected_column = UINT16_MAX;
						m_selected_row = UINT16_MAX;
					}
				}
			}
		}
		else if (event.type == sf::Event::KeyPressed)
		{
			if (event.key.code == sf::Keyboard::Escape)
			{
				m_window->close();
			}
		}
		else if (event.type == sf::Event::Closed)
		{
			m_window->close();
		}
	}
}

void MainWindow::readConfiguration()
{
	boost::property_tree::ptree pt;
	read_xml("config.xml", pt, boost::property_tree::xml_parser::no_comments);
	m_column_size = pt.get<uint16_t>("samegame.column_size");
	m_row_size = pt.get<uint16_t>("samegame.row_size");
	m_block_width = pt.get<uint16_t>("samegame.block_width");
	m_block_height = pt.get<uint16_t>("samegame.block_height");
	m_block_needed = pt.get<uint8_t>("samegame.block_needed");
	for (boost::property_tree::ptree::value_type &value : pt.get_child("samegame.block_colors"))
	{
		m_block_colors.push_back(
			sf::Color(
			static_cast<uint8_t>(std::strtoul(value.second.data().substr(1, 2).c_str(), nullptr, 16)),
			static_cast<uint8_t>(std::strtoul(value.second.data().substr(3, 2).c_str(), nullptr, 16)),
			static_cast<uint8_t>(std::strtoul(value.second.data().substr(5, 2).c_str(), nullptr, 16))
			)
			);
	}
}

void MainWindow::writeDefaultConfiguration()
{
	boost::property_tree::ptree pt;
	pt.clear();
	m_block_colors.clear();
	pt.put("samegame.column_size", 10);
	pt.put("samegame.row_size", 10);
	pt.put("samegame.block_width", 32);
	pt.put("samegame.block_height", 32);
	pt.put("samegame.block_needed", 3);
	pt.add("samegame.block_colors.color", "#FF0000");
	pt.add("samegame.block_colors.color", "#FFFF00");
	pt.add("samegame.block_colors.color", "#00FF00");
	pt.add("samegame.block_colors.color", "#00FFFF");
	pt.add("samegame.block_colors.color", "#0000FF");
	try
	{
		boost::property_tree::xml_writer_settings<char> settings('\t', 1);
		write_xml("config.xml", pt, std::locale(), settings);
	}
	catch (...)
	{
	}
}

void MainWindow::checkConfiguration()
{
	if (m_column_size < 2 || m_column_size > 128) m_column_size = 10;
	if (m_row_size < 2 || m_row_size > 70) m_row_size = 10;
	if (m_block_width < 2 || m_block_width > 640) m_block_width = 32;
	if (m_block_height < 2 || m_block_height > 640) m_block_height = 32;
	if (m_block_colors.size() < 2)
	{
		m_block_colors.push_back(sf::Color(255, 0, 0));
		m_block_colors.push_back(sf::Color(255, 255, 0));
		m_block_colors.push_back(sf::Color(0, 255, 0));
		m_block_colors.push_back(sf::Color(0, 255, 255));
		m_block_colors.push_back(sf::Color(0, 0, 255));
	}
	if (m_block_needed < 2 || m_block_needed > 128) m_block_needed = 3;
}
} // SameGame


#include <b0/utils/time_sync.h>

#include <boost/thread.hpp>
#include <boost/lexical_cast.hpp>

#include <iostream>
#include <string>

// unit-test for the time sync algorithm
// (does not cover time synchronization between nodes)

namespace b0
{

// adds an offset to hardware clock

class TimeSync_TEST : public TimeSync
{
public:
    TimeSync_TEST(int64_t offset, double speed) : t0_(TimeSync::hardwareTimeUSec()), offset_(offset), speed_(speed) {}
    int64_t hardwareTimeUSec() const {return offset_ + speed_ * (TimeSync::hardwareTimeUSec() - t0_) + t0_;}
protected:
    int64_t t0_;
    int64_t offset_;
    double speed_;
};

}

// exit codes
#define E_SUCCESS 0
#define E_INVALID_ARGS 1
#define E_INVALID_TEST 2
#define E_TEST_FAILED 3

inline int64_t sec(double s) { return s * 1000 * 1000; }

int run_test(const char *name, bool (*f)(int, char**), int argc, char **argv)
{
    std::cout << "running test '" << name << "'..." << std::endl;
    bool result = false;
    try
    {
        result = f(argc, argv);
        std::cout << "test '" << name << "' " << (result ? "PASSED" : "FAILED") << std::endl;
    }
    catch(std::exception &ex)
    {
        std::cout << "test '" << name << "' EXCEPTION: " << ex.what() << std::endl;
    }
    return result ? E_SUCCESS : E_TEST_FAILED;
}

bool test_offset_recover(int argc, char **argv)
{
    double max_slope = 0.5;
    int64_t offset = sec(5);
    double speed = 1.0;
    b0::TimeSync c;
    c.setMaxSlope(max_slope);
    b0::TimeSync_TEST s(offset, speed);
    s.setMaxSlope(max_slope);

    int64_t test_end_time = offset * 1.2 / max_slope + c.hardwareTimeUSec(); // run for the required time to adjust clock + 20%
    int64_t error = 0;

    while(c.hardwareTimeUSec() < test_end_time)
    {
        int64_t server_time = s.hardwareTimeUSec();
        c.updateTime(server_time);
        error = c.timeUSec() - server_time;
        std::cout << "server_time=" << server_time << ", error=" << error << std::endl;
        boost::this_thread::sleep_for(boost::chrono::seconds{1});
    }

    return std::abs(error) < sec(0.05);
}

bool test_clock_tracking(int argc, char **argv)
{
    if(argc != 4)
    {
        std::cerr << "usage: " << argv[0] << " <max-slope> <test-clock speed> <expected bool result>" << std::endl;
        return false;
    }

    double max_slope = boost::lexical_cast<double>(argv[1]);
    int64_t offset = sec(0);
    double speed = boost::lexical_cast<double>(argv[2]);
    bool expected = boost::lexical_cast<bool>(argv[3]);
    b0::TimeSync c;
    c.setMaxSlope(max_slope);
    b0::TimeSync_TEST s(offset, speed);
    s.setMaxSlope(max_slope);

    int64_t test_end_time = c.hardwareTimeUSec() + sec(5);
    int64_t error = 0;

    while(c.hardwareTimeUSec() < test_end_time)
    {
        int64_t server_time = s.hardwareTimeUSec();
        c.updateTime(server_time);
        error = c.timeUSec() - server_time;
        std::cout << "server_time=" << server_time << ", error=" << error << std::endl;
        boost::this_thread::sleep_for(boost::chrono::seconds{1});
    }

    // if it will be able to track the clock, the final offset will be constant at approximately (1 - speed) seconds
    int64_t error_bound = sec((1 - speed) * 1.05 /* 5% tolerance */);
    return !(expected ^ (std::abs(error) < std::abs(error_bound)));
}

#define BEGIN_TEST_LIST if(0) {}
#define ADD_TEST(n) else if(test_name_arg == #n) {return run_test(#n, &test_##n, argc-1, argv+1);}
#define END_TEST_LIST else {std::cerr << "invalid test name: '" << test_name_arg << std::endl; return E_INVALID_TEST;}

int main(int argc, char **argv)
{
    if(argc < 2)
    {
        std::cerr << "usage: " << argv[0] << " <test_name>" << std::endl;
        return E_INVALID_ARGS;
    }

    std::string test_name_arg(argc > 1 ? argv[1] : "");

    BEGIN_TEST_LIST
    ADD_TEST(offset_recover)
    ADD_TEST(clock_tracking)
    END_TEST_LIST
}


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

// Copyright (c) 2007-2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2014 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2014 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014-2021.
// Modifications copyright (c) 2014-2021, Oracle and/or its affiliates.

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

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_LENGTH_HPP
#define BOOST_GEOMETRY_ALGORITHMS_LENGTH_HPP

#include <iterator>

#include <boost/core/ignore_unused.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/iterator.hpp>
#include <boost/range/value_type.hpp>

#include <boost/geometry/algorithms/assign.hpp>
#include <boost/geometry/algorithms/detail/calculate_null.hpp>
#include <boost/geometry/algorithms/detail/dummy_geometries.hpp>
#include <boost/geometry/algorithms/detail/multi_sum.hpp>
// #include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>
#include <boost/geometry/algorithms/detail/visit.hpp>

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/closure.hpp>
#include <boost/geometry/core/tag.hpp>
#include <boost/geometry/core/tags.hpp>
#include <boost/geometry/core/visit.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp> // For backward compatibility
#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/default_strategy.hpp>
#include <boost/geometry/strategies/default_length_result.hpp> // TODO: Move to algorithms
#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/length/cartesian.hpp>
#include <boost/geometry/strategies/length/geographic.hpp>
#include <boost/geometry/strategies/length/spherical.hpp>

#include <boost/geometry/views/closeable_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace length
{

template<typename Segment>
struct segment_length
{
    template <typename Strategies>
    static inline typename default_length_result<Segment>::type
    apply(Segment const& segment, Strategies const& strategies)
    {
        typedef typename point_type<Segment>::type point_type;
        point_type p1, p2;
        geometry::detail::assign_point_from_index<0>(segment, p1);
        geometry::detail::assign_point_from_index<1>(segment, p2);
        return strategies.distance(p1, p2).apply(p1, p2);
    }
};

/*!
\brief Internal, calculates length of a linestring using iterator pairs and
    specified strategy
\note for_each could be used here, now that point_type is changed by boost
    range iterator
*/
template<typename Range, closure_selector Closure>
struct range_length
{
    typedef typename default_length_result<Range>::type return_type;

    template <typename Strategies>
    static inline return_type
    apply(Range const& range, Strategies const& strategies)
    {
        typedef typename closeable_view<Range const, Closure>::type view_type;
        typedef typename boost::range_iterator
            <
                view_type const
            >::type iterator_type;

        return_type sum = return_type();
        view_type view(range);
        iterator_type it = boost::begin(view), end = boost::end(view);
        if (it != end)
        {
            auto const strategy = strategies.distance(dummy_point(), dummy_point());

            for(iterator_type previous = it++;
                    it != end;
                    ++previous, ++it)
            {
                // Add point-point distance using the return type belonging
                // to strategy
                sum += strategy.apply(*previous, *it);
            }
        }

        return sum;
    }
};

}} // namespace detail::length
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Geometry, typename Tag = typename tag<Geometry>::type>
struct length : detail::calculate_null
{
    typedef typename default_length_result<Geometry>::type return_type;

    template <typename Strategy>
    static inline return_type apply(Geometry const& geometry, Strategy const& strategy)
    {
        return calculate_null::apply<return_type>(geometry, strategy);
    }
};

template <typename Geometry>
struct length<Geometry, linestring_tag>
    : detail::length::range_length<Geometry, closed>
{};

// RING: length is currently 0; it might be argued that it is the "perimeter"

template <typename Geometry>
struct length<Geometry, segment_tag>
    : detail::length::segment_length<Geometry>
{};

template <typename MultiLinestring>
struct length<MultiLinestring, multi_linestring_tag> : detail::multi_sum
{
    template <typename Strategy>
    static inline typename default_length_result<MultiLinestring>::type
    apply(MultiLinestring const& multi, Strategy const& strategy)
    {
        return multi_sum::apply
               <
                   typename default_length_result<MultiLinestring>::type,
                   detail::length::range_length
                   <
                       typename boost::range_value<MultiLinestring>::type,
                       closed // no need to close it explicitly
                   >
               >(multi, strategy);

    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

namespace resolve_strategy {

template
<
    typename Strategies,
    bool IsUmbrella = strategies::detail::is_umbrella_strategy<Strategies>::value
>
struct length
{
    template <typename Geometry>
    static inline typename default_length_result<Geometry>::type
    apply(Geometry const& geometry, Strategies const& strategies)
    {
        return dispatch::length<Geometry>::apply(geometry, strategies);
    }
};

template <typename Strategy>
struct length<Strategy, false>
{
    template <typename Geometry>
    static inline typename default_length_result<Geometry>::type
    apply(Geometry const& geometry, Strategy const& strategy)
    {
        using strategies::length::services::strategy_converter;
        return dispatch::length<Geometry>::apply(
                geometry, strategy_converter<Strategy>::get(strategy));
    }
};

template <>
struct length<default_strategy, false>
{
    template <typename Geometry>
    static inline typename default_length_result<Geometry>::type
    apply(Geometry const& geometry, default_strategy const&)
    {
        typedef typename strategies::length::services::default_strategy
            <
                Geometry
            >::type strategies_type;

        return dispatch::length<Geometry>::apply(geometry, strategies_type());
    }
};

} // namespace resolve_strategy

namespace resolve_dynamic {

template <typename Geometry, typename Tag = typename geometry::tag<Geometry>::type>
struct length
{
    template <typename Strategy>
    static inline typename default_length_result<Geometry>::type
    apply(Geometry const& geometry, Strategy const& strategy)
    {
        return resolve_strategy::length<Strategy>::apply(geometry, strategy);
    }
};

template <typename Geometry>
struct length<Geometry, dynamic_geometry_tag>
{
    template <typename Strategy>
    static inline typename default_length_result<Geometry>::type
        apply(Geometry const& geometry, Strategy const& strategy)
    {
        typename default_length_result<Geometry>::type result = 0;
        traits::visit<Geometry>::apply([&](auto const& g)
        {
            result = length<util::remove_cref_t<decltype(g)>>::apply(g, strategy);
        }, geometry);
        return result;
    }
};

template <typename Geometry>
struct length<Geometry, geometry_collection_tag>
{
    template <typename Strategy>
    static inline typename default_length_result<Geometry>::type
        apply(Geometry const& geometry, Strategy const& strategy)
    {
        typename default_length_result<Geometry>::type result = 0;
        detail::visit_breadth_first([&](auto const& g)
        {
            result += length<util::remove_cref_t<decltype(g)>>::apply(g, strategy);
            return true;
        }, geometry);
        return result;
    }
};

} // namespace resolve_dynamic

/*!
\brief \brief_calc{length}
\ingroup length
\details \details_calc{length, length (the sum of distances between consecutive points)}. \details_default_strategy
\tparam Geometry \tparam_geometry
\param geometry \param_geometry
\return \return_calc{length}

\qbk{[include reference/algorithms/length.qbk]}
\qbk{[length] [length_output]}
 */
template<typename Geometry>
inline typename default_length_result<Geometry>::type
length(Geometry const& geometry)
{
    concepts::check<Geometry const>();

    // detail::throw_on_empty_input(geometry);

    return resolve_dynamic::length<Geometry>::apply(geometry, default_strategy());
}

/*!
\brief \brief_calc{length} \brief_strategy
\ingroup length
\details \details_calc{length, length (the sum of distances between consecutive points)} \brief_strategy. \details_strategy_reasons
\tparam Geometry \tparam_geometry
\tparam Strategy \tparam_strategy{distance}
\param geometry \param_geometry
\param strategy \param_strategy{distance}
\return \return_calc{length}

\qbk{distinguish,with strategy}
\qbk{[include reference/algorithms/length.qbk]}
\qbk{[length_with_strategy] [length_with_strategy_output]}
 */
template<typename Geometry, typename Strategy>
inline typename default_length_result<Geometry>::type
length(Geometry const& geometry, Strategy const& strategy)
{
    concepts::check<Geometry const>();

    // detail::throw_on_empty_input(geometry);

    return resolve_dynamic::length<Geometry>::apply(geometry, strategy);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_LENGTH_HPP


#include "aikido/io/trajectory.hpp"

#include <algorithm>
#include <fstream>
#include <iostream>

#include <boost/program_options.hpp>

#include "aikido/common/Spline.hpp"
#include "aikido/io/detail/yaml_extension.hpp"
#include "aikido/io/yaml.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
#include "aikido/trajectory/Interpolated.hpp"

using aikido::statespace::ConstStateSpacePtr;
using aikido::statespace::StateSpacePtr;
using aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr;
using aikido::statespace::dart::MetaSkeletonStateSpace;
using aikido::trajectory::Spline;
using aikido::trajectory::UniqueSplinePtr;

namespace aikido {
namespace io {

void saveTrajectory(
    const aikido::trajectory::Spline& trajectory, const std::string& savePath)
{
  std::ofstream file(savePath);
  YAML::Emitter emitter;

  auto skelSpace = std::dynamic_pointer_cast<const MetaSkeletonStateSpace>(
      trajectory.getStateSpace());
  if (!skelSpace)
    throw std::runtime_error(
        "Trajectory state space is not MetaSkeletonStateSpace");

  emitter << YAML::BeginMap;
  emitter << YAML::Key << "configuration";
  emitter << YAML::BeginMap;
  emitter << YAML::Key << "start_time" << YAML::Value
          << trajectory.getStartTime();
  emitter << YAML::Key << "dofs" << YAML::Flow
          << skelSpace->getProperties().getDofNames();
  emitter << YAML::Key << "type" << YAML::Value << "spline";
  emitter << YAML::Key << "spline";
  emitter << YAML::BeginMap;
  emitter << YAML::Key << "order" << YAML::Value
          << trajectory.getNumDerivatives();
  emitter << YAML::EndMap;
  emitter << YAML::EndMap;

  Eigen::VectorXd position(skelSpace->getDimension());
  aikido::io::detail::encode_impl<Eigen::MatrixXd, false> convertMatrix;
  aikido::io::detail::encode_impl<Eigen::VectorXd, false> convertVector;

  emitter << YAML::Key << "data";
  emitter << YAML::BeginSeq;
  for (std::size_t i = 0; i < trajectory.getNumSegments(); ++i)
  {
    auto startState = trajectory.getSegmentStartState(i);
    double duration = trajectory.getSegmentDuration(i);
    auto segmentCoeff = trajectory.getSegmentCoefficients(i);

    // Convert start state to Eigen vector
    skelSpace->logMap(startState, position);

    emitter << YAML::BeginMap;
    emitter << YAML::Key << "coefficients" << YAML::Flow
            << convertMatrix.encode(segmentCoeff);
    emitter << YAML::Key << "duration" << YAML::Value << duration;
    emitter << YAML::Key << "start_state" << YAML::Flow
            << convertVector.encode(position);
    emitter << YAML::EndMap;
  }

  emitter << YAML::EndSeq << YAML::EndMap;
  file << emitter.c_str();
}

//==============================================================================
UniqueSplinePtr loadSplineTrajectory(
    const std::string& trajPath,
    const ConstMetaSkeletonStateSpacePtr& metaSkeletonStateSpace)
{
  YAML::Node trajFile = YAML::LoadFile(trajPath);
  const YAML::Node& config = trajFile["configuration"];

  double startTime = config["start_time"].as<double>();
  std::vector<std::string> dofs = config["dofs"].as<std::vector<std::string>>();
  auto paramDofs = metaSkeletonStateSpace->getProperties().getDofNames();
  if (!std::equal(dofs.begin(), dofs.end(), paramDofs.begin()))
  {
    throw std::runtime_error("Dof names should be same");
  }

  std::string trajType = config["type"].as<std::string>();
  if (trajType.compare("spline"))
  {
    throw std::runtime_error("Trajectory type should be spline");
  }

  auto trajectory = ::aikido::common::make_unique<Spline>(
      metaSkeletonStateSpace, startTime);
  aikido::statespace::StateSpace::State* startState
      = metaSkeletonStateSpace->allocateState();

  const YAML::Node& segments = trajFile["data"];
  for (YAML::const_iterator it = segments.begin(); it != segments.end(); ++it)
  {
    const YAML::Node& segment = *it;
    Eigen::MatrixXd coefficients
        = segment["coefficients"].as<Eigen::MatrixXd>();
    double duration = segment["duration"].as<double>();
    Eigen::VectorXd position = segment["start_state"].as<Eigen::VectorXd>();

    // Convert position Eigen vector to StateSpace::State*
    metaSkeletonStateSpace->expMap(position, startState);

    trajectory->addSegment(coefficients, duration, startState);
  }

  metaSkeletonStateSpace->freeState(startState);
  return trajectory;
}

} // namespace io
} // namespace aikido


/**
 * @file budgeted_maximum_coverage_example.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-04-08
 */

//! [Budgeted Maximum Coverage Example]
#include <iostream>
#include <vector>
#include <iterator>

#include <boost/range/irange.hpp>
#include "paal/greedy/set_cover/budgeted_maximum_coverage.hpp"

int main() {
    std::vector<std::vector<int>> set_to_elements = {
        { 1 }, { 2 }, { 0, 1 }, { 2, 3, 4 }
    };
    std::vector<int> set_to_cost = { 1, 1, 5, 5 };
    std::vector<int> element_to_weight = { 3, 5, 1, 1, 1 };
    const int BUDGET = 10;
    auto sets = boost::irange(0, 4);
    std::vector<int> result;
    auto element_index = [](int el){return el;};
    auto covered = paal::greedy::budgeted_maximum_coverage(
        sets,
        [&](int set){return set_to_cost[set];},
        [&](int set){return set_to_elements[set];},
        back_inserter(result),
        element_index,
        BUDGET,
        [&](int el){return element_to_weight[el];});
    std::cout << "Covered: " << covered << std::endl;
}
//! [Budgeted Maximum Coverage Example]


#include <boost/accumulators/statistics/rolling_moment.hpp>


// 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_TRAITS_GRADIENT_INCLUDE
#define MTL_TRAITS_GRADIENT_INCLUDE

#include <boost/mpl/if.hpp>
#include <boost/numeric/mtl/mtl_fwd.hpp>
#include <boost/numeric/mtl/utility/category.hpp>
#include <boost/numeric/mtl/utility/linear_operator.hpp>

namespace mtl { namespace traits {

/// Type trait returning type for gradient of function with T as argument
template <typename T>
struct gradient
  : boost::mpl::if_<is_scalar<T>, mtl::dense_vector<T>, tag::unknown>
{};

template <typename Value, typename Para>
struct gradient<mtl::dense_vector<Value, Para> >
  : linear_operator<mtl::dense_vector<Value, Para>, mtl::dense_vector<Value, Para> >
{};

}} // namespace mtl::traits

#endif // MTL_TRAITS_GRADIENT_INCLUDE


#ifndef EXTRACTION_HELPER_FUNCTIONS_HPP
#define EXTRACTION_HELPER_FUNCTIONS_HPP

#include <boost/spirit/include/phoenix.hpp>
#include <boost/spirit/include/qi.hpp>

#include <boost/algorithm/string/replace.hpp>

#include <algorithm>
#include <cctype>
#include <iterator>
#include <limits>
#include <string>

#include "guidance/parsing_toolkit.hpp"

namespace osrm
{
namespace extractor
{

namespace detail
{

namespace qi = boost::spirit::qi;

template <typename Iterator> struct iso_8601_grammar : qi::grammar<Iterator, unsigned()>
{
    iso_8601_grammar() : iso_8601_grammar::base_type(root)

    {
        using qi::_1;
        using qi::_a;
        using qi::_b;
        using qi::_c;
        using qi::_pass;
        using qi::_val;
        using qi::char_;
        using qi::eoi;
        using qi::eps;
        using qi::uint_;

        hh = uint2_p[_pass = bind([](unsigned x) { return x < 24; }, _1), _val = _1];
        mm = uint2_p[_pass = bind([](unsigned x) { return x < 60; }, _1), _val = _1];
        ss = uint2_p[_pass = bind([](unsigned x) { return x < 60; }, _1), _val = _1];

        osm_time = (uint_p[_a = _1] >> eoi)[_val = _a * 60] |
                   (uint_p[_a = _1] >> ':' >> uint_p[_b = _1] >> eoi)[_val = _a * 3600 + _b * 60] |
                   (uint_p[_a = _1] >> ':' >> uint_p[_b = _1] >> ':' >> uint_p[_c = _1] >>
                    eoi)[_val = _a * 3600 + _b * 60 + _c];

        alternative_time =
            ('T' >> hh[_a = _1] >> mm[_b = _1] >> ss[_c = _1])[_val = _a * 3600 + _b * 60 + _c];

        extended_time = ('T' >> hh[_a = _1] >> ':' >> mm[_b = _1] >> ':' >>
                         ss[_c = _1])[_val = _a * 3600 + _b * 60 + _c];

        standard_time =
            ('T' >> -(uint_ >> char_("Hh"))[_a = _1] >> -(uint_ >> char_("Mm"))[_b = _1] >>
             -(uint_ >> char_("Ss"))[_c = _1])[_val = _a * 3600 + _b * 60 + _c];

        standard_date = (uint_ >> char_("Dd"))[_val = _1 * 86400];

        standard_week = (uint_ >> char_("Ww"))[_val = _1 * 604800];

        iso_period =
            osm_time[_val = _1] | ('P' >> standard_week >> eoi)[_val = _1] |
            ('P' >> (alternative_time[_a = 0, _b = _1] | extended_time[_a = 0, _b = _1] |
                     (eps[_a = 0, _b = 0] >> -standard_date[_a = _1] >> -standard_time[_b = _1])) >>
             eoi)[_val = _a + _b];

        root = iso_period;
    }

    qi::rule<Iterator, unsigned()> root;
    qi::rule<Iterator, unsigned(), qi::locals<unsigned, unsigned>> iso_period;
    qi::rule<Iterator, unsigned(), qi::locals<unsigned, unsigned, unsigned>> osm_time,
        standard_time, alternative_time, extended_time;
    qi::rule<Iterator, unsigned()> standard_date, standard_week;
    qi::rule<Iterator, unsigned()> hh, mm, ss;

    qi::uint_parser<unsigned, 10, 1, 2> uint_p;
    qi::uint_parser<unsigned, 10, 2, 2> uint2_p;
};
} // namespace detail

inline bool durationIsValid(const std::string &s)
{
    static detail::iso_8601_grammar<std::string::const_iterator> const iso_8601_grammar;

    std::string::const_iterator iter = s.begin();
    unsigned duration = 0;
    boost::spirit::qi::parse(iter, s.end(), iso_8601_grammar, duration);

    return !s.empty() && iter == s.end();
}

inline unsigned parseDuration(const std::string &s)
{
    static detail::iso_8601_grammar<std::string::const_iterator> const iso_8601_grammar;

    std::string::const_iterator iter = s.begin();
    unsigned duration = 0;
    boost::spirit::qi::parse(iter, s.end(), iso_8601_grammar, duration);

    return !s.empty() && iter == s.end() ? duration : std::numeric_limits<unsigned>::max();
}

inline std::string
trimLaneString(std::string lane_string, std::int32_t count_left, std::int32_t count_right)
{
    return guidance::trimLaneString(std::move(lane_string), count_left, count_right);
}

inline std::string applyAccessTokens(const std::string &lane_string,
                                     const std::string &access_tokens)
{
    return guidance::applyAccessTokens(lane_string, access_tokens);
}

// Takes a string representing a list separated by delim and canonicalizes containing spaces.
// Example: "aaa;bbb; ccc;  d;dd" => "aaa; bbb; ccc; d; dd"
inline std::string canonicalizeStringList(std::string strlist, const std::string &delim)
{
    // expand space after delimiter: ";X" => "; X"
    boost::replace_all(strlist, delim, delim + " ");

    // collapse spaces; this is needed in case we expand "; X" => ";  X" above
    // but also makes sense to do irregardless of the fact - canonicalizing strings.
    const auto spaces = [](unsigned char lhs, unsigned char rhs) {
        return ::isspace(lhs) && ::isspace(rhs);
    };
    auto it = std::unique(begin(strlist), end(strlist), spaces);
    strlist.erase(it, end(strlist));

    return strlist;
}

} // namespace extractor
} // namespace osrm

#endif // EXTRACTION_HELPER_FUNCTIONS_HPP


#include <stan/math/torsten/test/unit/test_fixture_twocpt.hpp>
#include <stan/math/torsten/test/unit/test_functors.hpp>
#include <stan/math/torsten/pmx_twocpt_model.hpp>
#include <stan/math/torsten/pmx_ode_model.hpp>
#include <boost/mp11.hpp>

TYPED_TEST_SUITE_P(test_twocpt);

TYPED_TEST_P(test_twocpt, multiple_bolus) {
  this -> compare_overload(1.e-10);
}

TYPED_TEST_P(test_twocpt, multiple_bolus_with_tlag) {
  this -> tlag[0][0] = 1.7;  // FIXME: tlag-generated event coincidents another event the test would fail
  this -> time[0] = 0;
  for(int i = 1; i < this -> nt; ++i) this -> time[i] = this -> time[i - 1] + 1;
  this -> amt[0] = 1200;

  this -> compare_param_overload(1.e-10);
}

TYPED_TEST_P(test_twocpt, multiple_bolus_ss) {
  this -> reset_events(3);
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < this -> nt; i++) this -> time[i] = this -> time[i - 1] + 5;

  this -> amt[0] = 1200;
  this -> addl[0] = 10;
  this -> ss[0] = 1;

  this -> compare_overload(1.e-10);
}

TYPED_TEST_P(test_twocpt, multiple_infusion) {
  this -> reset_events(3);
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < this -> nt; i++) this -> time[i] = this -> time[i - 1] + 5;
  this -> addl[0] = 1;
  this -> amt[0] = 1200;
  this -> rate[0] = 150;
  this -> addl[0] = 10;

  this -> compare_overload(1.e-10);
}

TYPED_TEST_P(test_twocpt, multiple_infusion_ss) {
  this -> reset_events(3);
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < this -> nt; i++) this -> time[i] = this -> time[i - 1] + 5;
  this -> addl[0] = 1;
  this -> amt[0] = 1200;
  this -> rate[0] = 150;
  this -> addl[0] = 10;
  this -> ss[0] = 1;

  this -> compare_overload(1.e-10);
}

TYPED_TEST_P(test_twocpt, single_infusion_cent) {
  this -> reset_events(3);
  this -> time[0] = 0.0;
  this -> time[1] = 0.0;
  for(int i = 2; i < this -> nt; i++) this -> time[i] = this -> time[i - 1] + 5;
  this -> theta[0][2] = 35;  // Vc
  this -> theta[0][3] = 105;  // Vp
  this -> amt[0] = 1200;
  this -> cmt[0] = 2;
  this -> rate[0] = 780;

  this -> compare_overload(1.e-10);
}

REGISTER_TYPED_TEST_SUITE_P(test_twocpt,
                            multiple_bolus,
                            multiple_bolus_with_tlag,
                            multiple_bolus_ss,
                            multiple_infusion,
                            multiple_infusion_ss,
                            single_infusion_cent);

using twocpt_test_types = boost::mp11::mp_product<
  std::tuple,
  ::testing::Types<pmx_solve_twocpt_functor>, // solver 1
  ::testing::Types<pmx_solve_twocpt_functor>, // solver 2
  ::testing::Types<double>,  // TIME
  ::testing::Types<double, stan::math::var_value<double>>,  // AMT
  ::testing::Types<double, stan::math::var_value<double>> , // RATE
  ::testing::Types<double> , // II
  ::testing::Types<double, stan::math::var_value<double>> , // PARAM
  ::testing::Types<stan::math::var_value<double>> , // BIOVAR
  ::testing::Types<stan::math::var_value<double>> , // TLAG
  ::testing::Types<torsten::PMXTwoCptODE>                   // ODE
    >;

INSTANTIATE_TYPED_TEST_SUITE_P(PMX, test_twocpt, twocpt_test_types);


/*
 * Copyright (C) 2021 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

/*
 * Development of this module has been funded by the Monterey Bay Aquarium
 * Research Institute (MBARI) and the David and Lucile Packard Foundation
 */

#include "HydrodynamicsPlugin.hh"

#include <Eigen/Eigen>

#include <ignition/msgs.hh>

namespace tethys
{

class HydrodynamicsPrivateData
{
  /// \brief Values to set via Plugin Parameters.
  /// Plugin Parameter: Added mass in surge, X_\dot{u}.
  public: double paramXdotU;

  /// \brief Plugin Parameter: Added mass in sway, Y_\dot{v}.
  public: double paramYdotV;

  /// \brief Plugin Parameter: Added mass in heave, Z_\dot{w}.
  public: double paramZdotW;

  /// \brief Plugin Parameter: Added mass in roll, K_\dot{p}.
  public: double paramKdotP;

  /// \brief Plugin Parameter: Added mass in pitch, M_\dot{q}.
  public: double paramMdotQ;

  /// \brief Plugin Parameter: Added mass in yaw, N_\dot{r}.
  public: double paramNdotR;

  /// \brief Plugin Parameter: Linear drag in surge.
  public: double paramXu;

  /// \brief Plugin Parameter: Quadratic drag in surge.
  public: double paramXuu;

  /// \brief Plugin Parameter: Linear drag in sway.
  public: double paramYv;

  /// \brief Plugin Parameter: Quadratic drag in sway.
  public: double paramYvv;

  /// \brief Plugin Parameter: Linear drag in heave.
  public: double paramZw;

  /// \brief Plugin Parameter: Quadratic drag in heave.
  public: double paramZww;

  /// \brief Plugin Parameter: Linear drag in roll.
  public: double paramKp;

  /// \brief Plugin Parameter: Quadratic drag in roll.
  public: double paramKpp;

  /// \brief Plugin Parameter: Linear drag in pitch.
  public: double paramMq;

  /// \brief Plugin Parameter: Quadratic drag in pitch.
  public: double paramMqq;

  /// \brief Plugin Parameter: Linear drag in yaw.
  public: double paramNr;

  /// \brief Plugin Parameter: Quadratic drag in yaw.
  public: double paramNrr;

  /// \brief Plugin Parameter: Disable coriolis as part of equation. This is
  /// occasionally useful for testing.
  public: bool enableCoriolis = true;

  /// \brief Water density [kg/m^3].
  public: double waterDensity;

  /// \brief Water current [m/s].
  public: ignition::math::Vector3d waterCurrent {0.0, 0.0, 0.0};

  public: Eigen::VectorXd prevState;

  public: Eigen::VectorXd prevStateDot;

  /// \brief Update current during simulation
  public: void UpdateCurrent(
    const ignition::msgs::Vector3d &_msg)
  {
    std::lock_guard<std::mutex> lock(this->mtx);
    this->waterCurrent = ignition::msgs::Convert(_msg);
  }

  /// Link entity
  public: ignition::gazebo::Entity linkEntity;

  public: ignition::transport::Node node;

  public: std::mutex mtx;
};

void AddAngularVelocityComponent(
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::AngularVelocity>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::AngularVelocity());
  }
    // Create an angular velocity component if one is not present.
  if (!_ecm.Component<ignition::gazebo::components::WorldAngularVelocity>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::WorldAngularVelocity());
  }
}

void AddWorldPose (
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::WorldPose>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::WorldPose());
  }
}

void AddWorldLinearVelocity(
  const ignition::gazebo::Entity &_entity,
  ignition::gazebo::EntityComponentManager &_ecm)
{
  if (!_ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(
      _entity))
  {
    _ecm.CreateComponent(_entity,
      ignition::gazebo::components::WorldLinearVelocity());
  }
}

double SdfParamDouble(
    const std::shared_ptr<const sdf::Element> &_sdf,
    const std::string &_field,
    double _default)
{
  if(!_sdf->HasElement(_field))
  {
    return _default;
  }
  return _sdf->Get<double>(_field);
}

HydrodynamicsPlugin::HydrodynamicsPlugin()
  : dataPtr(std::make_unique<HydrodynamicsPrivateData>())
{
}

void HydrodynamicsPlugin::Configure(
  const ignition::gazebo::Entity &_entity,
  const std::shared_ptr<const sdf::Element> &_sdf,
  ignition::gazebo::EntityComponentManager &_ecm,
  ignition::gazebo::EventManager &/*_eventMgr*/
)
{
  this->dataPtr->waterDensity     = SdfParamDouble(_sdf, "waterDensity", 997.7735);
  this->dataPtr->paramXdotU       = SdfParamDouble(_sdf, "xDotU"       , 5);
  this->dataPtr->paramYdotV       = SdfParamDouble(_sdf, "yDotV"       , 5);
  this->dataPtr->paramZdotW       = SdfParamDouble(_sdf, "zDotW"       , 0.1);
  this->dataPtr->paramKdotP       = SdfParamDouble(_sdf, "kDotP"       , 0.1);
  this->dataPtr->paramMdotQ       = SdfParamDouble(_sdf, "mDotQ"       , 0.1);
  this->dataPtr->paramNdotR       = SdfParamDouble(_sdf, "nDotR"       , 1);
  this->dataPtr->paramXu          = SdfParamDouble(_sdf, "xU"          , 20);
  this->dataPtr->paramXuu         = SdfParamDouble(_sdf, "xUU"         , 0);
  this->dataPtr->paramYv          = SdfParamDouble(_sdf, "yV"          , 20);
  this->dataPtr->paramYvv         = SdfParamDouble(_sdf, "yVV"         , 0);
  this->dataPtr->paramZw          = SdfParamDouble(_sdf, "zW"          , 20);
  this->dataPtr->paramZww         = SdfParamDouble(_sdf, "zWW"         , 0);
  this->dataPtr->paramKp          = SdfParamDouble(_sdf, "kP"          , 20);
  this->dataPtr->paramKpp         = SdfParamDouble(_sdf, "kPP"         , 0);
  this->dataPtr->paramMq          = SdfParamDouble(_sdf, "mQ"          , 20);
  this->dataPtr->paramMqq         = SdfParamDouble(_sdf, "mQQ"         , 0);
  this->dataPtr->paramNr          = SdfParamDouble(_sdf, "nR"          , 20);
  this->dataPtr->paramNrr         = SdfParamDouble(_sdf, "nRR"         , 0);

  _sdf->Get<bool>("enable_coriolis", this->dataPtr->enableCoriolis, true);

  // Create model object, to access convenient functions
  auto model = ignition::gazebo::Model(_entity);
  auto link_name = _sdf->Get<std::string>("link_name");
  this->dataPtr->linkEntity = model.LinkByName(_ecm, link_name);

  if (ignition::gazebo::kNullEntity == this->dataPtr->linkEntity)
  {
    ignerr << "Failed to find link named [" << link_name << "] in model ["
           << model.Name(_ecm) << "]. Plugin failed to initialize." << std::endl;
    return;
  }

  this->dataPtr->prevState = Eigen::VectorXd::Zero(6);
  this->dataPtr->prevStateDot = Eigen::VectorXd::Zero(6);

  AddWorldPose(this->dataPtr->linkEntity, _ecm);
  AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm);
  AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm);

  std::string ns;
  std::string currentTopic {"/ocean_current"};
  if (_sdf->HasElement("namespace"))
  {
    ns = _sdf->Get<std::string>("namespace");
    currentTopic = ignition::transport::TopicUtils::AsValidTopic(
        "/model/" + ns + "/ocean_current");
  }

  this->dataPtr->node.Subscribe(
    currentTopic,
    &HydrodynamicsPrivateData::UpdateCurrent,
    this->dataPtr.get());

  if(_sdf->HasElement("default_current"))
  {
    this->dataPtr->waterCurrent =
      _sdf->Get<ignition::math::Vector3d>("default_current");
  }
}

void HydrodynamicsPlugin::PreUpdate(
      const ignition::gazebo::UpdateInfo &_info,
      ignition::gazebo::EntityComponentManager &_ecm)
{
  if(_info.paused)
    return;

  // These variables are named following Fossen's scheme in "Guidance and Control
  // of Ocean Vehicles." The `state` vector contains the ship's current velocity
  // in the formate [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel].
  // `stateDot` consists of the first derivative in time of the state vector.
  // `Cmat` corresponds to the Centripetal matrix
  // `Dmat` is the drag matrix
  // `Ma` is the added mass.
  Eigen::VectorXd stateDot = Eigen::VectorXd(6);
  Eigen::VectorXd state    = Eigen::VectorXd(6);
  Eigen::MatrixXd Cmat     = Eigen::MatrixXd::Zero(6, 6);
  Eigen::MatrixXd Dmat     = Eigen::MatrixXd::Zero(6, 6);
  Eigen::MatrixXd Ma = Eigen::MatrixXd::Zero(6,6);

  std::lock_guard<std::mutex> lock(this->dataPtr->mtx);
  // Get vehicle state
  ignition::gazebo::Link baseLink(this->dataPtr->linkEntity);
  auto linearVelocity =
    _ecm.Component<ignition::gazebo::components::WorldLinearVelocity>(this->dataPtr->linkEntity);
  auto rotationalVelocity = baseLink.WorldAngularVelocity(_ecm);

  if(!linearVelocity)
  {
    ignerr <<"no linear vel" <<"\n";
    return;
  }

  // Transform state to local frame
  auto pose = baseLink.WorldPose(_ecm);
  // Since we are transforming angular and linear velocity we only care about
  // rotation
  auto localLinearVelocity = pose->Rot().Inverse() *
    (linearVelocity->Data() - this->dataPtr->waterCurrent);
  auto localRotationalVelocity = pose->Rot().Inverse() * *rotationalVelocity;

  state(0) = localLinearVelocity.X();
  state(1) = localLinearVelocity.Y();
  state(2) = localLinearVelocity.Z();

  state(3) = localRotationalVelocity.X();
  state(4) = localRotationalVelocity.Y();
  state(5) = localRotationalVelocity.Z();

  auto dt = (double)_info.dt.count()/1e9;

  auto alpha = 0.9;
  stateDot = alpha * (state - this->dataPtr->prevState)/dt
    + (1-alpha) * this->dataPtr->prevStateDot;

  this->dataPtr->prevStateDot = stateDot;

  this->dataPtr->prevState = state;

  // Added mass according to Fossen's equations (p 37)
  Ma(0,0) = this->dataPtr->paramXdotU;
  Ma(1,1) = this->dataPtr->paramYdotV;
  Ma(2,2) = this->dataPtr->paramZdotW;
  Ma(3,3) = this->dataPtr->paramKdotP;
  Ma(4,4) = this->dataPtr->paramMdotQ;
  Ma(5,5) = this->dataPtr->paramNdotR;
  const Eigen::VectorXd kAmassVec = - Ma * stateDot;

  // Coriollis and Centripetal forces for under water vehicles (Fossen P. 37)
  // Note: this is significantly different from VRX because we need to account
  // for the under water vehicle's additional DOF
  Cmat(0,4) = - this->dataPtr->paramZdotW * state(2);
  Cmat(0,5) = - this->dataPtr->paramYdotV * state(1);
  Cmat(1,3) = this->dataPtr->paramZdotW * state(2);
  Cmat(1,5) = - this->dataPtr->paramXdotU * state(0);
  Cmat(2,3) = - this->dataPtr->paramYdotV * state(1);
  Cmat(2,4) = this->dataPtr->paramXdotU * state(0);
  Cmat(3,1) = - this->dataPtr->paramZdotW * state(2);
  Cmat(3,2) = this->dataPtr->paramYdotV * state(1);
  Cmat(3,4) = - this->dataPtr->paramNdotR * state(5);
  Cmat(3,5) = this->dataPtr->paramMdotQ * state(4);
  Cmat(4,0) = this->dataPtr->paramZdotW * state(2);
  Cmat(4,2) = - this->dataPtr->paramXdotU * state(0);
  Cmat(4,3) = this->dataPtr->paramNdotR * state(5);
  Cmat(4,5) = - this->dataPtr->paramKdotP * state(3);
  Cmat(5,0) = this->dataPtr->paramZdotW * state(2);
  Cmat(5,1) = this->dataPtr->paramXdotU * state(0);
  Cmat(5,3) = - this->dataPtr->paramMdotQ * state(4);
  Cmat(5,4) = this->dataPtr->paramKdotP * state(3);
  const Eigen::VectorXd kCmatVec = - Cmat * state;

  // Damping forces (Fossen P. 43)
  Dmat(0,0) = - this->dataPtr->paramXu - this->dataPtr->paramXuu * abs(state(0));
  Dmat(1,1) = - this->dataPtr->paramYv - this->dataPtr->paramYvv * abs(state(1));
  Dmat(2,2) = - this->dataPtr->paramZw - this->dataPtr->paramZww * abs(state(2));
  Dmat(3,3) = - this->dataPtr->paramKp - this->dataPtr->paramKpp * abs(state(3));
  Dmat(4,4) = - this->dataPtr->paramMq - this->dataPtr->paramMqq * abs(state(4));
  Dmat(5,5) = - this->dataPtr->paramNr - this->dataPtr->paramNrr * abs(state(5));

  const Eigen::VectorXd kDvec = Dmat * state;

  Eigen::VectorXd kTotalWrench = kAmassVec +  kDvec;

  if (this->dataPtr->enableCoriolis)
    kTotalWrench += kCmatVec;

  ignition::math::Vector3d totalForce(-kTotalWrench(0),  -kTotalWrench(1), -kTotalWrench(2));
  ignition::math::Vector3d totalTorque(-kTotalWrench(3),  -kTotalWrench(4), -kTotalWrench(5));

  baseLink.AddWorldWrench(_ecm, pose->Rot()*(totalForce), pose->Rot()*totalTorque);
}

};

IGNITION_ADD_PLUGIN(
  tethys::HydrodynamicsPlugin,
  ignition::gazebo::System,
  tethys::HydrodynamicsPlugin::ISystemConfigure,
  tethys::HydrodynamicsPlugin::ISystemPreUpdate)


//Author: S Middleton
// Purpose: Holds functions for the fitting of Cosmic Tracks in tracker
// Mu2e Cosmics:
#include "CosmicReco/inc/CosmicTrackFit.hh"
#include "CosmicReco/inc/CosmicTrackFinderData.hh"
#include "RecoDataProducts/inc/CosmicTrack.hh"
// art
#include "canvas/Persistency/Common/Ptr.h"

//Mu2e General:
#include "GeometryService/inc/GeomHandle.hh"
#include "GeometryService/inc/DetectorSystem.hh"
#include "RecoDataProducts/inc/TrkFitFlag.hh"
#include "TrackerGeom/inc/Tracker.hh"
#include "RecoDataProducts/inc/TimeCluster.hh"

//For Drift:
#include "BTrk/BaBar/BaBar.hh"
#include "BTrk/BbrGeom/Trajectory.hh"
#include "BTrk/KalmanTrack/KalRep.hh"
#include "BTrk/BbrGeom/HepPoint.h"
#include "BTrk/TrkBase/TrkPoca.hh"
#include "BTrkData/inc/TrkStrawHit.hh"
#include "BTrk/BbrGeom/BbrVectorErr.hh"
#include "BTrk/TrkBase/TrkPoca.hh"
#include "BTrk/ProbTools/ChisqConsistency.hh"
#include "BTrk/TrkBase/TrkMomCalculator.hh"
//Fitting
#include "Mu2eUtilities/inc/ParametricFit.hh"
#include "Mu2eUtilities/inc/BuildLinearFitMatrixSums.hh"
#include "CosmicReco/inc/MinuitDriftFitter.hh"
#include "CosmicReco/inc/DriftFitUtils.hh"
#include "DataProducts/inc/XYZVec.hh"
//ROOT:
#include "TMatrixD.h"
#include "Math/VectorUtil.h"
#include "TMath.h"
#include "Math/Math.h"
#include "Math/DistFunc.h"

// boost
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
//String:
#include <string>

using namespace std;
using namespace boost::accumulators;
using CLHEP::Hep3Vector;
using CLHEP::HepVector;
using namespace ROOT::Math;

std::vector<double> TrackerConstraints(1500, 1500);

struct ycomp : public std::binary_function<XYZVec,XYZVec,bool> {
    bool operator()(XYZVec const& p1, XYZVec p2) { return p1.y() > p2.y(); }
  };

namespace mu2e
{

    CosmicTrackFit::CosmicTrackFit(const Config& conf) :
	_Npara (conf.Npara()),
   	_diag (conf.diag()),
   	_debug  (conf.debug()),
	_dontuseflag (conf.dontuseflag()),
    	_minnsh   (conf.minnsh()),
    	_minnch  (conf.minnch()),
    	_n_outliers (conf.n_outliers()),
	_maxniter (conf.maxniter()),
        _max_seed_chi2 (conf.max_seed_chi2()),
	_max_chi2_change (conf.max_chi2_change()),
	_max_position_deviation (conf.max_position_deviation()),
	_maxHitDOCA (conf.maxHitDOCA()),
	_maxLogL (conf.maxLogL()),
	_gaussTres (conf.gaussTres()),
	_maxTres (conf.maxTres()),
	_maxd (conf.maxd()),
	_maxpull (conf.maxpull())
    	{}

    /* ---------------Initialize Fit----------------//
    //----------------------------------------------*/
    bool CosmicTrackFit::initCosmicTrack(const char* title, CosmicTrackFinderData& TrackData) {

    bool is_ok(false);
    RunFitChi2(title, TrackData);
    is_ok = TrackData._tseed._status.hasAllProperties(TrkFitFlag::helixOK);
    return is_ok;
  }

  std::vector<XYZVec> SortPoints(std::vector<XYZVec> pointY){
        std::vector<XYZVec> sortedPoints;
  	std::sort(pointY.begin(), pointY.end(),ycomp());
  	for (unsigned i=0; i<pointY.size(); ++i) {
      		sortedPoints.push_back(pointY[i]);
      	}
      	return sortedPoints;
   }

  /*-------------Line Direction-------------------------//
    Range of methods for finding track directions - some are redundent-check!
  //----------------------------------------------*/
  //ComboHits
  XYZVec CosmicTrackFit::InitLineDirection(const ComboHit *ch0, const ComboHit *chN) {
      float tx = chN->pos().x() - ch0->pos().x();
      float ty = chN->pos().y() - ch0->pos().y();
      float tz = chN->pos().z() - ch0->pos().z();
      XYZVec track(tx,ty,tz);

      return track.Unit();
    }

    XYZVec CosmicTrackFit::LineDirection(double a1, double b1, const ComboHit *ch0, const ComboHit *chN, XYZVec ZPrime) {
      XYZVec track(a1,b1,1);
      return track.Unit();
    }

    XYZVec CosmicTrackFit::ConvertPointToDetFrame(XYZVec vec){
        Hep3Vector vec1(vec.x(),vec.y(),vec.z());
        GeomHandle<DetectorSystem> det;
        Hep3Vector vec2 = det->toDetector(vec1);
	XYZVec XYZ(vec2.x(), vec2.y(), vec2.z());
	return XYZ;

    }
//--------------Fit-----------------//
// This is the top level call to Fitting routines....
//-------------------------------------------//
  void CosmicTrackFit::BeginFit(const char* title, CosmicTrackFinderData& TrackData){
    //Clear Previous Flags:
    TrackData._tseed._status.clear(TrkFitFlag::helixOK);
    // Initialize:
    bool init(false);
    if (!TrackData._tseed._status.hasAllProperties(TrkFitFlag::circleInit)) {
      init = true;
      if (initCosmicTrack( title, TrackData))
	TrackData._tseed._status.merge(TrkFitFlag::circleInit);
      else
	return;
    }
    //Start Chi2 Fitting:
    if (!init)RunFitChi2(title, TrackData);
  }
/*------------------------------Chi 2 Fit----------------------------//
//   Adds Chi-2 optimization to fitting routine //
//   Refits and adjusts track fit paramters by weights//
//------------------------------------------------------------------*/
void CosmicTrackFit::RunFitChi2(const char* title, CosmicTrackFinderData& TrackData) {
   CosmicTrack* track = &TrackData._tseed._track;
   TrackData._tseed._status.merge(TrkFitFlag::helixOK);
   TrackData._tseed._status.merge(TrkFitFlag::helixConverged);
   FitAll(title, TrackData, track);

}

 void CosmicTrackFit::FitAll(const char* title, CosmicTrackFinderData& trackData,  CosmicTrack* cosmictrack){

     //auto S = std::make_unique<::BuildLinearFitMatrixSums>();// S;
     ::BuildLinearFitMatrixSums S;
     ComboHit   *hitP1(0), *hitP2(0);
     size_t nHits (trackData._chHitsToProcess.size());
     int DOF = (nHits);// - (_Npara);
     const ComboHit* ch0 = &trackData._chHitsToProcess[0];
     const ComboHit* chN = &trackData._chHitsToProcess[trackData._chHitsToProcess.size()-1];
     cosmictrack->SetFirstHitVec(ch0->pos().x(), ch0->pos().y(), ch0->pos().z());
     cosmictrack->SetLastHitVec(chN->pos().x(), chN->pos().y(), chN->pos().z());
     cosmictrack->Set_N(nHits);

     //Step 1: Get Initial Estimate of track direction
     XYZVec ZPrime = InitLineDirection(ch0, chN);
     std::vector<XYZVec> AxesList = ParametricFit::GetAxes(ZPrime);
     TrackAxes InitAxes = ParametricFit::GetTrackAxes(ZPrime);

    //Step 2: Loop over hits and get track parameters based on above estimated track direction
    for (size_t f1=0; f1<nHits; ++f1){
      hitP1 = &trackData._chHitsToProcess[f1];
      if (!use_hit(*hitP1) && hitP1->nStrawHits() < _minnsh)  continue;
      XYZVec point(hitP1->pos().x(),hitP1->pos().y(),hitP1->pos().z());
      std::vector<double> ErrorsXY = ParametricFit::GetErrors(hitP1, InitAxes._XDoublePrime, InitAxes._YDoublePrime);
      S.addPoint(point, InitAxes._XDoublePrime, InitAxes._YDoublePrime, InitAxes._ZPrime, ErrorsXY[0], ErrorsXY[1]);
     }

      //Step 3: Get the first estmiate of track parameters, get a updated track direction vector from these parameters
      double a0 = S.GetAlphaX()[0][0];
      double a1 = S.GetAlphaX()[1][0];
      double b0 = S.GetAlphaY()[0][0];
      double b1 = S.GetAlphaY()[1][0];
      XYZVec Direction(a1,b1,1);
      XYZVec UpdatedTrackDirection =Direction.Unit();

      //Step 4: Update axes and store them as initial axes for plotting
      ZPrime = UpdatedTrackDirection;
      TrackAxes Axes = ParametricFit::GetTrackAxes(ZPrime);
      TrackParams InitParams(a0,a1,b0,b1);

      cosmictrack->SetInitParams(InitParams);
      cosmictrack->SetTrackDirection(UpdatedTrackDirection);
      cosmictrack->SetInitTrackCoordSystem(Axes);
      cosmictrack->SetFitTrackCoOrdSystem(Axes);
      //Step 5: Loop for initial diagnostics
      if(_debug>0){
	cosmictrack->set_initchisq_dofY(S.GetChi2Y()/abs(DOF));
	cosmictrack->set_initchisq_dofX(S.GetChi2X()/abs(DOF));
	cosmictrack->set_initchisq_dof(S.GetTotalChi2()/abs(DOF));
      }

      //Step 6: Begin iteration for finding the best track fit possible.
      unsigned niter(0);
      ::BuildLinearFitMatrixSums S_niteration;
      bool converged = false;
      CosmicTrack* BestTrack = cosmictrack;
      double chi2_best_track = 10000000;//chosen arbitary high number

      //Step 7: find best prev chi2
      while(niter < _maxniter && converged==false){
      	niter +=1;
 	double previous_chi2, changed_chi2;
      	if (niter == 1) {
 	     	 previous_chi2 = S.GetTotalChi2()/abs(DOF);
 	     	 chi2_best_track = previous_chi2;
       }
      	else {
      	 	previous_chi2 = S_niteration.GetTotalChi2()/abs(DOF);
      	}
     	//Step 8: Remove previous sums and use previously stored axes from updated last iteration:
     	S_niteration.clear();
     	Axes = ParametricFit::GetTrackAxes(cosmictrack->GetTrackDirection());
     	cosmictrack->set_niter(niter );
     	for (size_t f4=0; f4 < nHits; ++f4){

     	      hitP2 = &trackData._chHitsToProcess[f4];
      	      if (((!use_hit(*hitP2) ) && (hitP2->nStrawHits() < _minnsh) )) continue;
	      XYZVec point(hitP2->pos().x(),hitP2->pos().y(),hitP2->pos().z());
	      std::vector<double> ErrorsXY = ParametricFit::GetErrors(hitP2, Axes._XDoublePrime, Axes._YDoublePrime);
	      S_niteration.addPoint(point, Axes._XDoublePrime, Axes._YDoublePrime, Axes._ZPrime, ErrorsXY[0],ErrorsXY[1]);
    	  }
          //Get new parameters
     	  a0 = S_niteration.GetAlphaX()[0][0];
          a1 = S_niteration.GetAlphaX()[1][0];
	  b0 = S_niteration.GetAlphaY()[0][0];
	  b1 = S_niteration.GetAlphaY()[1][0];

    	  //Step 9: Get new direction:
	   XYZVec DirectionSecond(a1,b1,1);
	   XYZVec UpdatedTrackDirectionSecond = DirectionSecond.Unit();

 	   //Step 10 - Get New Axes:
 	   //Axes = ParametricFit::GetTrackAxes(UpdatedTrackDirectionSecond);
	   cosmictrack->SetTrackDirection(UpdatedTrackDirectionSecond);
	   cosmictrack->SetFitTrackCoOrdSystem(Axes);
	   //Step 11: Update Parameters:
	   TrackParams FitParams(a0,a1,b0,b1);
	   cosmictrack->SetFitParams(FitParams);

           //Step 12: Update Chi2:
           if(_debug > 0){
           	   cosmictrack->Diag.FinalChiTot = (S_niteration.GetTotalChi2()/abs(DOF));
		   cosmictrack->Diag.FinalChiY = S_niteration.GetChi2Y()/abs(DOF);
		   cosmictrack->Diag.FinalChiX = S_niteration.GetChi2X()/abs(DOF);
	   }

	   if(abs((a0) - (InitParams.A0)) > _max_position_deviation or abs((b0) - (InitParams.B0)) > _max_position_deviation){continue;}

	   //Step13: Check if Chi2 is improved:, If so call this "BestTrack"
	   float updated_chi2 = S_niteration.GetTotalChi2()/abs(DOF);
	   changed_chi2 = chi2_best_track - updated_chi2;

           if (updated_chi2 < chi2_best_track  ){

		//BestTrack->clear_diag();
		TrackParams FitParams(a0,a1,b0,b1);
	   	BestTrack->SetFitParams(FitParams);
	        BestTrack->SetFitTrackCoOrdSystem(Axes);
	        BestTrack->SetTrackDirection(cosmictrack->GetTrackDirection());

      		XYZVec EndTrackPosition(BestTrack->FitParams.A0,BestTrack->FitParams.B0,0);
      		TrackEquation EndTrack(EndTrackPosition, BestTrack->TrackCoordSystem._ZPrime);
      		BestTrack->SetTrackEquation(EndTrack);

   		//Step 14: Save this Chi2:
                if(_debug > 0){
                	BestTrack->Diag.FinalChiTot = (S_niteration.GetTotalChi2()/abs(DOF));
		   	BestTrack->Diag.FinalChiY = S_niteration.GetChi2Y()/abs(DOF);
		        BestTrack->Diag.FinalChiX = S_niteration.GetChi2X()/abs(DOF);

	        }
                chi2_best_track = updated_chi2;
                cosmictrack=BestTrack;
	      }

         //If on any iteration the change in chi2 becomes small than we can assume we have converged on a track
         if( abs(changed_chi2) < _max_chi2_change ){
                  converged = true;
                  cosmictrack->converged = true;
 	      }

           //If at end of the iteration process but the track is still not converging then remove it
           if(niter == _maxniter && converged ==false ){
 		    trackData._tseed._status.clear(TrkFitFlag::helixOK);
 		    trackData._tseed._status.clear(TrkFitFlag::helixConverged);
 		    continue;
		 }

   }//end while
   cosmictrack=BestTrack;
   if(cosmictrack->converged and  _diag > 0){
	 for (size_t f5=0; f5<nHits; ++f5){
     		hitP2 = &trackData._chHitsToProcess[f5];
                if (((!use_hit(*hitP2) ) && (hitP2->nStrawHits() < _minnsh) )) continue;
		XYZVec point(hitP2->pos().x(),hitP2->pos().y(),hitP2->pos().z());
		XYZVec point_prime(point.Dot(BestTrack->TrackCoordSystem._XDoublePrime), point.Dot(BestTrack->TrackCoordSystem._YDoublePrime), point.Dot(BestTrack->TrackCoordSystem._ZPrime));

		std::vector<double> ErrorsXY = ParametricFit::GetErrors(hitP1, BestTrack->TrackCoordSystem._XDoublePrime, BestTrack->TrackCoordSystem._YDoublePrime);

	      	float newRx = ParametricFit::GetResidualX(BestTrack->FitParams.A0, BestTrack->FitParams.A1, point_prime);
		float newRy = ParametricFit::GetResidualY(BestTrack->FitParams.B0, BestTrack->FitParams.B1, point_prime);
		// Remove bad tracks:
		if(abs(newRx)/ErrorsXY[0] > _maxpull or abs(newRy)/ErrorsXY[1] > _maxpull ){ cosmictrack->converged = false;}

		BestTrack->SetCovarience(S_niteration.GetCovX()[0][0],S_niteration.GetCovX()[0][1], S_niteration.GetCovX()[1][0], S_niteration.GetCovX()[1][1], S_niteration.GetCovY()[0][0], S_niteration.GetCovY()[0][1], S_niteration.GetCovY()[1][0], S_niteration.GetCovY()[1][1]);

	      	cosmictrack = BestTrack;

      	}
    }
     S.clear();

     if(cosmictrack->converged == true){

          ConvertFitToDetectorFrame(trackData, cosmictrack->TrackCoordSystem, cosmictrack->GetTrackPosition(), cosmictrack->GetTrackDirection(), cosmictrack, true, false);

     }

   }

/*------------Translate fit back into XYZ and/or the detector frame------//
Using matrices to ctransform from local to global coordinates
//-----------------------------------------------------------------------*/
void CosmicTrackFit::ConvertFitToDetectorFrame(CosmicTrackFinderData& trackData, TrackAxes axes, XYZVec Position, XYZVec Direction, CosmicTrack* cosmictrack, bool seed, bool det){
	TMatrixD A(3,3);
	A[0][0] = axes._XDoublePrime.X();
	A[0][1] = axes._YDoublePrime.X();
	A[0][2] = axes._ZPrime.X();

	A[1][0] = axes._XDoublePrime.Y();
	A[1][1] = axes._YDoublePrime.Y();
	A[1][2] = axes._ZPrime.Y();

	A[2][0] = axes._XDoublePrime.Z();
	A[2][1] = axes._YDoublePrime.Z();
	A[2][2] = axes._ZPrime.Z();

	TMatrixD P(3,1);
	P[0][0] = Position.X();
	P[1][0] = Position.Y();
	P[2][0] = Position.Z();

	TMatrixD D(3,1);
	D[0][0] = Direction.X();
	D[1][0] = Direction.Y();
	D[2][0] = Direction.Z();

	TMatrixD PXYZ(A*P);
	TMatrixD DXYZ(A*D);

	DXYZ[0][0] = DXYZ[0][0]/ DXYZ[2][0];
	DXYZ[1][0] = DXYZ[1][0]/ DXYZ[2][0];
	DXYZ[2][0] = DXYZ[2][0]/ DXYZ[2][0];

	PXYZ[0][0] = PXYZ[0][0] - DXYZ[0][0]*PXYZ[2][0]/ DXYZ[2][0];
	PXYZ[1][0] = PXYZ[1][0]- DXYZ[1][0]*PXYZ[2][0]/ DXYZ[2][0];
	PXYZ[2][0] = PXYZ[2][0]- DXYZ[2][0]*PXYZ[2][0]/ DXYZ[2][0];
	/*
	//TMatrixD sigmaPos(3,1);
	TMatrixD Cov(3,3);
	TMatrixD At(A);
	At.T();
	for(unsigned i =0; i< 3; i++){
		for(unsigned j =0 ; j <3; j++){

		    if(i==0 and j==0) {Cov[i][j] =  cosmictrack->FitParams.Covarience.sigA0; }
		    if(i==1 and j ==1) { Cov[i][j] = cosmictrack->FitParams.Covarience.sigA1;}
		    else {Cov[i][j] =0;}
		}
	}
	TMatrixD CovXYZ(A*Cov*At);
	for(unsigned i=0; i<3 ; i++){
		for(unsigned j=0; j<3; j++){
			cout<<"i= "<<i<<" j= "<<j<<" "<<Cov[i][j]<<"=======> "<<CovXYZ[i][j]<<endl;

		}

	}
	*/
	TMatrixD sigmaPos(3,1);
	sigmaPos[0][0] = cosmictrack->FitParams.Covarience.sigA0;
	sigmaPos[1][0] = cosmictrack->FitParams.Covarience.sigB0;
	sigmaPos[2][0] = 0;
	TMatrixD sigmaPosXYZ(A*sigmaPos);

	TMatrixD sigmaDir(3,1);
	sigmaDir[0][0] = cosmictrack->FitParams.Covarience.sigA1;
	sigmaDir[1][0] = cosmictrack->FitParams.Covarience.sigB1;
	sigmaDir[2][0] = 0;
	TMatrixD sigmaDirXYZ(A*sigmaDir);

	XYZVec Pos(PXYZ[0][0], PXYZ[1][0], PXYZ[2][0]);
	XYZVec Dir(DXYZ[0][0], DXYZ[1][0] , DXYZ[2][0]);

	if(seed == true){//is this the local coords at start?
		if (det == true){ // is this detector frame?
			XYZVec PosInDet = ConvertPointToDetFrame(Pos);
			TrackEquation XYZTrack(PosInDet, Dir);
			cosmictrack->SetTrackEquationXYZ(XYZTrack);
			cosmictrack->sigmaPos.SetXYZ(sigmaPosXYZ[0][0], sigmaPosXYZ[1][0], sigmaPosXYZ[2][0]);
			cosmictrack->sigmaDir.SetXYZ(sigmaDirXYZ[0][0], sigmaDirXYZ[1][0], sigmaDirXYZ[2][0]);

		} else {
			TrackEquation XYZTrack(Pos, Dir);
			cosmictrack->SetTrackEquationXYZ(XYZTrack);

		}}
	else{
		TrackEquation XYZTrack(Pos, Dir);
		cosmictrack->SetMinuitTrackEquation(XYZTrack);
		cosmictrack->set_fit_phi(acos(Dir.x()/Dir.Mag2()));
		cosmictrack->set_fit_theta(acos(Dir.y()/sqrt(Dir.Mag2())));
	}

}

bool CosmicTrackFit::goodTrack(CosmicTrack& track) //Not used
  {
    if(track.Diag.FinalChiTot < _max_seed_chi2) return true;
    else return false;
  }

/*--------------USE? ---------------------------//
//          Checks if flag                        //
//------------------------------------------------*/
bool CosmicTrackFit::use_hit(const ComboHit& thit) const //currently not used -keep option to set
  {
    return (!thit._flag.hasAnyProperty(_dontuseflag));
  }
bool CosmicTrackFit::use_track(double track_length) const //not used but keep for now
  {
     return (track_length > _maxd) ? false : true ;
  }
 /*-------------Drift Fit Diagnotics--------------//
This is were the fitter "talks" to the Minuit fitter. "EndResult" is the minimzed track parameters
//------------------------------------------------*/
void CosmicTrackFit::DriftFit(CosmicTrackFinderData& trackData, StrawResponse const& _srep ){

	 FitResult endresult = MinuitDriftFitter::DoFit(_diag, trackData, _srep, _tracker, _maxHitDOCA, _minnch, _maxLogL, _gaussTres, _maxTres);

         trackData._tseed._track.MinuitFitParams.A0 =  endresult.bestfit[0];//a0
         trackData._tseed._track.MinuitFitParams.A1 =  endresult.bestfit[1];//a1
         trackData._tseed._track.MinuitFitParams.B0 =  endresult.bestfit[2];//b0
         trackData._tseed._track.MinuitFitParams.B1 =  endresult.bestfit[3];//b1
         trackData._tseed._track.MinuitFitParams.T0 =  endresult.bestfit[4];//t0

	 trackData._tseed._track.MinuitFitParams.deltaA0 =  endresult.bestfiterrors[0];//erra0
         trackData._tseed._track.MinuitFitParams.deltaA1 =  endresult.bestfiterrors[1];//erra1
         trackData._tseed._track.MinuitFitParams.deltaB0 =  endresult.bestfiterrors[2];//errb0
         trackData._tseed._track.MinuitFitParams.deltaB1 =  endresult.bestfiterrors[3];//errb1
         trackData._tseed._track.MinuitFitParams.deltaT0 =  endresult.bestfiterrors[4];//errt0

         if(endresult.bestfitcov.size() !=0 ){
		 TrackCov Cov(endresult.bestfitcov[0], 0., 0., endresult.bestfitcov[1], endresult.bestfitcov[2],0.,0., endresult.bestfitcov[3]);
		 trackData._tseed._track.MinuitFitParams.Covarience = Cov;
         }
         if(endresult.NLL !=0){ trackData._tseed._track.minuit_converged = true; }

	 XYZVec X(1,0,0);
	 XYZVec Y(0,1,0);
	 XYZVec Z(0,0,1);

	 TrackAxes XYZ(X,Y,Z);
	 trackData._tseed._track.MinuitCoordSystem = XYZ;

	if(endresult.FullFitEndTimeResiduals.size() >0){
		for(unsigned i = 0; i< endresult.FullFitEndTimeResiduals.size()-1; i++){
			if( endresult.FullFitEndTimeResiduals[i] > _maxTres or isnan(endresult.FullFitEndTimeResiduals[i])==true){
				trackData._tseed._track.n_outliers +=1;
				trackData._tseed._straw_chits[i]._flag.merge(StrawHitFlag::outlier);

		}
		}

	}
        if( trackData._tseed._track.n_outliers  > _n_outliers) {
		trackData._tseed._track.minuit_converged = false;
	  }

	/*This code was built to help transofrm into BTrk - it didnt work but may be useful:
 	std::vector<TrkStrawHitSeed>const trkseeds = trackData._tseed.trkstrawhits();
        cout<<"size track seed "<<trkseeds.size()<<" "<<trackData._tseed._straw_chits.size()<<std::endl;
     	for(auto const& ths : trkseeds ){

     	 	size_t index = ths.index();
     		const ComboHit& strawhit(trackData._tseed._straw_chits.at(index));
      		const Straw& straw = _tracker->getStraw(strawhit.strawId());

        	cout<<"nSH "<<strawhit.nStrawHits()<<endl;
        	TrkStrawHit* trkhit = new TrkStrawHit(_srep, strawhit, straw, ths.index(),ths.t0(),100., 5.,1.);
        	cout<<" Phi "<<trkhit->driftPhi()<<" v drift "<<trkhit->driftVelocity()<<" time"<<trkhit->driftTime()<<endl;

 		}*/

}

}//end namespace