/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * 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.
 *
 *******************************************************************************/

#include <miopen/solver.hpp>

#include <miopen/algorithm.hpp>
#include <miopen/conv/data_invoke_params.hpp>
#include <miopen/conv_solution.hpp>
#include <miopen/convolution_fft.hpp>
#include <miopen/env.hpp>
#include <miopen/tensor.hpp>

#include <boost/any.hpp>

namespace miopen {
namespace solver {

MIOPEN_DECLARE_ENV_VAR(MIOPEN_DEBUG_CONV_FFT)

static void cgemm_grid(size_t* global_work_size,
                       size_t* local_work_size,
                       int cgemm_choice,
                       const int N,
                       const int out_c,
                       const int out_n)
{
    unsigned int threadTile[2];
    unsigned int groupSize[2];

    // grid for cgemm
    if(cgemm_choice == 1)
    {
        threadTile[0] = 4;
        threadTile[1] = 4;

        groupSize[0] = 16;
        groupSize[1] = 16;

        local_work_size[0] = 16;
        local_work_size[1] = 16;
    }
    else if(cgemm_choice == 2)
    {
        threadTile[0] = 4;
        threadTile[1] = 4;

        groupSize[0] = 4;
        groupSize[1] = 4;

        local_work_size[0] = 64;
        local_work_size[1] = 1;
    }
    else
    {
        threadTile[0] = 2;
        threadTile[1] = 2;

        groupSize[0] = 4;
        groupSize[1] = 4;

        local_work_size[0] = 64;
        local_work_size[1] = 1;
    }

    global_work_size[2] = 1;
    global_work_size[2] *= N;

    unsigned int sizeOfC0         = out_c;
    unsigned int sizeOfC1         = out_n;
    auto macroTile0               = static_cast<unsigned int>(groupSize[0] * threadTile[0]);
    auto macroTile1               = static_cast<unsigned int>(groupSize[1] * threadTile[1]);
    unsigned int totalWorkGroups0 = sizeOfC0 / macroTile0;
    unsigned int totalWorkGroups1 = sizeOfC1 / macroTile1;
    // b/c single kernel, add extra work-group here if edge needed
    if(totalWorkGroups0 * macroTile0 < sizeOfC0)
    {
        totalWorkGroups0++;
    }
    if(totalWorkGroups1 * macroTile1 < sizeOfC1)
    {
        totalWorkGroups1++;
    }
    global_work_size[0] = totalWorkGroups0 * local_work_size[0];
    global_work_size[1] = totalWorkGroups1 * local_work_size[1];
}

bool fft::IsApplicable(const ConvolutionContext& ctx) const
{
    // disable running any FFT based convolutions by checking this env variable
    if(ctx.direction.IsBackwardWrW() || !ctx.conv_problem.IsFp32())
        return false;

    if(!ctx.IsLayoutDefault())
    {
        return false;
    }

    const auto is_fwd    = ctx.direction.IsForward();
    decltype(auto) conv  = ctx.conv_problem.GetConv();
    decltype(auto) xDesc = is_fwd ? ctx.conv_problem.GetIn() : ctx.conv_problem.GetOut();
    decltype(auto) yDesc = is_fwd ? ctx.conv_problem.GetOut() : ctx.conv_problem.GetIn();
    decltype(auto) wDesc = ctx.conv_problem.GetWeights();

    if(conv.GetSpatialDimension() != 2 || conv.group_count != 1 ||
       !miopen::all_of(conv.GetConvDilations(), [](auto v) { return v == 1; }))
        return false;

    int in_n, in_c, in_h, in_w;
    int out_n, out_c, out_h, out_w;
    int wei_k, wei_c, wei_h, wei_w;
    std::tie(in_n, in_c, in_h, in_w)     = miopen::tien<4>(xDesc.GetLengths());
    std::tie(out_n, out_c, out_h, out_w) = miopen::tien<4>(yDesc.GetLengths());
    std::tie(wei_k, wei_c, wei_h, wei_w) = miopen::tien<4>(wDesc.GetLengths());

    // FFT convolutions only works for specific config(s)
    // coverage to expand gradually

    if((in_n < 1) || (in_n > 512) || (wei_k < 1) || (wei_k > 512) || ((in_c * in_n) % 16 != 0) ||
       (wei_c * wei_k) % 16 != 0 || (out_c * out_n) % 16 != 0)
        return false;

    if((std::tie(in_h, in_w) != std::make_tuple(28, 28)) &&
       (std::tie(in_h, in_w) != std::make_tuple(27, 27)) &&
       (std::tie(in_h, in_w) != std::make_tuple(14, 14)) &&
       (std::tie(in_h, in_w) != std::make_tuple(7, 7)))
        return false;

    const auto cparam = std::make_tuple(conv.GetConvPads()[0],
                                        conv.GetConvPads()[1],
                                        conv.GetConvStrides()[0],
                                        conv.GetConvStrides()[1]);

    return std::tie(wei_h, wei_w) == std::make_tuple(5, 5) && cparam == std::make_tuple(2, 2, 1, 1);
}

size_t fft::GetWorkspaceSize(const ConvolutionContext& ctx) const
{
    const auto fwd       = ctx.direction.IsForward();
    decltype(auto) xDesc = fwd ? ctx.conv_problem.GetIn() : ctx.conv_problem.GetOut();
    decltype(auto) yDesc = fwd ? ctx.conv_problem.GetOut() : ctx.conv_problem.GetIn();
    decltype(auto) wDesc = ctx.conv_problem.GetWeights();

    int in_n, in_c, in_h, in_w;
    std::tie(in_n, in_c, in_h, in_w) = miopen::tien<4>(xDesc.GetLengths());

    int out_n, out_c, out_h, out_w;
    std::tie(out_n, out_c, out_h, out_w) = miopen::tien<4>(yDesc.GetLengths());

    int wei_k, wei_c, wei_h, wei_w;
    std::tie(wei_k, wei_c, wei_h, wei_w) = miopen::tien<4>(wDesc.GetLengths());

    // FFT convolutions only works for specific config(s)
    // coverage to expand gradually

    const int N       = FFTConvParams::TileSize(in_h, in_w);
    const int Padding = FFTConvParams::TransposePadding;

    int temp_size = 0;

    if(fwd)
    {
        int temp_size1 = (in_c * in_n + Padding) + (wei_k * wei_c + Padding);
        int temp_size2 = (out_n * out_c + Padding);
        temp_size      = std::max(temp_size1, temp_size2);
    }
    else
    {
        int temp_size1 = (out_n * out_c + Padding) + (wei_k * wei_c + Padding);
        int temp_size2 = (in_c * in_n + Padding);
        temp_size      = std::max(temp_size1, temp_size2);
    }

    return 2 * 2 * N * temp_size * sizeof(float);
}

ConvSolution fft::GetSolution(const ConvolutionContext& ctx) const
{
    int in_n = ctx.batch_sz, in_c = ctx.n_inputs, in_h = ctx.in_height, in_w = ctx.in_width;
    int out_n = ctx.batch_sz, out_c = ctx.n_outputs;

    const int N          = FFTConvParams::TileSize(in_h, in_w);
    const int NumKernels = FFTConvParams::NumKernels;

    size_t global_work_size[NumKernels][3];
    size_t local_work_size[NumKernels][3];

    for(int ik = 0; ik < NumKernels; ik++)
    {
        global_work_size[ik][0] = local_work_size[ik][0] = 1;
        global_work_size[ik][1] = local_work_size[ik][1] = 1;
        global_work_size[ik][2] = local_work_size[ik][2] = 1;
    }

    // grid for FFT kernels
    if((in_h == 7) && (in_w == 7))
    {
        local_work_size[0][0]  = 192;
        global_work_size[0][0] = ((in_c * out_n) / 16) * local_work_size[0][0];

        local_work_size[1][0]  = 192;
        global_work_size[1][0] = ((in_c * out_c) / 16) * local_work_size[1][0];

        local_work_size[6][0]  = 192;
        global_work_size[6][0] = ((out_n * out_c) / 16) * local_work_size[6][0];
    }
    else if((in_h == 14) && (in_w == 14))
    {
        local_work_size[0][0]  = 128;
        global_work_size[0][0] = ((in_c * out_n) / 4) * local_work_size[0][0];

        local_work_size[1][0]  = 128;
        global_work_size[1][0] = ((in_c * out_c) / 4) * local_work_size[1][0];

        local_work_size[6][0]  = 128;
        global_work_size[6][0] = ((out_n * out_c) / 4) * local_work_size[6][0];
    }
    else
    {
        local_work_size[0][0]  = 64;
        global_work_size[0][0] = in_c * out_n * local_work_size[0][0];

        local_work_size[1][0]  = 64;
        global_work_size[1][0] = in_c * out_c * local_work_size[1][0];

        local_work_size[6][0]  = 64;
        global_work_size[6][0] = out_n * out_c * local_work_size[6][0];
    }

    // decide tranpose kernel options based on params
    int in_tranpose_choice = 0;
    int wt_tranpose_choice = 0;
    int ot_tranpose_choice = 0;

    // grid for transpose kernels
    if((in_h == 7) && (in_w == 7))
    {
        local_work_size[5][0]  = 256;
        global_work_size[5][0] = (1 + N / 16) * (out_n * out_c / 16) * local_work_size[5][0];
    }
    else if((in_h == 14) && (in_w == 14))
    {
        local_work_size[2][0]  = 256;
        global_work_size[2][0] = (1 + N / 16) * (in_c * out_n / 16) * local_work_size[2][0];

        local_work_size[3][0]  = 256;
        global_work_size[3][0] = (1 + N / 16) * (in_c * out_c / 16) * local_work_size[3][0];

        local_work_size[5][0]  = 256;
        global_work_size[5][0] = (1 + N / 16) * (out_n * out_c / 16) * local_work_size[5][0];
    }
    else
    {
        if((in_n * in_c >= 64) && ((in_n * in_c) % 32 == 0))
            in_tranpose_choice = 1;
        if((out_c * in_c >= 64) && ((out_c * in_c) % 32 == 0))
            wt_tranpose_choice = 1;
        if((out_n * out_c >= 64) && ((out_n * out_c) % 32 == 0))
            ot_tranpose_choice = 1;

        int in_tranpose_bwidth = in_tranpose_choice != 0 ? 32 : 16;
        int wt_tranpose_bwidth = wt_tranpose_choice != 0 ? 32 : 16;
        int ot_tranpose_bwidth = ot_tranpose_choice != 0 ? 32 : 16;

        local_work_size[2][0] = 256;
        global_work_size[2][0] =
            (N / in_tranpose_bwidth) * (in_c * out_n / in_tranpose_bwidth) * local_work_size[2][0];

        local_work_size[3][0] = 256;
        global_work_size[3][0] =
            (N / wt_tranpose_bwidth) * (in_c * out_c / wt_tranpose_bwidth) * local_work_size[3][0];

        local_work_size[5][0] = 256;
        global_work_size[5][0] =
            (N / ot_tranpose_bwidth) * (out_n * out_c / ot_tranpose_bwidth) * local_work_size[5][0];
    }

    // cgemm kernel options
    int cgemm_choice = 0;

    if(((in_h == 28) && (in_w == 28)) || ((in_h == 14) && (in_w == 14)) ||
       ((in_h == 7) && (in_w == 7)))
        cgemm_choice = 2;
    else if((in_h == 27) && (in_w == 27))
        cgemm_choice = 1;

    if((in_n < 16) || (in_c < 16) || (out_c < 16))
        cgemm_choice = 0;

    cgemm_grid(global_work_size[4], local_work_size[4], cgemm_choice, N, out_c, out_n);

    std::string parms;

    if(in_tranpose_choice == 0)
        parms += " -DCFF_TRANSP_IN_MOD16=1";
    if(wt_tranpose_choice == 0)
        parms += " -DCFF_TRANSP_WT_MOD16=1";
    if(ot_tranpose_choice == 0)
        parms += " -DCFF_TRANSP_OT_MOD16=1";

    switch(cgemm_choice)
    {
    case 1: parms += " -DCFF_CGEMM_CHOICE_1=1"; break;
    case 2: parms += " -DCFF_CGEMM_CHOICE_2=1"; break;
    default: parms += " -DCFF_CGEMM_CHOICE_0=1"; break;
    }

    if((in_h == 28) && (in_w == 28))
        parms += " -DCFF_IMG_SZ_28_28";
    else if((in_h == 27) && (in_w == 27))
        parms += " -DCFF_IMG_SZ_27_27";
    else if((in_h == 14) && (in_w == 14))
        parms += " -DCFF_IMG_SZ_14_14";
    else if((in_h == 7) && (in_w == 7))
        parms += " -DCFF_IMG_SZ_7_7";

    const auto workSpaceSize = GetWorkspaceSize(ctx);

    parms += " -DCFF_IMG_H=";
    parms += std::to_string(in_h);
    parms += " -DCFF_IMG_W=";
    parms += std::to_string(in_w);
    parms += " -DCFF_BATCH=";
    parms += std::to_string(in_n);
    parms += " -DCFF_NFILTER=";
    parms += std::to_string(out_c);
    parms += " -DCFF_CHANNELS=";
    parms += std::to_string(in_c);
    parms += " -DCFF_HALFW=";
    parms += std::to_string(workSpaceSize / (2 * 2 * sizeof(float)));

    if(!ctx.direction.IsForward())
    {
        parms += " -DCFF_BACKWARD";
    }

    const std::string algorithm    = "miopenConvolutionFwdAlgoFFT";
    const std::string program_name = "MIOpenConvFFT.cl";

    auto sol        = ConvSolution{miopenStatusSuccess};
    sol.workspce_sz = workSpaceSize;

    // skip front transposes for 7x7
    const auto skip_front_transposes = (in_h == 7) && (in_w == 7);

    for(int ik = 0; ik < NumKernels; ik++)
    {
        if(skip_front_transposes && ((ik == 2) || (ik == 3)))
            continue;

        const auto kernel_name = [=]() {
            switch(ik)
            {
            case 0: return "MIOpenConvFFT_fwd_in";
            case 1: return "MIOpenConvFFT_fwd_we";
            case 2: return "MIOpenConvFFT_transpose_in";
            case 3: return "MIOpenConvFFT_transpose_we";
            case 4: return "MIOpenConvFFT_cgemm";
            case 5: return "MIOpenConvFFT_transpose_out";
            case 6: return "MIOpenConvFFT_inv_out";
            default: assert(false); return "";
            }
        }();

        std::vector<size_t> vld(3);
        std::vector<size_t> vgd(3);

        vld[0] = local_work_size[ik][0];
        vld[1] = local_work_size[ik][1];
        vld[2] = local_work_size[ik][2];

        vgd[0] = global_work_size[ik][0];
        vgd[1] = global_work_size[ik][1];
        vgd[2] = global_work_size[ik][2];

        auto kernel         = KernelInfo{};
        kernel.kernel_file  = program_name;
        kernel.kernel_name  = kernel_name;
        kernel.comp_options = parms;
        kernel.g_wk         = vgd;
        kernel.l_wk         = vld;
        sol.construction_params.push_back(kernel);
    }

    sol.invoker_factory = [=](const std::vector<Kernel>& kernels) {
        int halfw = static_cast<int>(workSpaceSize) / (2 * 2 * static_cast<int>(sizeof(float)));
        const int padding = FFTConvParams::TransposePadding;

        return [=](const Handle& handle, const AnyInvokeParams& primitive_params) {
            const auto& params  = primitive_params.CastTo<conv::DataInvokeParams>();
            const auto& tensors = params.tensors;

            if(params.workSpaceSize < workSpaceSize)
                MIOPEN_THROW("Not enough workspace for FFT: expected " +
                             std::to_string(workSpaceSize) + ", got " +
                             std::to_string(params.workSpaceSize));

            float time_fft = 0;
            int kernel_id  = 0;
            for(int ik = 0; ik < NumKernels; ik++)
            {
                if(skip_front_transposes && ((ik == 2) || (ik == 3)))
                    continue;

                const auto& k = handle.Run(kernels[kernel_id++]);

                switch(ik)
                {
                case 0: k(tensors.in, params.workSpace); break;
                case 1: k(tensors.w, params.workSpace); break;
                case 4: {
                    k(params.workSpace,
                      0,
                      halfw + N * (in_n * in_c + padding),
                      halfw + 0,
                      out_c,
                      out_n * out_c + padding,
                      in_c,
                      in_c * out_c + padding,
                      in_c,
                      in_n * in_c + padding,
                      out_c,
                      in_n,
                      N,
                      in_c);
                    break;
                }
                case 6: k(params.workSpace, tensors.out); break;
                case 2:
                case 3:
                case 5: k(params.workSpace); break;
                default: assert(false);
                }

                if(handle.IsProfilingEnabled())
                    time_fft += handle.GetKernelTime();
            }

            if(handle.IsProfilingEnabled())
            {
                handle.ResetKernelTime();
                handle.AccumKernelTime(time_fft);
            }
        };
    };

    return sol;
}

} // namespace solver
} // namespace miopen


#include "Controller.hpp"
#include "AS5600.hpp"
#include "Canon.hpp"
#include "DRV8825.hpp"
#include "INA219.hpp"
#include <boost/circular_buffer.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/math/statistics/linear_regression.hpp>
#include <boost/math/statistics/univariate_statistics.hpp>
#include <cmath>
#include <cstring>
#include <fcntl.h>
#include <iostream>
#include <linux/input.h>
#include <poll.h>
#include <unistd.h>

using namespace std::chrono_literals;

extern void Max_Search_Hill_Climbing(const std::function<bool()> &loop, const std::function<double(double *)> &f,
									 double *a, double *fa, double *b, double *fb, double c, double fc,
									 double grid_size, double tolerance);

extern void Max_Search_Global(const std::function<bool()> &loop, const std::function<double(double)> &f, double *a,
							  double *fa, double *b, double *fb, double grid_size);

#define eprintf(...)                                                                                \
	if (config_.verbose)                                                                                \
		printf(__VA_ARGS__);

Controller::Controller(std::atomic<float> &focus, std::atomic<float> &blur_stddev_, unsigned int aperture,
					   unsigned int focal_length)
	: focus_(focus), blur_stddev_(blur_stddev_), aperture_init(aperture), focal_len_init(focal_length),
	  current_angle(NAN), zoom_direction(0), angle_poll_ms(100)
{
}

void Controller::Initialize()
{
	// Init lens
	lens = new Canon(config_.lens_port.c_str());
	std::string lens_name;
	lens->getInfo(lens_name, focus_len_min, focus_len_max);
	std::cout << lens_name << std::endl;

	// Init current sensor
	char path[20];
	sprintf(path, "/dev/i2c-%d", config_.i2c);
	sensor = new INA219(path, config_.ina219_addr);
	encoder = new AS5600(path, config_.as5600_addr);

	// Init motor
	sprintf(path, "/dev/gpiochip%d", config_.gpio_chip);
	bool second_motor = config_.motor == 2;
	motor = new DRV8825(config_.steps, path, second_motor ? M2_DIR_PIN : M1_DIR_PIN,
						second_motor ? M2_STEP_PIN : M1_STEP_PIN, second_motor ? M2_ENABLE_PIN : M1_ENABLE_PIN);
	motor->setMicrostep(config_.micro_step);
	motor->setSpeedProfile(DRV8825::LINEAR_SPEED, config_.accel, config_.decel);
	motor->setWaitFunc([&](unsigned long d) { return wait(std::chrono::microseconds(d)); });
	std::printf("Initialized motor %d with step angle %.1f°\n", second_motor ? 2 : 1, 360.0 / config_.steps);

	// Init controller thread
	stop_thread = false;
	input_thread = new std::thread(&Controller::InputMonitor, this);
	pwm_thread = new std::thread(&Controller::PowerMonitor, this);
	angle_thread = new std::thread(&Controller::AngleMonitor, this);
	zoom_thread = new std::thread(&Controller::Zoom, this, lens->getFocalLength());
	ctrl_thread = new std::thread(&Controller::AutoFocus, this);
}

void Controller::Teardown()
{
	{
		std::lock_guard<std::mutex> lk(stop_mutex);
		stop_thread = true;
	}
	stop_cv.notify_all();
	if (ctrl_thread)
		ctrl_thread->join();
	if (zoom_thread)
		zoom_thread->join();
	if (angle_thread)
		angle_thread->join();
	if (pwm_thread)
		pwm_thread->join();
	if (input_thread)
		input_thread->join();

	delete ctrl_thread;
	delete zoom_thread;
	delete encoder;
	delete pwm_thread;
	delete input_thread;
	delete motor;
	delete sensor;
	delete lens;
}

template <typename Rep, typename Period>
bool Controller::wait(std::chrono::duration<Rep, Period> delay)
{
	std::unique_lock<std::mutex> lk(stop_mutex);
	return !stop_cv.wait_for(lk, delay, [this] { return stop_thread; });
}

bool Controller::limitSwitch(float startAngle)
{
	long steps = std::abs(motor->calcStepsForRotation(current_angle - startAngle));
	bool res = (std::abs(motor->getStepsCompleted() - steps) < max_steps_diff);
	if (!res)
	{
		eprintf("\tLimit switch tripped\n");
	}
	return res;
}

void Controller::AutoFocus()
{
	std::cout << "Autofocus: controller started" << std::endl;

	try
	{
		lens->aperture(aperture_init);
		while (wait(0ns) && lens->isMoving())
			;

		while (WaitFocusChange() && wait(0ns))
		{
			AutoFocusLocal() || (AutoFocusGlobal() && AutoFocusLocal());
		}
	}
	catch (std::exception const &e)
	{
		std::cerr << "ERROR: *** " << e.what() << " ***" << std::endl;
	}

	std::cout << "Autofocus: controller stopped" << std::endl;
}

bool Controller::AutoFocusLocal()
{
	int currentFocusPosition = lens->getFocusPosition();
	double a = SHRT_MIN;
	double b = SHRT_MAX;
	double c = currentFocusPosition;
	double fa = 0;
	double fb = 0;
	double fc = getCurrentFocus();

	std::cout << "Autofocus: hill climbing" << std::endl;

	std::vector<float> values;
	values.push_back(fc);

	std::function<double(double *)> f = [&](double *p) {
		int position = static_cast<int>(*p);
		position -= currentFocusPosition;
		lens->move(position);
		while (wait(0ns) && lens->isMoving())
			;
		currentFocusPosition = lens->getFocusPosition();
		wait(10ms); // Wait for image stabilization
		float f = getCurrentFocus();
		values.push_back(f);
		eprintf("\tPosition %d, focus %0.1f\n", currentFocusPosition, f);
		*p = currentFocusPosition;
		return f;
	};

	std::function<bool()> l = [&]() {
		return wait(0ns);
	};

	eprintf("\tPosition %.0f, focus %0.1f\n", c, fc);

	Max_Search_Hill_Climbing(l, f, &a, &fa, &b, &fb, c, fc, config_.grid_size, config_.tolerance);
	printf("\tLocal maximum is between (%.0f, %.1f) and (%.0f, %.1f)\n", a, fa, b, fb);

	if (fa > fb)
		f(&a);
	else
		f(&b);

	double variance = boost::math::statistics::sample_variance(values);
	double standardDeviation = std::sqrt(variance);
	bool complete = (standardDeviation > config_.refocus_th_1);
	printf("Focus σ (%.1f) %s rf_th1 (%.1f)\n", standardDeviation, complete ? ">" : "<", config_.refocus_th_1);
	return complete;
}

bool Controller::AutoFocusGlobal()
{
	int minFocusPos;
	int maxFocusPos;
	std::vector<float> values;

	std::cout << "Autofocus: global search" << std::endl;

	// Rewind to one side
	lens->backward();
	while (wait(0ns) && lens->isMoving())
		;
	lens->reset();
	minFocusPos = lens->getFocusPosition();
	values.push_back(getCurrentFocus());

	// Start moving to another side
	lens->forward();
	while (wait(0ns) && lens->isMoving())
	{
		// Capture focus while moving
		values.push_back(getCurrentFocus());
	}
	maxFocusPos = lens->getFocusPosition();
	values.push_back(getCurrentFocus());

	double k = double(maxFocusPos - minFocusPos) / values.size();

	// Search maximum focus
	std::function<double(double)> f = [&](double p) {
		float f = values[static_cast<int>(p)];
		p = minFocusPos + p * k;
		eprintf("\tPosition %0.0f, focus %0.1f\n", p, f);
		return f;
	};

	std::function<bool()> l = [&]() {
		return wait(0ns);
	};

	double a = 0;
	double b = values.size();
	double fa = values.front();
	double fb = values.back();
	Max_Search_Global(l, f, &a, &fa, &b, &fb, 1.0 / values.size());
	a = minFocusPos + a * k;
	b = minFocusPos + b * k;
	printf("\tGlobal maximum is between (%.0f, %.1f) and (%.0f, %.1f)\n", a, fa, b, fb);

	// Rewind to the highest focus
	eprintf("\tRewinding to %.0f\n", a);
	lens->move(-maxFocusPos + (int)a);
	while (wait(0ns) && lens->isMoving())
		;

	return true;
}

bool Controller::WaitFocusChange()
{
	if (getCurrentFocus() < 0)
	{
		while (wait(50ms) && getCurrentFocus() < 0)
			;
		return true;
	}

	boost::circular_buffer<float> history(100);
	boost::circular_buffer<float> x(boost::counting_iterator<int>(0),
									boost::counting_iterator<int>(history.capacity()));
	while (wait(25ms))
	{
#ifdef BUILD_OpenCV
		history.push_back(getBlurStdDev());
#else
		history.push_back(getCurrentFocus());
#endif
		if (!history.full())
		{
			continue;
		}

		double mean = boost::math::statistics::mean(history);

#ifdef BUILD_OpenCV
		if (mean < config_.refocus_th_2)
		{
			printf("Mean blur σ (%.1f) < rf_th2 (%.1f).\n", mean, config_.refocus_th_2);
			return true;
		}
#else
		if (mean < config_.refocus_th_4)
		{
			printf("Mean focus (%.1f) < rf_th4 (%.1f).\n", mean, config_.refocus_th_4);
			return true;
		}

		auto [c0, c1] = boost::math::statistics::simple_ordinary_least_squares(x, history);

		if (c1 < config_.refocus_th_3)
		{
			printf("Detected focus descent (%.1f) < rf_th3 (%.1f).\n", c1, config_.refocus_th_3);
			return true;
		}
#endif
	}

	return false;
}

void Controller::Zoom(unsigned int current_focal_length)
{
	long steps_to_move;

	while (wait(50ms) && current_angle == NAN)
		;

	std::function<long(unsigned int)> steps = [&](unsigned int fl) {
		return static_cast<long>((float)max_steps * (float)(fl - focus_len_min) /
								 (float)(focus_len_max - focus_len_min));
	};

	max_steps_diff = motor->calcStepsForRotation(config_.stop_deg_dif);
	if (config_.track_deg_len < 0)
	{
		MeasureTrackLength();
		current_focal_length = focus_len_min;
	}
	max_steps = motor->calcStepsForRotation(config_.track_deg_len);
	current_steps = steps(current_focal_length);

	if (focal_len_init > 0 && focal_len_init != current_focal_length)
	{
		focal_len_init = std::max(focus_len_min, std::min(focal_len_init, focus_len_max));
		steps_to_move = steps(focal_len_init) - current_steps;
		ZoomTo(steps_to_move, []() { return false; });
	}

	do
	{
		steps_to_move = -current_steps;
		if (zoom_direction == 1)
		{
			steps_to_move += max_steps;
		}
		if (zoom_direction != 0)
		{
			ZoomTo(steps_to_move, [&]() { return zoom_direction == 0; });
		}

		std::unique_lock<std::mutex> lk(stop_mutex);
		stop_cv.wait(lk);
	} while (!stop_thread);
}

void Controller::ZoomTo(long steps_to_move, const std::function<bool()> &brake)
{
	angle_poll_ms = 0;
	motor->begin(config_.rpm, config_.micro_step);
	try
	{
		bool brake_started = false;
		bool limit_not_reached = true;
		float angle = current_angle;
		motor->startMove(steps_to_move);
		while (wait(0ns) && motor->nextAction() && limit_not_reached)
		{
			if (!brake_started && brake())
			{
				motor->startBrake();
				brake_started = true;
			}
			limit_not_reached = limitSwitch(angle);
		}
		if (limit_not_reached)
			current_steps += motor->getDirection() * motor->getStepsCompleted();
		else
			current_steps = motor->getDirection() < 0 ? 0 : max_steps;
	}
	catch (std::exception const &e)
	{
		std::cerr << "ERROR: *** " << e.what() << " ***" << std::endl;
	}
	motor->disable();
	angle_poll_ms = 100;
}

void Controller::MeasureTrackLength()
{
	std::cout << "Autofocus: measuring track length" << std::endl;

	float angle_min = std::numeric_limits<float>::min();
	float angle_max = std::numeric_limits<float>::max();

	angle_poll_ms = 0;
	motor->begin(config_.rpm, config_.micro_step);
	try
	{
		for (long steps : { SHRT_MAX, SHRT_MIN })
		{
			float angle = current_angle;
			motor->startMove(steps);
			while (wait(0ns) && motor->nextAction() && limitSwitch(angle))
				;
			angle = current_angle;
			angle_min = std::max(angle_min, angle);
			angle_max = std::fmin(angle_max, angle);
		}
	}
	catch (std::exception const &e)
	{
		std::cerr << "ERROR: *** " << e.what() << " ***" << std::endl;
	}
	motor->disable();
	angle_poll_ms = 100;

	config_.track_deg_len = static_cast<long>(std::abs(angle_max - angle_min));

	printf("\tTrack length: %ld°, write this value in JSON file that configures the post-processing\n",
		   config_.track_deg_len);
}

void Controller::AngleMonitor()
{
	float angle_cur = encoder->getAngle();
	float angle_new;
	float angle_ttl;
	int round = 0;
	while (wait(std::chrono::milliseconds(angle_poll_ms)))
	{
		angle_new = encoder->getAngle();
		if ((angle_cur > 270) && (angle_new < 90))
		{
			round++;
		}
		if ((angle_cur < 90) && (angle_new > 270))
		{
			round--;
		}
		angle_cur = angle_new;
		angle_ttl = 360 * static_cast<float>(round) + angle_cur;
		current_angle = angle_ttl;
	}
}

void Controller::PowerMonitor()
{
	unsigned long max_delay = 300;
	unsigned long delay;
	do
	{
		float bus_voltage = sensor->GetBusVoltage_V(); // voltage on V- (load side)
		float shunt_voltage = sensor->GetShuntVoltage_mV() / 1000; // voltage between V+ and V- across the shunt
		float current = sensor->GetCurrent_mA() / 1000; // current in A
		float power = sensor->GetPower_W(); // power in W
		float p = (bus_voltage - config_.min_voltage) / (config_.max_voltage - config_.min_voltage) * 100.0f;
		p = std::max(0.0f, std::min(p, 100.0f));

		std::printf("\n--------------------------INA219--------------------------\n");
		std::printf("    PSU Voltage: %6.3f V         Current: %9.6f A\n", bus_voltage + shunt_voltage, current);
		std::printf("  Shunt Voltage: %9.6f V        Power: %6.3f W\n", shunt_voltage, power);
		std::printf("   Load Voltage: %6.3f V         Percent: %4.1f%%\n", bus_voltage, p);
		std::printf("----------------------------------------------------------\n\n");

		if (config_.alert_th > p)
		{
			delay = static_cast<long>(fmax(10, static_cast<float>(max_delay) * p / 100.0f));
		}
		else
		{
			delay = max_delay;
		}
	} while (wait(std::chrono::seconds(delay)));
}

void Controller::InputMonitor()
{
	int fd = open(config_.input_dev.c_str(), O_RDONLY);
	if (fd == -1)
	{
		std::cerr << "Cannot open " << config_.input_dev.c_str() << ": " << strerror(errno) << std::endl;
		return;
	}
	struct input_event ev
	{
	};
	pollfd p[1] = { { fd, POLLIN, 0 } };
	while (wait(0ns))
	{
		int res = poll(p, 1, 50);
		if (res <= 0 || !(p[0].revents & POLLIN))
		{
			continue;
		}

		ssize_t n = read(fd, &ev, sizeof ev);
		if (n == -1)
		{
			if (errno == EINTR)
				continue;
			else
				break;
		}
		if (ev.type != EV_KEY || ev.value < 0 || ev.value > 1)
		{
			continue;
		}

		switch (ev.code)
		{
		case KEY_UP:
		case KEY_PAGEUP:
			zoom_direction = ev.value == 1 ? +1 : 0;
			stop_cv.notify_all();
			break;
		case KEY_DOWN:
		case KEY_PAGEDOWN:
			zoom_direction = ev.value == 1 ? -1 : 0;
			stop_cv.notify_all();
			break;
		}
	}
	close(fd);
}


//  Copyright John Maddock 2006.
//  Copyright Paul A. Bristow 2010

//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifdef _MSC_VER
#  pragma warning (disable : 4224)
#endif

#include <pch_light.hpp> // include /libs/math/src/
#include "test_cbrt.hpp"

#include <boost/math/special_functions/cbrt.hpp> // Added to avoid link failure missing cbrt variants.
// Assumes special functions library built rather than included?

//
// DESCRIPTION:
// ~~~~~~~~~~~~
//
// This file tests the function cbrt.  The accuracy tests
// use values generated with NTL::RR at 1000-bit precision
// and our generic versions of these functions.
//
// Note that when this file is first run on a new platform many of
// these tests will fail: the default accuracy is 1 epsilon which
// is too tight for most platforms.  In this situation you will
// need to cast a human eye over the error rates reported and make
// a judgement as to whether they are acceptable.  Either way please
// report the results to the Boost mailing list.  Acceptable rates of
// error are marked up below as a series of regular expressions that
// identify the compiler/stdlib/platform/data-type/test-data/test-function
// along with the maximum expected peek and RMS mean errors for that
// test.
//

void expected_results()
{
   //
   // Define the max and mean errors expected for
   // various compilers and platforms.
   //
   const char* largest_type;
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   if(boost::math::policies::digits<double, boost::math::policies::policy<> >() == boost::math::policies::digits<long double, boost::math::policies::policy<> >())
   {
      largest_type = "(long\\s+)?double|real_concept";
   }
   else
   {
      largest_type = "long double|real_concept";
   }
#else
   largest_type = "(long\\s+)?double|real_concept";
#endif
   add_expected_result(
      "Borland.*",                    // compiler
      ".*",                           // stdlib
      ".*",                           // platform
      "long double",                  // test type(s)
      ".*",                           // test data group
      ".*", 10, 6);                   // test function
   add_expected_result(
      ".*",                          // compiler
      ".*",                          // stdlib
      ".*",                          // platform
      largest_type,                  // test type(s)
      ".*",                          // test data group
      ".*", 2, 2);                   // test function
   //
   // Finish off by printing out the compiler/stdlib/platform names,
   // we do this to make it easier to mark up expected error rates.
   //
   std::cout << "Tests run with " << BOOST_COMPILER << ", "
      << BOOST_STDLIB << ", " << BOOST_PLATFORM << std::endl;
}

BOOST_AUTO_TEST_CASE( test_main )
{
   expected_results();
   BOOST_MATH_CONTROL_FP;
   test_cbrt(0.1F, "float");
   test_cbrt(0.1, "double");
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_cbrt(0.1L, "long double");
#ifndef BOOST_MATH_NO_REAL_CONCEPT_TESTS
   test_cbrt(boost::math::concepts::real_concept(0.1), "real_concept");
#endif
#endif
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file dreyfus_wagner_example.cpp
 * @brief
 * @author Maciej Andrejczuk
 * @version 1.0
 * @date 2013-08-01
 */
    //! [Dreyfus Wagner Example]
#include "test/test_utils/sample_graph.hpp"

#include "paal/steiner_tree/dreyfus_wagner.hpp"

#include <boost/range/algorithm/copy.hpp>

#include <iostream>

int main() {
    // prepare metric
    typedef sample_graphs_metrics SGM;
    auto gm = SGM::get_graph_metric_steiner();

    // prepare terminals and Steiner vertices
    std::vector<int> terminals = { SGM::A, SGM::B, SGM::C, SGM::D };
    std::vector<int> nonterminals = { SGM::E };

    // run algorithm
    auto dw =
        paal::make_dreyfus_wagner(gm, terminals, nonterminals);
    dw.solve();

    // print result
    std::cout << "Cost = " << dw.get_cost() << std::endl;
    std::cout << "Steiner points:" << std::endl;
    boost::copy(dw.get_steiner_elements(),
              std::ostream_iterator<int>(std::cout, "\n"));
    std::cout << "Edges:" << std::endl;
    for (auto edge : dw.get_edges()) {
        std::cout << "(" << edge.first << "," << edge.second << ")"
                  << std::endl;
    }

    return 0;
}
    //! [Dreyfus Wagner Example]


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/combinatorial/include/functions/factorial.hpp>

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

#include <nt2/include/constants/eight.hpp>
#include <nt2/include/constants/eleven.hpp>
#include <nt2/include/constants/five.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/nine.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/seven.hpp>
#include <nt2/include/constants/six.hpp>
#include <nt2/include/constants/ten.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/twelve.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/functions/min.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/saturate.hpp>

NT2_TEST_CASE_TPL ( factorial_real,  NT2_SIMD_REAL_TYPES)
{
  using nt2::factorial;
  using nt2::tag::factorial_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(factorial(nt2::Inf<vT>()), nt2::Inf<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Nan<vT>()), nt2::Nan<vT>());
#endif
  NT2_TEST_EQUAL(factorial(nt2::Eight<vT>()), nt2::min((nt2::splat<vT>(40320ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Eleven<vT>()), nt2::min((nt2::splat<vT>(39916800ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Five<vT>()), nt2::splat<vT>(120));
  NT2_TEST_EQUAL(factorial(nt2::Four<vT>()), nt2::splat<vT>(24));
  NT2_TEST_EQUAL(factorial(nt2::Nine<vT>()), nt2::min((nt2::splat<vT>(362880ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::One<vT>()), nt2::One<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Seven<vT>()), nt2::min((nt2::splat<vT>(5040ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Six<vT>()), nt2::min((nt2::splat<vT>(720ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Ten<vT>()), nt2::min((nt2::splat<vT>(3628800ll  )),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Three<vT>()), nt2::Six<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Twelve<vT>()), nt2::min((nt2::splat<vT>(479001600ll)),nt2::Valmax<vT>()));
  NT2_TEST_EQUAL(factorial(nt2::Two<vT>()), nt2::Two<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Zero<vT>()), nt2::One<vT>());
}
NT2_TEST_CASE_TPL ( factorial_int,  NT2_SIMD_INTEGRAL_TYPES)
{
  using nt2::factorial;
  using nt2::tag::factorial_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // specific values tests
  NT2_TEST_EQUAL(factorial(nt2::Eight<vT>()),nt2::splat<vT>(nt2::saturate<vT>(40320ull)));
  NT2_TEST_EQUAL(factorial(nt2::Eleven<vT>()),nt2::splat<vT>(nt2::saturate<vT>(39916800ull)));
  NT2_TEST_EQUAL(factorial(nt2::Five<vT>()),nt2::splat<vT>(nt2::saturate<vT>(120)));
  NT2_TEST_EQUAL(factorial(nt2::Four<vT>()),nt2::splat<vT>(nt2::saturate<vT>(24)));
  NT2_TEST_EQUAL(factorial(nt2::Nine<vT>()),nt2::splat<vT>(nt2::saturate<vT>(362880ull)));
  NT2_TEST_EQUAL(factorial(nt2::One<vT>()),nt2::One<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Seven<vT>()),nt2::splat<vT>(nt2::saturate<vT>(5040ull)));
  NT2_TEST_EQUAL(factorial(nt2::Six<vT>()),nt2::splat<vT>(nt2::saturate<vT>(720ull)));
  NT2_TEST_EQUAL(factorial(nt2::Ten<vT>()),nt2::splat<vT>(nt2::saturate<vT>(3628800ull)));
  NT2_TEST_EQUAL(factorial(nt2::Three<vT>()),nt2::Six<vT>());
  NT2_TEST_EQUAL(factorial(nt2::Twelve<vT>()),nt2::splat<vT>(nt2::saturate<vT>(479001600ull)));
  NT2_TEST_EQUAL(factorial(nt2::Zero<vT>()),nt2::One<vT>());
}


/**
 * Copyright 2020, Massachusetts Institute of Technology,
 * Cambridge, MA 02139
 * All Rights Reserved
 * Authors: Jingnan Shi, et al. (see THANKS for the full author list)
 * See LICENSE for the license information
 */

#include <map>
#include <iostream>
#include <chrono>

#include "mex.h"
#include <Eigen/Core>

#include "teaser_mex_utils.h"
#include "teaser/registration.h"

enum class INPUT_PARAMS : int {
  src = 0,
  dst = 1,
  cbar2 = 2,
  noise_bound = 3,
  estimate_scaling = 4,
  rotation_estimation_algorithm = 5,
  rotation_gnc_factor = 6,
  rotation_max_iterations = 7,
  rotation_cost_threshold = 8,
};

enum class OUTPUT_PARAMS : int {
  s_est = 0,
  R_est = 1,
  t_est = 2,
  time_taken = 3,
};

typedef bool (*mexTypeCheckFunction)(const mxArray*);
const std::map<INPUT_PARAMS, mexTypeCheckFunction> INPUT_PARMS_MAP{
    {INPUT_PARAMS::src, &isPointCloudMatrix},
    {INPUT_PARAMS::dst, &isPointCloudMatrix},
    {INPUT_PARAMS::cbar2, &isRealDoubleScalar},
    {INPUT_PARAMS::noise_bound, &isRealDoubleScalar},
    {INPUT_PARAMS::estimate_scaling, &mxIsLogicalScalar},
    {INPUT_PARAMS::rotation_estimation_algorithm, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_gnc_factor, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_max_iterations, &isRealDoubleScalar},
    {INPUT_PARAMS::rotation_cost_threshold, &isRealDoubleScalar},
};
const std::map<OUTPUT_PARAMS, mexTypeCheckFunction> OUTPUT_PARMS_MAP{
    {OUTPUT_PARAMS::s_est, &isRealDoubleScalar},
    {OUTPUT_PARAMS::R_est, &isRealDoubleMatrix<3, 3>},
    {OUTPUT_PARAMS::t_est, &isRealDoubleMatrix<3, 1>},
    {OUTPUT_PARAMS::time_taken, &isRealDoubleScalar},
};

/**
 * This is the MATLAB binding for TEASER++.
 *
 * Input:
 * - src: a 3-by-N matrix of 3D points representing points to be transformed
 * - dst: a 3-by-N matrix of 3D points representing points after transformation
 * - cbar2: square of maximum allowed ratio between noise and noise bound (see [1]).
 * - noise_bound: a floating-point number indicating the bound on noise
 * - estimate_scaling: a boolean indicating whether scale needs to be estimated
 * - rotation_max_iterations: maximum iterations for the rotation estimation loop
 * - rotation_cost_threshold: cost threshold for rotation termination
 * - rotation_gnc_factor: gnc factor for rotation estimation
 *                        for GNC-TLS method: it's multiplied on the GNC control parameter
 *                        for FGR method: it's divided on the GNC control parameter
 * - rotation_estimation_algorithm: a number indicating the rotation estimation method used;
 *                                  if it's 0: GNC-TLS
 *                                  if it's 1: FGR
 *
 * Output:
 * - s_est estimated scale (scalar)
 * - R_est estimated rotation matrix (3-by-3 matrix)
 * - t_est estimated translation vector (3-by-1 matrix)
 * - time_taken time it takes for the underlying TEASER++ library to compute a solution.
 *
 * [1] H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration,”
 * arXiv:2001.07715 [cs, math], Jan. 2020.
 *
 */
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {

  // Check for proper number of arguments
  if (nrhs != INPUT_PARMS_MAP.size()) {
    mexErrMsgIdAndTxt("teaserSolve:nargin", "Wrong number of input arguments.");
  }
  if (nlhs != OUTPUT_PARMS_MAP.size()) {
    mexErrMsgIdAndTxt("teaserSolve:nargin", "Wrong number of output arguments.");
  }

  // Check for proper input types
  for (const auto& pair : INPUT_PARMS_MAP) {
    if (!pair.second(prhs[toUType(pair.first)])) {
      std::stringstream error_msg;
      error_msg << "Argument " << toUType(pair.first) + 1 << " has the wrong type.\n";
      mexErrMsgIdAndTxt("teaserSolve:nargin", error_msg.str().c_str());
    }
  }

  mexPrintf("Arguments type checks passed.\n");
  mexEvalString("drawnow;");

  // Prepare parameters
  // Prepare source and destination Eigen point matrices
  Eigen::Matrix<double, 3, Eigen::Dynamic> src_eigen, dst_eigen;
  mexPointMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::src)], &src_eigen);
  mexPointMatrixToEigenMatrix(prhs[toUType(INPUT_PARAMS::dst)], &dst_eigen);

  // Other parameters
  auto cbar2 = static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::cbar2)]));
  auto noise_bound = static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::noise_bound)]));
  auto estimate_scaling =
      static_cast<bool>(*mxGetPr(prhs[toUType(INPUT_PARAMS::estimate_scaling)]));
  auto rotation_estimation_method =
      static_cast<int>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_estimation_algorithm)]));
  auto rotation_gnc_factor =
      static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_gnc_factor)]));
  auto rotation_max_iterations =
      static_cast<size_t>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_max_iterations)]));
  auto rotation_cost_threshold =
      static_cast<double>(*mxGetPr(prhs[toUType(INPUT_PARAMS::rotation_cost_threshold)]));

  // Prepare the TEASER++ solver for solving registration problem
  teaser::RobustRegistrationSolver::Params params;
  params.noise_bound = noise_bound;
  params.cbar2 = cbar2;
  params.estimate_scaling = estimate_scaling;
  params.rotation_max_iterations = rotation_max_iterations;
  params.rotation_gnc_factor = rotation_gnc_factor;
  params.rotation_cost_threshold = rotation_cost_threshold;

  switch (rotation_estimation_method) {
  case 0: { // GNC-TLS method
    mexPrintf("Use GNC-TLS for rotation estimation.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
    break;
  }
  case 1: { // FGR method
    mexPrintf("Use FGR for rotation estimation.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::FGR;
    break;
  }
  default: {
    mexPrintf("Rotation estimation method given does not exist. Use GNC-TLS instead.\n");
    params.rotation_estimation_algorithm =
        teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS;
    break;
  }
  }
  teaser::RobustRegistrationSolver solver(params);

  mexPrintf("Start TEASER++ solver.\n");
  mexEvalString("drawnow;");

  // Start the timer
  auto start = std::chrono::high_resolution_clock::now();

  // Solve
  assert(src_eigen.size() != 0);
  assert(dst_eigen.size() != 0);
  solver.solve(src_eigen, dst_eigen);

  // Stop the timer
  auto stop = std::chrono::high_resolution_clock::now();
  auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
  double duration_in_milliseconds = static_cast<double>(duration.count()) / 1000.0;

  auto solution = solver.getSolution();

  mexPrintf("TEASER++ has found a solution in %f milliseconds.\n", duration_in_milliseconds);
  mexEvalString("drawnow;");

  // Populate outputs
  plhs[toUType(OUTPUT_PARAMS::s_est)] = mxCreateDoubleScalar(solution.scale);
  // Populate output R matrix
  plhs[toUType(OUTPUT_PARAMS::R_est)] = mxCreateDoubleMatrix(3, 3, mxREAL);
  Eigen::Map<Eigen::Matrix3d> R_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::R_est)]), 3, 3);
  R_map = solution.rotation;

  // Populate output T vector
  plhs[toUType(OUTPUT_PARAMS::t_est)] = mxCreateDoubleMatrix(3, 1, mxREAL);
  Eigen::Map<Eigen::Matrix<double, 3, 1>> t_map(mxGetPr(plhs[toUType(OUTPUT_PARAMS::t_est)]), 3, 1);
  t_map = solution.translation;

  // Populate time output
  plhs[toUType(OUTPUT_PARAMS::time_taken)] = mxCreateDoubleScalar(duration_in_milliseconds);
}


//
// Copyright (c) 2015-2017, Deutsches Forschungszentrum für Künstliche Intelligenz GmbH.
// Copyright (c) 2015-2017, University of Bremen
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice, this
//   list of conditions and the following disclaimer.
//
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
#define BOOST_TEST_MODULE ToolsTest
#include <boost/test/unit_test.hpp>

#include <maps/grid/MLSMap.hpp>
#include <maps/grid/TraversabilityGrid.hpp>
#include <maps/grid/GridMap.hpp>
#include <maps/grid/Index.hpp>

#include <maps/tools/MLSToSlopes.hpp>

using namespace maps;
using namespace grid;
using namespace tools;

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_flatPlane)
{
    const Vector2ui numCells(20, 20);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z = 5;
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), 0, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), 0, 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_45)
{
    const Vector2ui numCells(20, 20);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z = y * resolution[0];
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), M_PI/4, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), resolution[0], 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_Plane_withStdDev)
{
    const Vector2ui numCells(20, 20);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z = 5;
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps, true);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), 0, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), 2 * stddev, 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_45_withStdDev)
{
    const Vector2ui numCells(20, 30);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z = y * resolution[0];
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps, true);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), M_PI/4, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), resolution[0] + 2 * stddev, 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_Plane_windowSizeLimit)
{
    const Vector2ui numCells(5, 5);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z = 5;
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes, 5);
    MLSToSlopes::computeMaxSteps(mls, maxSteps, true);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), 0, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), 2 * stddev, 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_Plane_withStdDev_holesInMLS)
{
    const Vector2ui numCells(40, 40);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            if (x >= 10 && x <= 20 && y >= 15 && y <= 25)
                continue;
            float z = 5;
            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps, true);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else if (x >= 10 && x <= 20 && y >= 15 && y <= 25)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), 0, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), 2 * stddev, 0.0001);
            }
        }
    }
}

BOOST_AUTO_TEST_CASE(test_MLSToSlopes_45_half_and_half)
{
    const Vector2ui numCells(40, 40);
    const Vector2d resolution(0.01, 0.01);
    MLSMapKalman mls = MLSMapKalman();
    mls.resize(numCells);
    mls.setResolution(resolution);
    float stddev = 0.1;
    float variance = stddev * stddev;

    for (uint x = 0; x < numCells[0] ; ++x)
    {
        for (uint y = 0; y < numCells[1]; ++y)
        {
            float z;
            if (y >= numCells[1] / 2)
                z = ((numCells[1] / 2) - y + numCells[1] / 2) * resolution[0];
            else z = y * resolution[0];

            MLSMapKalman::Patch newPatch = MLSMapKalman::Patch(z, variance);
            mls.mergePatch(grid::Index(x, y), newPatch);
        }
    }

    GridMapF slopes = GridMapF();
    GridMapF maxSteps = GridMapF();

    MLSToSlopes::computeSlopes(mls, slopes);
    MLSToSlopes::computeMaxSteps(mls, maxSteps, true);

    for (size_t y = 0; y < numCells[1]; ++y)
    {
        for (size_t x = 0; x < numCells[0]; ++x)
        {
            if (x <= 0 || x >= numCells[0] - 1 || y <= 0 || y >= numCells[1] - 1)
            {
                BOOST_CHECK_EQUAL(slopes.at(Index(x, y)), -std::numeric_limits<double>::infinity());
                BOOST_CHECK_EQUAL(maxSteps.at(Index(x, y)), -std::numeric_limits<double>::infinity());
            }
            else if (y == numCells[1] / 2)
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), 0, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), resolution[0] + 2 * stddev, 0.0001);
            }
            else
            {
                BOOST_CHECK_CLOSE(slopes.at(Index(x, y)), M_PI/4, 0.0001);
                BOOST_CHECK_CLOSE(maxSteps.at(Index(x, y)), resolution[0] + 2 * stddev, 0.0001);
            }
        }
    }
}


#include <iostream>
#include <fstream>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
using namespace std;
using namespace boost;

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

 typedef adjacency_list<setS, vecS, undirectedS> graph_type;
 int fromNode;
 int toNode;
 ifstream inputFile;
 inputFile.open(argv[1]);
 ofstream outputFile;
 outputFile.open(argv[2]);
 char trashBuffer[256];
 const int MAX_NODES = 325729;

// Build graph
 graph_type graph(MAX_NODES);

 inputFile.getline(trashBuffer, 256);
 inputFile.getline(trashBuffer, 256);
 inputFile.getline(trashBuffer, 256);
 inputFile.getline(trashBuffer, 256);

 while(inputFile >> fromNode >> toNode)
 {
  add_edge(fromNode,  toNode, graph);
 }

 write_graphviz(outputFile, graph);
 inputFile.close();
 outputFile.close();

 return 0;
}


#ifndef STAN_MATH_TEST_FIXTURE_ODE_SHO_HPP
#define STAN_MATH_TEST_FIXTURE_ODE_SHO_HPP

#include <stan/math/rev.hpp>
#include <boost/numeric/odeint.hpp>
#include <gtest/gtest.h>
#include <test/unit/math/prim/functor/harmonic_oscillator.hpp>
#include <test/unit/math/rev/functor/coupled_mm.hpp>
#include <test/unit/math/rev/functor/test_fixture_ode.hpp>
#include <test/unit/util.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <limits>
#include <string>

template <typename T>
struct harmonic_oscillator_ode_base {
  struct sho_square_fun {
    template <typename T0, typename T1, typename T2, typename T3, typename T4>
    inline Eigen::Matrix<stan::return_type_t<T1, T2>, -1, 1> operator()(
        const T0& t_in, const T1& y_in, std::ostream* msgs, const T2& theta,
        const T3& x, const T4& x_int) const {
      if (y_in.size() != 2)
        throw std::domain_error("Functor called with inconsistent state");

      Eigen::Matrix<stan::return_type_t<T1, T2>, -1, 1> f(2);
      f << (y_in(1)), (-theta.at(0) * theta.at(0) * y_in(0));

      return f;
    }
  };

  harm_osc_ode_fun f;
  harm_osc_ode_fun_eigen f_eigen;
  harm_osc_ode_data_fun f_data;
  harm_osc_ode_data_fun_eigen f_data_eigen;
  harm_osc_ode_wrong_size_1_fun f_wrong_size_1;
  harm_osc_ode_wrong_size_2_fun f_wrong_size_2;
  sho_square_fun f_square;

  using T_t = std::tuple_element_t<2, T>;
  using T_y = std::tuple_element_t<3, T>;
  using T_p = std::tuple_element_t<4, T>;

  std::vector<T_p> theta;
  Eigen::Matrix<T_y, -1, 1> y0;
  double t0;
  std::vector<T_t> ts;
  std::vector<double> x_r;
  std::vector<int> x_i;
  double rtol;
  double atol;
  int max_num_step;

  harmonic_oscillator_ode_base()
      : theta{0.15},
        y0(2),
        t0(0),
        ts(100),
        rtol(1.e-6),
        atol(1.e-8),
        max_num_step(100000) {
    y0 << 1.0, 0.0;
    for (size_t i = 0; i < ts.size(); ++i) {
      ts[i] = t0 + 0.1 * (i + 1);
    }
  }

  std::vector<T_t>& times() { return ts; }
  Eigen::VectorXd init() { return y0; }
  std::vector<double> param() { return theta; }
};

/**
 * Inheriting base type, various fixtures differs by the type of ODE
 * functor used in apply_solver calls, intended for
 * different kind of tests.
 *
 */
template <typename T>
struct harmonic_oscillator_test
    : public harmonic_oscillator_ode_base<T>,
      public ODETestFixture<harmonic_oscillator_test<T>> {
  harmonic_oscillator_test() : harmonic_oscillator_ode_base<T>() {}

  auto apply_solver() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, this->y0, this->t0, this->ts, nullptr,
               this->theta, this->x_r, this->x_i);
  }

  template <typename T1, typename T2>
  auto apply_solver(T1&& init, T2&& theta_in) {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, init, this->t0, this->ts, nullptr, theta_in,
               this->x_r, this->x_i);
  }

  auto apply_solver_tol() {
    std::tuple_element_t<1, T> sol;
    return sol(this->f_eigen, this->y0, this->t0, this->ts, this->rtol,
               this->atol, this->max_num_step, nullptr, this->theta, this->x_r,
               this->x_i);
  }

  void test_bad() {
    const auto y0_(this->y0);

    this->y0.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::invalid_argument,
                     "initial state has size 0");
    this->y0 = y0_;

    const auto t0_ = this->t0;
    this->t0 = 2.0;
    EXPECT_THROW_MSG(apply_solver(), std::domain_error,
                     "initial time is 2, but must be less than 0.1");
    this->t0 = t0_;

    const auto ts_ = this->ts;
    this->ts.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::invalid_argument, "times has size 0");
    this->ts = ts_;

    this->ts.resize(2);
    this->ts[0] = 3.0;
    this->ts[1] = 1.0;
    EXPECT_THROW_MSG(apply_solver(), std::domain_error,
                     "times is not a valid sorted vector");
    this->ts = ts_;

    const double rtol_ = this->rtol;
    this->rtol = -1;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "relative_tolerance");
    this->rtol = rtol_;

    const double atol_ = this->atol;
    this->atol = -1;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "absolute_tolerance");
    this->atol = atol_;

    const auto theta_ = this->theta;
    const auto x_r_ = this->x_r;
    const auto x_i_ = this->x_i;

    // NaN errors
    double nan = std::numeric_limits<double>::quiet_NaN();
    std::stringstream expected_is_nan;
    expected_is_nan << "is " << nan;

    this->y0[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->y0 = y0_;

    this->t0 = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->t0 = t0_;

    this->ts[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->ts = ts_;

    this->theta[0] = nan;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->theta = theta_;

    this->x_r.push_back(nan);
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_nan.str());
    this->x_r = x_r_;

    // inf test
    std::stringstream expected_is_inf;
    expected_is_inf << "is " << std::numeric_limits<double>::infinity();
    std::stringstream expected_is_neg_inf;
    expected_is_neg_inf << "is " << -std::numeric_limits<double>::infinity();
    double inf = std::numeric_limits<double>::infinity();

    this->y0[0] = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->y0[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial state");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->y0 = y0_;

    this->t0 = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->t0 = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "initial time");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->t0 = t0_;

    this->ts.back() = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->ts.back() = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "times");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->ts = ts_;

    this->theta[0] = inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->theta[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->theta = theta_;

    this->x_r = std::vector<double>{inf};
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_inf.str());
    this->x_r[0] = -inf;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     "ode parameters and data");
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error,
                     expected_is_neg_inf.str());
    this->x_r = x_r_;
  }

  void test_value(double t0_in) {
    this->t0 = t0_in;
    for (size_t i = 0; i < this->ts.size(); ++i) {
      this->ts[i] = this->t0 + 0.1 * (i + 1);
    }

    this->rtol = 1e-8;
    this->atol = 1e-10;
    this->max_num_step = 1e6;
    auto res = apply_solver_tol();

    EXPECT_NEAR(0.995029, stan::math::value_of(res[0][0]), 1e-5);
    EXPECT_NEAR(-0.0990884, stan::math::value_of(res[0][1]), 1e-5);

    EXPECT_NEAR(-0.421907, stan::math::value_of(res[99][0]), 1e-5);
    EXPECT_NEAR(0.246407, stan::math::value_of(res[99][1]), 1e-5);
  }
};

template <typename T>
struct harmonic_oscillator_t0_ad_test
    : public harmonic_oscillator_ode_base<T>,
      public ODETestFixture<harmonic_oscillator_t0_ad_test<T>> {
  stan::math::var t0v;

  harmonic_oscillator_t0_ad_test()
      : harmonic_oscillator_ode_base<T>(), t0v(stan::math::to_var(this->t0)) {
    this->ts = std::vector<double>{5.0, 10.0};
  }

  auto apply_solver() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_eigen, this->y0, t0v, this->ts, nullptr, this->theta,
               this->x_r, this->x_i);
  }

  void test_t0_ad(double tol) {
    stan::math::nested_rev_autodiff nested;
    auto res = apply_solver();
    res[0][0].grad();
    EXPECT_NEAR(t0v.adj(), -0.66360742442816977871, tol);
    nested.set_zero_all_adjoints();
    t0v.adj() = 0.0;
    res[0][1].grad();
    EXPECT_NEAR(t0v.adj(), 0.23542843380353062344, tol);
    nested.set_zero_all_adjoints();
    t0v.adj() = 0.0;
    res[1][0].grad();
    EXPECT_NEAR(t0v.adj(), -0.2464078910913158893, tol);
    nested.set_zero_all_adjoints();
    t0v.adj() = 0.0;
    res[1][1].grad();
    EXPECT_NEAR(t0v.adj(), -0.38494826636037426937, tol);
    nested.set_zero_all_adjoints();
    t0v.adj() = 0.0;
  }
};

/**
 * Bad ODE RHS type tests.
 *
 */
template <typename T>
struct harmonic_oscillator_bad_ode_test
    : public harmonic_oscillator_ode_base<T>,
      public ODETestFixture<harmonic_oscillator_bad_ode_test<T>> {
  harmonic_oscillator_bad_ode_test() : harmonic_oscillator_ode_base<T>() {}

  auto apply_solver() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_wrong_size_1, stan::math::to_array_1d(this->y0),
               this->t0, this->ts, this->theta, this->x_r, this->x_i, 0);
  }

  auto apply_solver_tol() {
    std::tuple_element_t<1, T> sol;
    return sol(this->f_wrong_size_1, stan::math::to_array_1d(this->y0),
               this->t0, this->ts, this->theta, this->x_r, this->x_i, 0,
               this->rtol, this->atol, this->max_num_step);
  }

  void test_bad_ode() {
    std::string error_msg = "dy_dt (3) and states (2) must match in size";
    EXPECT_THROW_MSG(apply_solver_tol(), std::invalid_argument, error_msg);
  }
};

/**
 * ODE RHS type that utilize double & int data
 *
 */
template <typename T>
struct harmonic_oscillator_data_test
    : public harmonic_oscillator_ode_base<T>,
      public ODETestFixture<harmonic_oscillator_data_test<T>> {
  harmonic_oscillator_data_test() : harmonic_oscillator_ode_base<T>() {
    this->x_r = std::vector<double>(3, 1);
    this->x_i = std::vector<int>(2, 0);
  }

  auto apply_solver() {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_data_eigen, this->y0, this->t0, this->ts, 0, this->theta,
               this->x_r, this->x_i);
  }

  template <typename T1, typename T2>
  auto apply_solver(T1&& init, T2&& theta_in) {
    std::tuple_element_t<0, T> sol;
    return sol(this->f_data_eigen, init, this->t0, this->ts, nullptr, theta_in,
               this->x_r, this->x_i);
  }

  auto apply_solver_tol() {
    std::tuple_element_t<1, T> sol;
    return sol(this->f_data_eigen, this->y0, this->t0, this->ts, this->rtol,
               this->atol, this->max_num_step, 0, this->theta, this->x_r,
               this->x_i);
  }

  void test_bad_param_and_data() {
    const auto theta_ = this->theta;
    this->theta.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::out_of_range, "vector");
    this->theta = theta_;

    const auto x_r_ = this->x_r;
    this->x_r.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::out_of_range, "vector");
    this->x_r = x_r_;

    const auto x_i_ = this->x_i;
    this->x_i.resize(0);
    EXPECT_THROW_MSG(apply_solver(), std::out_of_range, "vector");
    this->x_i = x_i_;

    this->max_num_step = -1;
    EXPECT_THROW_MSG(apply_solver_tol(), std::domain_error, "max_num_steps");
  }

  void test_value(double t0_in) {
    this->t0 = t0_in;
    for (size_t i = 0; i < this->ts.size(); ++i) {
      this->ts[i] = this->t0 + 0.1 * (i + 1);
    }

    this->rtol = 1e-8;
    this->atol = 1e-10;
    this->max_num_step = 1e6;
    auto res = apply_solver_tol();

    EXPECT_NEAR(0.995029, stan::math::value_of(res[0][0]), 1e-5);
    EXPECT_NEAR(-0.0990884, stan::math::value_of(res[0][1]), 1e-5);

    EXPECT_NEAR(-0.421907, stan::math::value_of(res[99][0]), 1e-5);
    EXPECT_NEAR(0.246407, stan::math::value_of(res[99][1]), 1e-5);
  }
};

template <typename T>
struct harmonic_oscillator_dv_functor : public harmonic_oscillator_ode_base<T> {
  harmonic_oscillator_dv_functor() : harmonic_oscillator_ode_base<T>() {}

  template <typename Tx>
  Eigen::Matrix<Tx, -1, 1> operator()(Eigen::Matrix<Tx, -1, 1>& x) const {
    std::tuple_element_t<0, T> sol;
    std::vector<Tx> theta_tx{x(0)};
    auto ys = sol(this->f_square, this->y0, this->t0, this->ts, nullptr,
                  theta_tx, this->x_r, this->x_i);
    return ys[0];
  }
};

template <typename T>
struct harmonic_oscillator_vd_functor : public harmonic_oscillator_ode_base<T> {
  harmonic_oscillator_vd_functor() : harmonic_oscillator_ode_base<T>() {}

  template <typename Tx>
  Eigen::Matrix<Tx, -1, 1> operator()(Eigen::Matrix<Tx, -1, 1>& x) const {
    std::tuple_element_t<0, T> sol;
    Eigen::Matrix<Tx, -1, 1> y0_tx(2);
    y0_tx << x(0), this->y0(1);
    auto ys = sol(this->f_square, y0_tx, this->t0, this->ts, nullptr,
                  this->theta, this->x_r, this->x_i);
    return ys[0];
  }
};

template <typename T>
struct harmonic_oscillator_vv_functor : public harmonic_oscillator_ode_base<T> {
  harmonic_oscillator_vv_functor() : harmonic_oscillator_ode_base<T>() {}

  template <typename Tx>
  Eigen::Matrix<Tx, -1, 1> operator()(Eigen::Matrix<Tx, -1, 1>& x) const {
    std::tuple_element_t<0, T> sol;
    std::vector<Tx> theta_tx{x(0)};
    Eigen::Matrix<Tx, -1, 1> y0_tx(2);
    y0_tx << x(1), this->y0(1);
    auto ys = sol(this->f_square, y0_tx, this->t0, this->ts, nullptr, theta_tx,
                  this->x_r, this->x_i);
    return ys[0];
  }
};

template <typename T>
struct harmonic_oscillator_analytical_test
    : public ODETestFixture<harmonic_oscillator_analytical_test<T>> {
  harmonic_oscillator_dv_functor<T> ode_sol_dv;
  harmonic_oscillator_vd_functor<T> ode_sol_vd;
  harmonic_oscillator_vv_functor<T> ode_sol_vv;

  auto analy_sol_functor() {
    auto f = [](double t, double omega, double chi) {
      Eigen::VectorXd y(2);
      y << chi * cos(omega * t), -omega * chi * sin(omega * t);
      return y;
    };
    return f;
  }

  auto analy_grad_sol_functor() {
    auto f = [](double t, double omega, double chi) {
      Eigen::MatrixXd dy(2, 2);
      dy(0, 0) = -t * chi * sin(omega * t);
      dy(1, 0) = cos(omega * t);
      dy(0, 1) = -chi * (sin(omega * t) + omega * t * cos(omega * t));
      dy(1, 1) = -omega * sin(omega * t);
      return dy;
    };
    return f;
  }

  auto analy_grad_omega_sol_functor() {
    auto f = [this](double t, double omega, double chi) {
      Eigen::MatrixXd dy = analy_grad_sol_functor()(t, omega, chi);
      Eigen::Matrix<double, -1, -1> d(1, 2);
      d(0, 0) = dy(0, 0);
      d(0, 1) = dy(0, 1);
      return d;
    };
    return f;
  }

  auto analy_grad_chi_sol_functor() {
    auto f = [this](double t, double omega, double chi) {
      Eigen::MatrixXd dy = analy_grad_sol_functor()(t, omega, chi);
      Eigen::Matrix<double, -1, -1> d(1, 2);
      d(0, 0) = dy(1, 0);
      d(0, 1) = dy(1, 1);
      return d;
    };
    return f;
  }
};

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(Deserializer)

    struct AbsFixture : public ParserFlatbuffersSerializeFixture
    {
        explicit AbsFixture(const std::string &inputShape,
                             const std::string &outputShape,
                             const std::string &dataType)
        {
            m_JsonString = R"(
            {
                inputIds: [0],
                outputIds: [2],
                layers: [
                    {
                        layer_type: "InputLayer",
                        layer: {
                            base: {
                                layerBindingId: 0,
                                base: {
                                    index: 0,
                                    layerName: "InputLayer",
                                    layerType: "Input",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [{
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + inputShape + R"(,
                                            dataType: )" + dataType + R"(
                                        }
                                    }]
                                }
                            }
                        }
                    },
                    {
                        layer_type: "AbsLayer",
                        layer: {
                            base: {
                                index: 1,
                                layerName: "AbsLayer",
                                layerType: "Abs",
                                inputSlots: [{
                                    index: 0,
                                    connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                }],
                                outputSlots: [{
                                    index: 0,
                                    tensorInfo: {
                                        dimensions: )" + outputShape + R"(,
                                        dataType: )" + dataType + R"(
                                    }
                                }]
                            }

                        }
                    },
                    {
                        layer_type: "OutputLayer",
                        layer: {
                            base:{
                                layerBindingId: 2,
                                base: {
                                    index: 2,
                                    layerName: "OutputLayer",
                                    layerType: "Output",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [{
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + outputShape + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                    }],
                                }
                            }
                        },
                    }
                ]
            }
        )";
            Setup();
        }
    };

    struct SimpleAbsFixture : AbsFixture
    {
        SimpleAbsFixture()
                : AbsFixture("[ 1, 2, 2, 2 ]",     // inputShape
                              "[ 1, 2, 2, 2 ]",     // outputShape
                              "Float32")            // dataType
        {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleAbsTest, SimpleAbsFixture)
    {
        RunTest<4, armnn::DataType::Float32>(
                0,
                {{"InputLayer",  { -100.0f, -50.5f, -25.9999f, -0.5f , 0.0f, 1.5555f, 25.5f, 100.0f }}},
                {{"OutputLayer", { 100.0f, 50.0f, 25.9999f, 0.5f , 0.0f, 1.5555f, 25.5f, 100.0f }}});
    }

BOOST_AUTO_TEST_SUITE_END()


//  io_ex1.cpp  ----------------------------------------------------------//

//  Copyright 2010 Howard Hinnant
//  Copyright 2010 Vicente J. Botet Escriba
//  Copyright (c) Microsoft Corporation 2014

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was adapted by Vicente J. Botet Escriba from Hinnant's html documentation.
Many thanks to Howard for making his code available under the Boost license.

*/

#include <iostream>
#include <boost/chrono/config.hpp>
#include <boost/chrono/chrono_io.hpp>
#include <boost/chrono/system_clocks.hpp>
#include <boost/chrono/thread_clock.hpp>
#include <boost/chrono/process_cpu_clocks.hpp>

int main()
{
    using std::cout;
    using namespace boost;
    using namespace boost::chrono;

    cout << "milliseconds(1)  = "
         <<  milliseconds(1)  << '\n';
    cout << "milliseconds(3) + microseconds(10) = "
         <<  milliseconds(3) + microseconds(10) << '\n';

    cout << "hours(3) + minutes(10) = "
         <<  hours(3) + minutes(10) << '\n';

    typedef duration<long long, ratio<1, 2500000000ULL> > ClockTick;
    cout << "ClockTick(3) + nanoseconds(10) = "
         <<  ClockTick(3) + nanoseconds(10) << '\n';

    cout << "\nSet cout to use short names:\n";
#if BOOST_CHRONO_VERSION==2
    cout << duration_fmt(duration_style::symbol);
#else
    cout << duration_short;
#endif
    cout << "milliseconds(3) + microseconds(10) = "
         <<  milliseconds(3) + microseconds(10) << '\n';

    cout << "hours(3) + minutes(10) = "
         <<  hours(3) + minutes(10) << '\n';

    cout << "ClockTick(3) + nanoseconds(10) = "
         <<  ClockTick(3) + nanoseconds(10) << '\n';

    cout << "\nsystem_clock::now() = " << system_clock::now() << '\n';
#if defined _MSC_VER && _MSC_VER == 1700
#else
#if BOOST_CHRONO_VERSION==2
    cout << "\nsystem_clock::now() = " << time_fmt(chrono::timezone::local) << system_clock::now() << '\n';
    cout << "\nsystem_clock::now() = " << time_fmt(chrono::timezone::local,"%Y/%m/%d") << system_clock::now() << '\n';
#endif
#endif

#ifdef BOOST_CHRONO_HAS_CLOCK_STEADY
    cout << "steady_clock::now() = " << steady_clock::now() << '\n';
#endif
#if BOOST_CHRONO_VERSION==2
    cout << "\nSet cout to use long names:\n" << duration_fmt(duration_style::prefix)
         << "high_resolution_clock::now() = " << high_resolution_clock::now() << '\n';
#else
    cout << "\nSet cout to use long names:\n" <<  duration_long
         << "high_resolution_clock::now() = " << high_resolution_clock::now() << '\n';
#endif
#if defined(BOOST_CHRONO_HAS_THREAD_CLOCK)
    cout << "\nthread_clock::now() = " << thread_clock::now() << '\n';
#endif
#if defined(BOOST_CHRONO_HAS_PROCESS_CLOCKS)
    cout << "\nprocess_real_cpu_clock::now() = " << process_real_cpu_clock::now() << '\n';
#if BOOST_PLAT_WINDOWS_DESKTOP
    cout << "\nprocess_user_cpu_clock::now() = " << process_user_cpu_clock::now() << '\n';
    cout << "\nprocess_system_cpu_clock::now() = " << process_system_cpu_clock::now() << '\n';
    cout << "\nprocess_cpu_clock::now() = " << process_cpu_clock::now() << '\n';
#endif
#endif
    return 0;
}


#include <boost/atomic.hpp>

template<typename T, size_t Size>
class ringbuffer {
public:
  ringbuffer() : head_(0), tail_(0) {}

  bool push(const T & value)
  {
    size_t head = head_.load(boost::memory_order_relaxed);
    size_t next_head = next(head);
    if (next_head == tail_.load(boost::memory_order_acquire))
      return false;
    ring_[head] = value;
    head_.store(next_head, boost::memory_order_release);
    return true;
  }
  bool pop(T & value)
  {
    size_t tail = tail_.load(boost::memory_order_relaxed);
    if (tail == head_.load(boost::memory_order_acquire))
      return false;
    value = ring_[tail];
    tail_.store(next(tail), boost::memory_order_release);
    return true;
  }
private:
  size_t next(size_t current)
  {
    return (current + 1) % Size;
  }
  T ring_[Size];
  boost::atomic<size_t> head_, tail_;
};
#if 0
ringbuffer<int, 32> r;

// try to insert an element
if (r.push(42)) { /* succeeded */ }
else { /* buffer full */ }

// try to retrieve an element
int value;
if (r.pop(value)) { /* succeeded */ }
else { /* buffer empty */ }
#endif


//---------------------------------------------------------------------------//
//!
//! \file   tstInverseAngstromUnit.cpp
//! \author Alex Robinson
//! \brief  The inverse Angstrom unit unit tests
//!
//---------------------------------------------------------------------------//

// Boost Includes
#include <boost/units/quantity.hpp>
#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/si/length.hpp>

// Trilinos Includes
#include <Teuchos_UnitTestHarness.hpp>

// FRENSIE Includes
#include "Utility_InverseAngstromUnit.hpp"
#include "Utility_AngstromUnit.hpp"

using namespace Utility::Units;
using boost::units::quantity;
namespace si = boost::units::si;
namespace cgs = boost::units::cgs;

//---------------------------------------------------------------------------//
// Tests
//---------------------------------------------------------------------------//
// Check that the inverse angstrom unit can be initialized
TEUCHOS_UNIT_TEST( InverseAngstromUnit, initialize )
{
  quantity<InverseAngstrom> inverse_angstrom_quantity( 1.0*inverse_angstrom );

  TEST_EQUALITY_CONST( inverse_angstrom_quantity.value(), 1.0 );

  inverse_angstrom_quantity = 2.0*inverse_angstrom;

  TEST_EQUALITY_CONST( inverse_angstrom_quantity.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inverse angstrom unit can be initialized from the Angstrom
TEUCHOS_UNIT_TEST( InverseAngstromUnit, initialize_from_angstrom )
{
  quantity<InverseAngstrom> inverse_angstrom_quantity( 1.0/angstrom );

  TEST_EQUALITY_CONST( inverse_angstrom_quantity.value(), 1.0 );

  inverse_angstrom_quantity = 2.0/angstrom;

  TEST_EQUALITY_CONST( inverse_angstrom_quantity.value(), 2.0 );
}

//---------------------------------------------------------------------------//
// Check that the inverse angstrom unit can be initialized from cgs, si units
TEUCHOS_UNIT_TEST( InverseAngstromUnit, initialize_from_cgs_si )
{
  quantity<InverseAngstrom> inverse_angstrom_cgs( 1.0/cgs::centimeter );

  quantity<InverseAngstrom> inverse_angstrom_si( 1.0/si::meter );

  TEST_EQUALITY_CONST( inverse_angstrom_cgs.value(), 1e-8 );
  TEST_EQUALITY_CONST( inverse_angstrom_si.value(), 1e-10 );
}

//---------------------------------------------------------------------------//
// end tstInverseAngstromUnit.cpp
//---------------------------------------------------------------------------//


//=======================================================================
// Copyright (c) 2014 Piotr Wygocki
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file lsh_nearest_neighbors_regression_efficiency.cpp
 * @brief
 * @author Piotr Wygocki
 * @version 1.0
 * @date 2014-11-27
 */

#include "paal/regression/lsh_nearest_neighbors_regression.hpp"
#include "paal/utils/functors.hpp"

#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/irange.hpp>

#include <iostream>
#include <iterator>
#include <utility>
#include <vector>
#include <random>

int main() {
    using coordinate_t = int;
    using result_t = double;
    using point_coordinates_t = std::vector<coordinate_t>;
    using training_point_t = std::tuple<point_coordinates_t, result_t>;

    constexpr paal::utils::tuple_get<0> get_coordinates{};
    constexpr paal::utils::tuple_get<1> get_result{};

    auto const COORDINATE_NR = 10;
    auto const POINTS_NR = int(1e5);

    std::vector<training_point_t> training_points(POINTS_NR,
            training_point_t{point_coordinates_t(COORDINATE_NR), 0.0});
    std::vector<point_coordinates_t> test_points(POINTS_NR, point_coordinates_t(COORDINATE_NR));

    std::default_random_engine generator;
    std::uniform_int_distribution<int> distribution(0,1);

    for (auto & p : training_points) {
        for (auto & c : std::get<0>(p)) {
            c = distribution(generator);
        }
    }

    auto const dimensions = std::get<0>(training_points.front()).size();
    auto const PASSES = 50;
    auto const HASH_FUNCTIONS_PER_POINT = 4;
    auto const THREADS_COUNT = 2;
    auto const REPEAT_NR = 10;

    for (auto i = 0; i < REPEAT_NR; ++i) {
        auto model = paal::make_lsh_nearest_neighbors_regression_tuple_hash(
                training_points | boost::adaptors::transformed(get_coordinates),
                training_points | boost::adaptors::transformed(get_result),
                PASSES,
                paal::lsh::hamming_hash_function_generator{dimensions},
                HASH_FUNCTIONS_PER_POINT, THREADS_COUNT);

        std::vector<result_t> results;
        results.reserve(test_points.size());
        model.test(test_points, std::back_inserter(results));
    }

    return 0;
}