/**
 * Sample host program to generate a sequence
 * using jump and parallel generation.
 *
 * 1. make initial state from a seed
 * 2. calculate initial position for each work group.
 *    (This step is time comsuming)
 * 3. Loop:
 *   3.1 generate sub-sequences parallel
 *   3.2 jump for next loop
 */

#include "opencl_tools.hpp"
#include <cstddef>
#include <cfloat>
#include <ctime>
#include <NTL/GF2X.h>
#include <NTL/ZZ.h>

typedef uint32_t uint;
#include "mtgp32-calc-poly.hpp"
#include "mtgp-calc-jump.hpp"
#include "mtgp32-fast-jump.h"
#include "mtgp32-sample-common.h"
#include "parse_opt.h"

using namespace std;
using namespace cl;
using namespace NTL;

/* ================== */
/* OpenCL information */
/* ================== */
std::vector<cl::Platform> platforms;
std::vector<cl::Device> devices;
cl::Context context;
std::string programBuffer;
cl::Program program;
cl::Program::Sources source;
cl::CommandQueue queue;
std::string errorMessage;

/* ========================= */
/* Sample global variables
/* ========================= */
/**
 * max size of jump table
 * 2^(2*MAX_JUMP_TABLE-1) work groups will be supported
 * currently max 2048 work groups are supported
 */
#define MAX_JUMP_TABLE 6
static mtgp32_fast_t mtgp32;
static bool thread_max = false;
/* small size for check */
static const int jump_step = MTGP32_LS * 10;
static ZZ jump;
static uint32_t jump_poly[MTGP32_N];
static uint32_t jump_initial[MTGP32_N * MAX_JUMP_TABLE];

/* =========================
   declaration
   ========================= */
static int test(int argc, char * argv[]);
static void make_jump_table(int group_num);
static void initialize_by_seed(options& opt,
			       Buffer& status_buffer,
			       int group,
			       uint32_t seed);
static void initialize_by_array(options& opt,
				Buffer& status_buffer,
				int group,
				uint32_t seed_array[],
				int seed_size);
static void status_jump(Buffer& status_buffer, int group);
static void generate_uint32(int group_num,
			    Buffer& status_buffer,
			    int data_size);
static void generate_single12(int group_num,
			      Buffer& status_buffer,
			      int data_size);
static void generate_single01(int group_num,
			      Buffer& status_buffer,
			      int data_size);
static int init_check_data(mtgp32_fast_t * mtgp32,
			   uint32_t seed);
static int init_check_data_array(mtgp32_fast_t * mtgp32,
				 uint32_t seed_array[],
				 int size);
static void free_check_data(mtgp32_fast_t * mtgp32);
static void check_data(uint32_t * h_data, int num_data);
static void check_single12(float * h_data, int num_data);
static void check_single01(float * h_data, int num_data);

/* =========================
   mtgp32 sample code
   ========================= */
/**
 * main
 * catch errors
 *@param argc number of arguments
 *@param argv array of arguments
 *@return 0 normal, -1 error
 */
int main(int argc, char * argv[]) {
    try {
	return test(argc, argv);
    } catch (cl::Error e) {
	cerr << "Error Code:" << e.err() << endl;
	cerr << errorMessage << endl;
	cerr << e.what() << endl;
    }
}

/**
 * sample main
 *@param argc number of arguments
 *@param argv array of arguments
 *@return 0 normal, -1 error
 */
static int test(int argc, char * argv[]) {
#if defined(DEBUG)
    cout << "test start" << endl;
#endif
    options opt;
    if (!parse_opt(opt, argc, argv)) {
	return -1;
    }
    // OpenCL setup
#if defined(DEBUG)
    cout << "openCL setup start" << endl;
#endif
    platforms = getPlatforms();
    devices = getDevices();
    context = getContext();
#if defined(APPLE) || defined(__MACOSX) || defined(__APPLE__)
    source = getSource("mtgp32-jump.cli");
#else
    source = getSource("mtgp32-jump.cl");
#endif
    program = getProgram();
    queue = getCommandQueue();
#if defined(DEBUG)
    cout << "openCL setup end" << endl;
#endif

    int max_group_size = getMaxGroupSize();
    if (opt.group_num > max_group_size) {
	cout << "group_num greater than max value("
	     << max_group_size << ")"
	     << endl;
	return -1;
    }
    int max_size = getMaxWorkItemSize(0);
    if (MTGP32_TN > max_size) {
	cout << "workitem size is greater than max value("
	     << dec << max_size << ")"
	     << "current:" << dec << MTGP32_N << endl;
	return -1;
    }
    if (MTGP32_N > max_size) {
	thread_max = true;
    }
    int local_mem_size = getLocalMemSize();
    if (local_mem_size < sizeof(uint32_t) * MTGP32_N * 2) {
	cout << "local memory size is smaller than min value("
	     << dec << sizeof(uint32_t) * MTGP32_N * 2
	     << ") current:"
	     << dec << local_mem_size << endl;
	return -1;
    }
    Buffer status_buffer(context,
			 CL_MEM_READ_WRITE,
			 sizeof(uint32_t) * MTGP32_N * opt.group_num);

    make_jump_table(opt.group_num);
    int data_count = opt.data_count;
    int data_unit = jump_step * opt.group_num;

    // initialize by seed
    // generate uint32_t
    init_check_data(&mtgp32, 1234);
    initialize_by_seed(opt, status_buffer, opt.group_num, 1234);
    while (data_count > 0) {
	generate_uint32(opt.group_num, status_buffer, data_unit);
	status_jump(status_buffer, opt.group_num);
	data_count -= data_unit;
    }
    free_check_data(&mtgp32);

    // initialize by array
    // generate single float
    uint32_t seed_array[5] = {1, 2, 3, 4, 5};
    init_check_data_array(&mtgp32, seed_array, 5);
    initialize_by_array(opt, status_buffer, opt.group_num,
			seed_array, 5);
    data_count = opt.data_count;
    while (data_count > 0) {
	generate_single12(opt.group_num, status_buffer, data_unit);
	status_jump(status_buffer, opt.group_num);
	data_count -= data_unit;
	if (data_count > 0) {
	    generate_single01(opt.group_num, status_buffer, data_unit);
	    status_jump(status_buffer, opt.group_num);
	    data_count -= data_unit;
	}
    }
    free_check_data(&mtgp32);
    return 0;
}

/**
 * prepare jump polynomial.
 * this step may be pre-computed in practical use.
 * @param group_num number of work groups
 */
static void make_jump_table(int group_num)
{
#if defined(DEBUG)
    cout << "make_jump_table start" << endl;
#endif
    mtgp32_fast_t dummy;
    int rc = mtgp32_init(&dummy, &mtgp32_params_fast_11213[0], 1);
    if (rc) {
	cerr << "init error" << endl;
	throw cl::Error(rc, "mtgp32 init error");
    }
    GF2X poly;
    clock_t start = clock();
    calc_characteristic(poly, &dummy);
    clock_t end = clock();
    double time = (double)(end - start) / CLOCKS_PER_SEC * 1000.0;
    cout << "calc_characteristic: " << dec << time << "ms" << endl;
    ZZ step;
    step = jump_step;
    start = clock();
    for (int i = 0; i < MAX_JUMP_TABLE; i++) {
	calc_jump(&jump_initial[i * MTGP32_N],
		  MTGP32_N,
		  step,
		  poly);
	step *= 4;
    }
    step = jump_step;
    step *= group_num - 1;
    calc_jump(jump_poly, MTGP32_N, step, poly);
    end = clock();
    time = (double)(end - start) / CLOCKS_PER_SEC * 1000.0;
    cout << "make jump table: " << dec << time << "ms" << endl;
#if defined(DEBUG)
    cout << "step:" << dec << step << endl;
    cout << "jump_poly[0]:" << hex << jump_poly[0] << endl;
    cout << "jump_poly[1]:" << hex << jump_poly[1] << endl;
    cout << "jump_initial[0]:" << hex << jump_initial[0 * MTGP32_N] << endl;
    cout << "jump_initial[1]:" << hex << jump_initial[1 * MTGP32_N] << endl;
    cout << "jump_initial[2]:" << hex << jump_initial[2 * MTGP32_N] << endl;
#endif
#if defined(DEBUG)
    cout << "make_jump_table end" << endl;
#endif
}

/**
 * initialize mtgp status in device global memory
 * using seed and fixed jump.
 * jump step is fixed to 3^162.
 *@param opt command line option
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 *@param seed seed for initialization
 */
static void initialize_by_seed(options& opt,
			       Buffer& status_buffer,
			       int group,
			       uint32_t seed)
{
#if defined(DEBUG)
    cout << "initialize_by_seed start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t),
			     jump_initial);

    Kernel init_kernel(program, "mtgp32_jump_seed_kernel");
#if defined(DEBUG)
    cout << "arg0 start" << endl;
#endif
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, seed);
    init_kernel.setArg(2, jump_table_buffer);
#if defined(DEBUG)
    cout << "arg2 end" << endl;
#endif
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
#if defined(DEBUG)
    cout << "global:" << dec << group * local_item << endl;
    cout << "group:" << dec << group << endl;
    cout << "local:" << dec << local_item << endl;
#endif
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
    cout << "initializing time = " << time * 1000 << "ms" << endl;
#if 0
    uint status[group * MTGP32_N];
    queue.enqueueReadBuffer(status_buffer,
			    CL_TRUE,
			    0,
			    sizeof(uint32_t) * MTGP32_N * group,
			    status);
#if defined(DEBUG)
    cout << "status[0]:" << hex << status[0] << endl;
    cout << "status[MTGP32_N - 1]:" << hex << status[MTGP32_N - 1] << endl;
    cout << "status[MTGP32_N]:" << hex << status[MTGP32_N] << endl;
    cout << "status[MTGP32_N + 1]:" << hex << status[MTGP32_N + 1] << endl;
#endif
    check_status(status, group);
#endif
#if defined(DEBUG)
    cout << "initialize_by_seed end" << endl;
#endif
}

/**
 * initialize mtgp status in device global memory
 * using an array of seeds and jump.
 *@param opt command line option
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 *@param seed_array seeds for initialization
 *@param seed_size size of seed_array
 */
static void initialize_by_array(options& opt,
				Buffer& status_buffer,
				int group,
				uint32_t seed_array[],
				int seed_size)
{
#if defined(DEBUG)
    cout << "initialize_by_array start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * MAX_JUMP_TABLE * sizeof(uint32_t),
			     jump_initial);

    Buffer seed_array_buffer(context,
			     CL_MEM_READ_WRITE,
			     seed_size * sizeof(uint32_t));
    queue.enqueueWriteBuffer(seed_array_buffer,
			     CL_TRUE,
			     0,
			     seed_size * sizeof(uint32_t),
			     seed_array);
    Kernel init_kernel(program, "mtgp32_jump_array_kernel");
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, seed_array_buffer);
    init_kernel.setArg(2, seed_size);
    init_kernel.setArg(3, jump_table_buffer);
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
#if 0
    uint status[group * MTGP32_N];
    queue.enqueueReadBuffer(status_buffer,
			    CL_TRUE,
			    0,
			    sizeof(uint32_t) * MTGP32_N * group,
			    status);
    check_status(status, group);
#endif
    cout << "initializing time = " << time * 1000 << "ms" << endl;
#if defined(DEBUG)
    cout << "initialize_by_array end" << endl;
#endif
}

/**
 * jump mtgp status in device global memory
 *@param status_buffer mtgp status in device global memory
 *@param group number of group
 */
static void status_jump(Buffer& status_buffer, int group)
{
#if defined(DEBUG)
    cout << "jump start" << endl;
#endif
    // jump table
    Buffer jump_table_buffer(context,
			     CL_MEM_READ_WRITE,
			     MTGP32_N * sizeof(uint32_t));
    queue.enqueueWriteBuffer(jump_table_buffer,
			     CL_TRUE,
			     0,
			     MTGP32_N * sizeof(uint32_t),
			     jump_poly);

    Kernel init_kernel(program, "mtgp32_jump_kernel");
    init_kernel.setArg(0, status_buffer);
    init_kernel.setArg(1, jump_table_buffer);
    int local_item = MTGP32_N;
    if (thread_max) {
	local_item = MTGP32_TN;
    }
    NDRange global(group * local_item);
    NDRange local(local_item);
    Event event;
#if defined(DEBUG)
    cout << "global:" << dec << group * local_item << endl;
    cout << "group:" << dec << group << endl;
    cout << "local:" << dec << local_item << endl;
#endif
    queue.enqueueNDRangeKernel(init_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &event);
    double time = get_time(event);
    cout << "jump time = " << time * 1000 << "ms" << endl;
#if defined(DEBUG)
    cout << "jump end" << endl;
#endif
}

/**
 * generate 32 bit unsigned random numbers in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_uint32(int group_num,
			    Buffer& status_buffer,
			    int data_size)
{
#if defined(DEBUG)
    cout << "generate_uint32 start" << endl;
#endif
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel uint_kernel(program, "mtgp32_uint32_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(uint32_t));
    uint_kernel.setArg(0, status_buffer);
    uint_kernel.setArg(1, output_buffer);
    uint_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
#if defined(DEBUG)
    cout << "generate_uint32 enque kernel start" << endl;
#endif
    queue.enqueueNDRangeKernel(uint_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
#if defined(DEBUG)
    cout << "generate_uint32 enque kernel end" << endl;
#endif
    uint32_t * output = new uint32_t[data_size];
#if defined(DEBUG)
    cout << "generate_uint32 event wait start" << endl;
#endif
    generate_event.wait();
#if defined(DEBUG)
    cout << "generate_uint32 event wait end" << endl;
#endif
#if defined(DEBUG)
    cout << "generate_uint32 readbuffer start" << endl;
#endif
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(uint32_t),
			    &output[0]);
#if defined(DEBUG)
    cout << "generate_uint32 readbuffer end" << endl;
#endif
    check_data(output, data_size);
    print_uint32(&output[0], data_size, item_num);
    double time = get_time(generate_event);
    cout << "generate time:" << time * 1000 << "ms" << endl;
    delete[] output;
#if defined(DEBUG)
    cout << "generate_uint32 end" << endl;
#endif
}

/**
 * generate single precision floating point numbers in the range [1, 2)
 * in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_single12(int group_num,
			      Buffer& status_buffer,
			      int data_size)
{
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel single_kernel(program, "mtgp32_single12_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(float));
    single_kernel.setArg(0, status_buffer);
    single_kernel.setArg(1, output_buffer);
    single_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
    queue.enqueueNDRangeKernel(single_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
    float * output = new float[data_size];
    generate_event.wait();
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(float),
			    &output[0]);
    check_single12(output, data_size);
    print_float(output, data_size, item_num);
    double time = get_time(generate_event);
    delete[] output;
    cout << "generate time:" << time * 1000 << "ms" << endl;
}

/**
 * generate single precision floating point numbers in the range [0, 1)
 * in device global memory
 *@param group_num number of groups for execution
 *@param status_buffer mtgp status in device global memory
 *@param data_size number of data to generate
 */
static void generate_single01(int group_num,
			      Buffer& status_buffer,
			      int data_size)
{
    int item_num = MTGP32_TN * group_num;
    int min_size = MTGP32_LS * group_num;
    if (data_size % min_size != 0) {
	data_size = (data_size / min_size + 1) * min_size;
    }
    Kernel single_kernel(program, "mtgp32_single01_kernel");
    Buffer output_buffer(context,
			 CL_MEM_READ_WRITE,
			 data_size * sizeof(float));
    single_kernel.setArg(0, status_buffer);
    single_kernel.setArg(1, output_buffer);
    single_kernel.setArg(2, data_size / group_num);
    NDRange global(item_num);
    NDRange local(MTGP32_TN);
    Event generate_event;
    queue.enqueueNDRangeKernel(single_kernel,
			       NullRange,
			       global,
			       local,
			       NULL,
			       &generate_event);
    float * output = new float[data_size];
    generate_event.wait();
    queue.enqueueReadBuffer(output_buffer,
			    CL_TRUE,
			    0,
			    data_size * sizeof(float),
			    &output[0]);
    check_single01(output, data_size);
    print_float(output, data_size, item_num);
    double time = get_time(generate_event);
    delete[] output;
    cout << "generate time:" << time * 1000 << "ms" << endl;
}

/* ==============
 * check programs
 * ==============*/
static int init_check_data(mtgp32_fast_t * mtgp32,
			   uint32_t seed)
{
#if defined(DEBUG)
    cout << "init_check_data start" << endl;
#endif
    int rc = mtgp32_init(mtgp32,
			 &mtgp32_params_fast_11213[0],
			 seed);
    if (rc) {
	return rc;
    }
#if defined(DEBUG)
    cout << "init_check_data end" << endl;
#endif
    return 0;
}

static int init_check_data_array(mtgp32_fast_t * mtgp32,
				 uint32_t seed_array[],
				 int size)
{
#if defined(DEBUG)
    cout << "init_check_data_array start" << endl;
#endif
    int rc = mtgp32_init_by_array(mtgp32,
				  &mtgp32_params_fast_11213[0],
				  seed_array,
				  size);
    if (rc) {
	return rc;
    }
#if defined(DEBUG)
    cout << "init_check_data_array end" << endl;
#endif
    return 0;
}

static void free_check_data(mtgp32_fast_t * mtgp32)
{
#if defined(DEBUG)
    cout << "free_check_data start" << endl;
#endif
    mtgp32_free(mtgp32);
#if defined(DEBUG)
    cout << "free_check_data end" << endl;
#endif
}

static void check_data(uint32_t * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_data start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	uint32_t r = mtgp32_genrand_uint32(&mtgp32);
	if ((h_data[j] != r) && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << hex << h_data[j]
		 << " r = " << hex << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_data check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_data end" << endl;
#endif
}

static void check_single12(float * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_single start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	float r =  mtgp32_genrand_close1_open2(&mtgp32);
	if (!(-FLT_EPSILON <= h_data[j] - r &&
	     h_data[j] - r <= FLT_EPSILON)
	    && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << dec << h_data[j]
		 << " r = " << dec << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_single check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_single end" << endl;
#endif
}

static void check_single01(float * h_data, int num_data)
{
#if defined(DEBUG)
    cout << "check_single start" << endl;
#endif
    bool error = false;
    bool disp_flg = true;
    int count = 0;
    for (int j = 0; j < num_data; j++) {
	float r =  mtgp32_genrand_close_open(&mtgp32);
	if (!(-FLT_EPSILON <= h_data[j] - r &&
	     h_data[j] - r <= FLT_EPSILON)
	    && disp_flg) {
	    cout << "mismatch"
		 << " j = " << dec << j
		 << " data = " << dec << h_data[j]
		 << " r = " << dec << r << endl;
	    cout << "check_data check N.G!" << endl;
	    count++;
	    error = true;
	}
	if (count > 10) {
	    disp_flg = false;
	}
    }
    if (!error) {
	cout << "check_single check O.K!" << endl;
    } else {
	throw cl::Error(-1, "mtgp32 check_data error!");
    }
#if defined(DEBUG)
    cout << "check_single end" << endl;
#endif
}


/**
 * 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 "teaser/matcher.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <flann/flann.hpp>

#include "teaser/geometry.h"

namespace teaser {

std::vector<std::pair<int, int>> Matcher::calculateCorrespondences(
    teaser::PointCloud& source_points, teaser::PointCloud& target_points,
    teaser::FPFHCloud& source_features, teaser::FPFHCloud& target_features, bool use_absolute_scale,
    bool use_crosscheck, bool use_tuple_test, float tuple_scale) {

  Feature cloud_features;
  pointcloud_.push_back(source_points);
  pointcloud_.push_back(target_points);

  // It compute the global_scale_ required to set correctly the search radius
  normalizePoints(use_absolute_scale);

  for (auto& f : source_features) {
    Eigen::VectorXf fpfh(33);
    for (int i = 0; i < 33; i++)
      fpfh(i) = f.histogram[i];
    cloud_features.push_back(fpfh);
  }
  features_.push_back(cloud_features);

  cloud_features.clear();
  for (auto& f : target_features) {
    Eigen::VectorXf fpfh(33);
    for (int i = 0; i < 33; i++)
      fpfh(i) = f.histogram[i];
    cloud_features.push_back(fpfh);
  }
  features_.push_back(cloud_features);

  advancedMatching(use_crosscheck, use_tuple_test, tuple_scale);

  return corres_;
}

void Matcher::normalizePoints(bool use_absolute_scale) {
  int num = 2;
  float scale = 0;

  means_.clear();

  for (int i = 0; i < num; ++i) {
    float max_scale = 0;

    // compute mean
    Eigen::Vector3f mean;
    mean.setZero();

    int npti = pointcloud_[i].size();
    for (int ii = 0; ii < npti; ++ii) {
      Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z);
      mean = mean + p;
    }
    mean = mean / npti;
    means_.push_back(mean);

    for (int ii = 0; ii < npti; ++ii) {
      pointcloud_[i][ii].x -= mean(0);
      pointcloud_[i][ii].y -= mean(1);
      pointcloud_[i][ii].z -= mean(2);
    }

    // compute scale
    for (int ii = 0; ii < npti; ++ii) {
      Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z);
      float temp = p.norm(); // because we extract mean in the previous stage.
      if (temp > max_scale) {
        max_scale = temp;
      }
    }

    if (max_scale > scale) {
      scale = max_scale;
    }
  }

  // mean of the scale variation
  if (use_absolute_scale) {
    global_scale_ = 1.0f;
  } else {
    global_scale_ = scale; // second choice: we keep the maximum scale.
  }

  if (global_scale_ != 1.0f) {
    for (int i = 0; i < num; ++i) {
      int npti = pointcloud_[i].size();
      for (int ii = 0; ii < npti; ++ii) {
        pointcloud_[i][ii].x /= global_scale_;
        pointcloud_[i][ii].y /= global_scale_;
        pointcloud_[i][ii].z /= global_scale_;
      }
    }
  }
}
void Matcher::advancedMatching(bool use_crosscheck, bool use_tuple_test, float tuple_scale) {

  int fi = 0; // source idx
  int fj = 1; // destination idx

  bool swapped = false;

  if (pointcloud_[fj].size() > pointcloud_[fi].size()) {
    int temp = fi;
    fi = fj;
    fj = temp;
    swapped = true;
  }

  int nPti = pointcloud_[fi].size();
  int nPtj = pointcloud_[fj].size();

  ///////////////////////////
  /// Build FLANNTREE
  ///////////////////////////
  KDTree feature_tree_i(flann::KDTreeSingleIndexParams(15));
  buildKDTree(features_[fi], &feature_tree_i);

  KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15));
  buildKDTree(features_[fj], &feature_tree_j);

  std::vector<int> corres_K, corres_K2;
  std::vector<float> dis;
  std::vector<int> ind;

  std::vector<std::pair<int, int>> corres;
  std::vector<std::pair<int, int>> corres_cross;
  std::vector<std::pair<int, int>> corres_ij;
  std::vector<std::pair<int, int>> corres_ji;

  ///////////////////////////
  /// INITIAL MATCHING
  ///////////////////////////
  std::vector<int> i_to_j(nPti, -1);
  for (int j = 0; j < nPtj; j++) {
    searchKDTree(&feature_tree_i, features_[fj][j], corres_K, dis, 1);
    int i = corres_K[0];
    if (i_to_j[i] == -1) {
      searchKDTree(&feature_tree_j, features_[fi][i], corres_K, dis, 1);
      int ij = corres_K[0];
      i_to_j[i] = ij;
    }
    corres_ji.push_back(std::pair<int, int>(i, j));
  }

  for (int i = 0; i < nPti; i++) {
    if (i_to_j[i] != -1)
      corres_ij.push_back(std::pair<int, int>(i, i_to_j[i]));
  }

  int ncorres_ij = corres_ij.size();
  int ncorres_ji = corres_ji.size();

  // corres = corres_ij + corres_ji;
  for (int i = 0; i < ncorres_ij; ++i)
    corres.push_back(std::pair<int, int>(corres_ij[i].first, corres_ij[i].second));
  for (int j = 0; j < ncorres_ji; ++j)
    corres.push_back(std::pair<int, int>(corres_ji[j].first, corres_ji[j].second));

  ///////////////////////////
  /// CROSS CHECK
  /// input : corres_ij, corres_ji
  /// output : corres
  ///////////////////////////
  if (use_crosscheck) {
    std::cout << "CROSS CHECK" << std::endl;
    // build data structure for cross check
    corres.clear();
    corres_cross.clear();
    std::vector<std::vector<int>> Mi(nPti);
    std::vector<std::vector<int>> Mj(nPtj);

    int ci, cj;
    for (int i = 0; i < ncorres_ij; ++i) {
      ci = corres_ij[i].first;
      cj = corres_ij[i].second;
      Mi[ci].push_back(cj);
    }
    for (int j = 0; j < ncorres_ji; ++j) {
      ci = corres_ji[j].first;
      cj = corres_ji[j].second;
      Mj[cj].push_back(ci);
    }

    // cross check
    for (int i = 0; i < nPti; ++i) {
      for (int ii = 0; ii < Mi[i].size(); ++ii) {
        int j = Mi[i][ii];
        for (int jj = 0; jj < Mj[j].size(); ++jj) {
          if (Mj[j][jj] == i) {
            corres.push_back(std::pair<int, int>(i, j));
            corres_cross.push_back(std::pair<int, int>(i, j));
          }
        }
      }
    }
  } else {
    std::cout << "Skipping Cross Check." << std::endl;
  }

  ///////////////////////////
  /// TUPLE CONSTRAINT
  /// input : corres
  /// output : corres
  ///////////////////////////
  if (use_tuple_test && tuple_scale != 0) {
    std::cout << "TUPLE CONSTRAINT" << std::endl;
    srand(time(NULL));
    int rand0, rand1, rand2;
    int idi0, idi1, idi2;
    int idj0, idj1, idj2;
    float scale = tuple_scale;
    int ncorr = corres.size();
    int number_of_trial = ncorr * 100;
    std::vector<std::pair<int, int>> corres_tuple;

    for (int i = 0; i < number_of_trial; i++) {
      rand0 = rand() % ncorr;
      rand1 = rand() % ncorr;
      rand2 = rand() % ncorr;

      idi0 = corres[rand0].first;
      idj0 = corres[rand0].second;
      idi1 = corres[rand1].first;
      idj1 = corres[rand1].second;
      idi2 = corres[rand2].first;
      idj2 = corres[rand2].second;

      // collect 3 points from i-th fragment
      Eigen::Vector3f pti0 = {pointcloud_[fi][idi0].x, pointcloud_[fi][idi0].y,
                              pointcloud_[fi][idi0].z};
      Eigen::Vector3f pti1 = {pointcloud_[fi][idi1].x, pointcloud_[fi][idi1].y,
                              pointcloud_[fi][idi1].z};
      Eigen::Vector3f pti2 = {pointcloud_[fi][idi2].x, pointcloud_[fi][idi2].y,
                              pointcloud_[fi][idi2].z};

      float li0 = (pti0 - pti1).norm();
      float li1 = (pti1 - pti2).norm();
      float li2 = (pti2 - pti0).norm();

      // collect 3 points from j-th fragment
      Eigen::Vector3f ptj0 = {pointcloud_[fj][idj0].x, pointcloud_[fj][idj0].y,
                              pointcloud_[fj][idj0].z};
      Eigen::Vector3f ptj1 = {pointcloud_[fj][idj1].x, pointcloud_[fj][idj1].y,
                              pointcloud_[fj][idj1].z};
      Eigen::Vector3f ptj2 = {pointcloud_[fj][idj2].x, pointcloud_[fj][idj2].y,
                              pointcloud_[fj][idj2].z};

      float lj0 = (ptj0 - ptj1).norm();
      float lj1 = (ptj1 - ptj2).norm();
      float lj2 = (ptj2 - ptj0).norm();

      if ((li0 * scale < lj0) && (lj0 < li0 / scale) && (li1 * scale < lj1) &&
          (lj1 < li1 / scale) && (li2 * scale < lj2) && (lj2 < li2 / scale)) {
        corres_tuple.push_back(std::pair<int, int>(idi0, idj0));
        corres_tuple.push_back(std::pair<int, int>(idi1, idj1));
        corres_tuple.push_back(std::pair<int, int>(idi2, idj2));
      }
    }
    corres.clear();

    for (size_t i = 0; i < corres_tuple.size(); ++i)
      corres.push_back(std::pair<int, int>(corres_tuple[i].first, corres_tuple[i].second));
  } else {
    std::cout << "Skipping Tuple Constraint." << std::endl;
  }

  if (swapped) {
    std::vector<std::pair<int, int>> temp;
    for (size_t i = 0; i < corres.size(); i++)
      temp.push_back(std::pair<int, int>(corres[i].second, corres[i].first));
    corres.clear();
    corres = temp;
  }
  corres_ = corres;

  ///////////////////////////
  /// ERASE DUPLICATES
  /// input : corres_
  /// output : corres_
  ///////////////////////////
  std::sort(corres_.begin(), corres_.end());
  corres_.erase(std::unique(corres_.begin(), corres_.end()), corres_.end());
}

template <typename T> void Matcher::buildKDTree(const std::vector<T>& data, Matcher::KDTree* tree) {
  int rows, dim;
  rows = (int)data.size();
  dim = (int)data[0].size();
  std::vector<float> dataset(rows * dim);
  flann::Matrix<float> dataset_mat(&dataset[0], rows, dim);
  for (int i = 0; i < rows; i++)
    for (int j = 0; j < dim; j++)
      dataset[i * dim + j] = data[i][j];
  KDTree temp_tree(dataset_mat, flann::KDTreeSingleIndexParams(15));
  temp_tree.buildIndex();
  *tree = temp_tree;
}

template <typename T>
void Matcher::searchKDTree(Matcher::KDTree* tree, const T& input, std::vector<int>& indices,
                           std::vector<float>& dists, int nn) {
  int rows_t = 1;
  int dim = input.size();

  std::vector<float> query;
  query.resize(rows_t * dim);
  for (int i = 0; i < dim; i++)
    query[i] = input(i);
  flann::Matrix<float> query_mat(&query[0], rows_t, dim);

  indices.resize(rows_t * nn);
  dists.resize(rows_t * nn);
  flann::Matrix<int> indices_mat(&indices[0], rows_t, nn);
  flann::Matrix<float> dists_mat(&dists[0], rows_t, nn);

  tree->knnSearch(query_mat, indices_mat, dists_mat, nn, flann::SearchParams(128));
}

} // namespace teaser


//
// Created by Liuyu Jin on Jun 9, 2016.
//

#include "ICLasso.hpp"
#include <Eigen/Dense>
#include <boost/math/distributions.hpp>
#include <math.h>
#include <unordered_map>
#include <iostream>

#ifdef BAZEL
#include "model/ModelOptions.hpp"
#else
#include "ModelOptions.hpp"
#endif

using namespace Eigen;
using namespace std;

ICLasso::ICLasso() {
    lambda1 = 1;
    lambda2 = 1;
    gamma = 1;
};

void ICLasso::set_X(MatrixXf new_X){
    X = new_X;
};

void ICLasso::set_Y(MatrixXf new_Y){
    Y = new_Y;
};

void ICLasso::set_XY(MatrixXf new_X, MatrixXf new_Y){
    X = new_X;
    Y = new_Y;
    //initialize Beta here
    Beta = MatrixXf::Random(X.cols(),Y.rows());
};

void ICLasso::set_lambda1(float new_l){
    lambda1 = new_l;
};

void ICLasso::set_lambda2(float new_l){
    lambda2 = new_l;
};

void ICLasso::set_gamma(float new_g){
    gamma = new_g;
};

void ICLasso::set_theta(MatrixXf new_t){
    Theta = new_t;
};

/* helpers for cost */
float square(float a){
    return a*a;
};

MatrixXf cov(MatrixXf X){
  MatrixXf centered = X.rowwise() - X.colwise().mean();
  MatrixXf result = (centered.adjoint() * centered) / float(X.rows() - 1);
  return result;
};

float sign(float x){
    if (x > 0) return 1;
    if (x < 0) return -1;
    return 0;
};

/* end helpers for cost */

float ICLasso::cost() {
    int n = X.cols();
    MatrixXf YXBeta = Y-X*Beta;
    MatrixXf squared = YXBeta.unaryExpr(std::ptr_fun(square));
    float loss1 = squared.sum()/n;
    float loss2 = (cov(Y)*Theta).trace() - log(Theta.determinant());
    float pen1 = Beta.cwiseAbs().sum();
    float pen2 = Theta.cwiseAbs().sum();
    float pen3 = 0;
    float incr;
    int size_T = Theta.rows();
    for (int i = 0; i < size_T; i++){
        for (int j = i+1; j < size_T; j++){
            incr = ((Beta.col(i) + sign(Theta(i,j))*Beta.col(j)).cwiseAbs().sum());
            pen3 = pen3 + abs(Theta(i, j))*incr;
        }
    }
    return loss1 + loss2 + lambda1 * pen1 + lambda2 * pen2 + gamma * pen3;
};

/* Helpers for optimize_theta */
float cost_theta(MatrixXf S, MatrixXf Beta, MatrixXf Theta, float lambda, float gamma) {
    float loss = (S*Theta).trace()-log(Theta.determinant());
    float pen2 = Theta.cwiseAbs().sum();
    float pen3 = 0;
    int size_T = Theta.rows();
    float incr;
    for (int i = 0; i < size_T; i++){
        for (int j = i+1; j < size_T; j++){
            incr = ((Beta.col(i) + sign(Theta(i,j))*Beta.col(j)).cwiseAbs().sum());
            pen3 = pen3 + abs(Theta(i, j))*incr;
        }
    }
    return loss + lambda * pen2 + gamma * pen3;
};

MatrixXf remove_row(MatrixXf X, int j){
    int numRows = X.rows()-1;
    int numCols = X.cols();
    MatrixXf result = MatrixXf::Zero(numRows, numCols);
    result.block(0, 0, j, numCols) = X.block(0, 0, j, numCols);
    result.block(j, 0, numRows-j, numCols) = X.block(j+1, 0, numRows-j, numCols);
    return result;
};

MatrixXf remove_col(MatrixXf X, int j){
    int numRows = X.rows();
    int numCols = X.cols() - 1;
    MatrixXf result = MatrixXf::Zero(numRows, numCols);
    result.block(0, 0, numRows, j) = X.block(0, 0, numRows, j);
    result.block(0, j, numRows, numCols - j) = X.block(0, j+1, numRows, numCols-j);
    return result;
};

void change_theta(MatrixXf C, MatrixXf Theta, int j){
    for (int i = 0; i < Theta.cols(); i++){
        if (i != j){
            Theta(i, j) = Theta(j, j) * C(i, j);
        }
    }
};

void symmetrize(MatrixXf Theta, int j){
    for (int i = 0; i < Theta.cols(); i++){
        if (i != j){
            Theta(j, i) = Theta(i, j);
        }
    }
};

MatrixXf bound_below(MatrixXf X){
    for (int i = 0; i < X.rows(); i++){
        for (int j = 0; j < X.cols(); j++){
            if (X(i, j) < 0){
                X(i, j) = 0;
            }
        }
    }
    return X;
}

MatrixXf update_beta_mex(MatrixXf X, MatrixXf S, MatrixXf a,
                     MatrixXf b, float lambda, float gamma, MatrixXf Beta){
    int p = X.cols();
    MatrixXf beta_new = MatrixXf::Zero(p, 1);
    int k, j;
    for (k = 0; k < p; k++) {
        beta_new(k,0) = Beta(k, 0);
    }
    float beta_k, upper_k, lower_k;
    for (k = 0; k < p; k++) {
        beta_k = S(k, 0);
        for (j = 0; j < p; j++) {
            if (j != k)
                beta_k += X(k,j)*beta_new(j, 0);
        }
        beta_k = -1*beta_k/X(k, k);
        upper_k = (gamma*a(k,0) + lambda)/X(k, k);
        lower_k = -1*(gamma*b(k,0) + lambda)/X(k, k);
        if (beta_k > upper_k) {
            beta_new(k,0) = beta_k - upper_k;
        } else if (beta_k < lower_k) {
            beta_new(k,0) = beta_k - lower_k;
        } else {
            beta_new(k,0) = 0;
        }
    }
    return beta_new;
}

MatrixXf optimize_block_coord_sigma(MatrixXf X,
       MatrixXf S, int maxiter, MatrixXf a, MatrixXf b, float lambda,
       float gamma){
    int p = X.cols();
    MatrixXf Beta = MatrixXf::Zero(p, 1);
    MatrixXf oldBeta;
    for (int i = 0; i < maxiter; i++){
        oldBeta = Beta;
        Beta = update_beta_mex(X,S,a,b,lambda,gamma,Beta);
        if ((Beta-oldBeta).norm() < 1e-3) break;
    }
    return -X*(bound_below(Beta)-bound_below(-Beta));
}

MatrixXf optimize_block_coord_beta(MatrixXf X,
    MatrixXf S, int maxiter, MatrixXf a, MatrixXf b, float lambda, float gamma){
    int p = X.cols();
    MatrixXf Beta = MatrixXf::Zero(p, 1);
    MatrixXf oldBeta;
    for (int i = 0; i < maxiter; i++){
        oldBeta = Beta;
        Beta = update_beta_mex(X,S,a,b,lambda,gamma,Beta);
        if ((Beta-oldBeta).norm() < 1e-3) break;
    }
    return Beta;
}

MatrixXf fused_prox_vector(MatrixXf beta, MatrixXf u, MatrixXf l){
    int len = beta.rows();
    MatrixXf w = MatrixXf::Zero(len, 1);
    for (int i = 0; i < len; i++){
        float beta_c = beta(i, 0);
        if (beta_c > u(i, 0)){
            beta(i, 0) = beta_c - u(i, 0);
        } else if (beta_c < l(i, 0)){
            beta(i, 0) = beta_c - l(i, 0);
        }
    }
    return w;

}

float fused_prox_scalar(float beta, float u, float l){
    if (beta > u){
        return beta - u;
    } else if (beta < l){
        return beta - l;
    } else {
        return 0;
    }
}

MatrixXf cwiseAdd(MatrixXf X, float p){
    for (int i = 0; i < X.rows(); i++){
        for (int j = 0; j < X.cols(); j++){
            X(i, j) = X(i, j) + p;
        }
    }
    return X;
}

/* type issues with Q and h_beta */

MatrixXf optimize_block_prox(MatrixXf X,
     MatrixXf s, int maxiter, MatrixXf a, MatrixXf b, float lambda, float gamma){

    //get dimension
    int n = X.rows();

    //initialize variables
    MatrixXf Beta = MatrixXf::Zero(n, 1);
    MatrixXf w = Beta;
    float theta = 1;
    float L = 10;
    MatrixXf objVals = MatrixXf::Zero(maxiter, 1);

    MatrixXf h_w, grad, z, beta_new, upper, lower;
    float h_beta, Q;
    float theta_new;

    //optimize Beta
    for (int iter = 0; iter < maxiter; iter++){
        //computer gradient
        grad = (X*w) + s;

        //compute new estimate of beta using line search
        h_w = w.transpose()*X*w/2 + s.transpose()*w;
        while (true){
            z = w - (1/L)*grad;
            upper = cwiseAdd(gamma*a, lambda)/L;
            lower = -cwiseAdd(gamma*b, lambda)/L;
            beta_new = fused_prox_vector(z,upper,lower);
            h_beta = ((beta_new.transpose()*X*beta_new)/2 + (s.transpose()*beta_new))(0, 0);
            Q = (h_w + (beta_new-w).transpose()*grad)(0,0)+
                L*((beta_new-w).cwiseProduct(beta_new-w)).sum()/2;
            if (h_beta <= Q) break;
            else L = 2*L;

            //update w and theta
            theta_new = (1 + sqrt(1 + 4*theta*theta))/2;
            w = beta_new + (theta-1)/(theta_new)*(beta_new-Beta);

            //store current objective value
            objVals(iter, 0) = h_beta + (cwiseAdd(gamma*a, lambda).transpose()*bound_below(beta_new))(0,0) +
                               (cwiseAdd(gamma*b, lambda).transpose()*bound_below(-beta_new))(0, 0);

            //store new variables
            Beta = beta_new;
            theta = theta_new;
            if (iter >= 5 && abs(objVals(iter, 0)-objVals(iter-1, 0))/abs(objVals(iter-1, 0)) < 1e-8)
            break;

        }
    }

    //calculate sigma
    MatrixXf sigma = -X*(bound_below(beta_new)-bound_below(-beta_new));
    return sigma;

}

/* end helpers for optimize_theta */

void ICLasso::optimize_theta(){

    int q = Y.rows();

    //precompute sample covariance & penalty matrices
    MatrixXf S = cov(Y);
    MatrixXf A = MatrixXf::Zero(q, q);
    MatrixXf B = MatrixXf::Zero(q, q);
    for (int j = 0; j < q; j++){
        for (int k = 0; k < q; k++){
            A(j, k) = (Beta.col(j) + Beta.col(k)).cwiseAbs().sum()/2;
            B(j, k) = (Beta.col(j) - Beta.col(k)).cwiseAbs().sum()/2;
        }
        A(j, j) = 0;
        B(j, j) = 0;
    }
    int maxiter = 10000;
    float tol = 1e-4;
    MatrixXf C, s, a, b;

    //store objective values and run times
    MatrixXf objVals = MatrixXf::Zero(maxiter, 1);

    //initialize values;
    MatrixXf Sigma = S + lambda * MatrixXf::Identity(q, q);

    int i = 0;
    //run optimization to obtain estimate of covariance
    for (int iter = 0; iter < maxiter; iter++){
        //iterate through each block
        C = MatrixXf::Zero(q, q);
        for (int j = 0; j < q; j++){
            X = remove_col(remove_row(Sigma, j), j);
            s = remove_row(S, j);
            a = remove_row(A, j);
            b = remove_row(B, j);

            MatrixXf sigma_j, beta;
            sigma_j = optimize_block_coord_sigma(X, s, maxiter, a, b, lambda, gamma);
            beta = optimize_block_coord_beta(X, s, maxiter, a, b, lambda, gamma);
            for (i = 0; i < S.rows(); i++){
                if (i != j) {
                    Sigma(i, j) = sigma_j(i, 0);
                    Sigma(j, i) = sigma_j(i, 0);
                }
            }
            //store beta
            for (i = 0; i < C.rows(); i++){
                if (i != j){
                    C(i, j) = beta(i, 0);
                }
            }
        }
        objVals(iter) = cost_theta(S, Beta, Sigma.inverse(), lambda, gamma);
        //check for convergence
        if (iter >= 10 && abs(objVals(iter)-objVals(iter-1))/abs(objVals(iter-1)) < tol)
        {break;}

    }

    //calculate inverse covariance
    Theta = MatrixXf::Zero(q, q);
    for (int j = 0; j < q; j++){
        MatrixXf removed_Sigma = remove_row(Sigma, j);
        MatrixXf removed_C = remove_row(C, j);
        Theta(j, j) = 1/(Sigma(j, j) +
               (removed_Sigma.col(j).transpose() * removed_C.col(j))(0, 0));
        change_theta(C, Theta, j);
        symmetrize(Theta, j);
    }

};


//eigen shits up the compiler outside of optimized builds
//all this stuff is compatible with eigen's shit

#define EIGEN_VECTORIZE_SSE4_2

#include <complex>
#include <iostream>
#include <cstdlib>
#include <vector>
#include <memory>
#include <utility>
#include <tuple>
#include <array>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/LU>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>

using namespace std;
using namespace Eigen;

#define todo
#include "linear_algebra_header.h"

using namespace std;
using namespace Eigen;

template<int num, class type> Matrix<type, num, num> eigen_read_matrix(const type* d) {
    Matrix<type, num, num> res;
    for (int y=0;y<num;++y) {
        for (int x=0;x<num;++x) {
            res(y, x)=d[y*num+x];
        }
    }
    return res;
}

template<int num, class type, class matrix> void eigen_assign_matrix(type* out, const matrix& d) {
    for (int y=0;y<num;++y) {
        for (int x=0;x<num;++x) {
            out[y*num+x]=d(y, x);
        }
    }
}

template<class type> Quaternion<type> eigen_read_quaternion(const type* d) {
    return Quaternion<type>(d[0], d[1], d[2], d[3]);
}

template<class type, class quaternion> void eigen_assign_quaternion(type* out, const quaternion& d) {
    out[0]=d.w();
    out[1]=d.x();
    out[2]=d.y();
    out[3]=d.z();
}

template<class type1, class type2, unsigned long size> array<type2, size> solve_least_squares_impl(
    const vector<array<type1, size>>& A,
    const vector<type2>& b
) {
    assert(A.size()==b.size());

    //Matrix<type2, Dynamic, Dynamic> A_matrix(b.size(), size); todo //slow
    Matrix<type1, Dynamic, size> A_matrix(b.size(), size);
    Matrix<type2, Dynamic, 1> b_matrix(b.size());

    for (int i=0;i<b.size();++i) {
        for (int j=0;j<size;++j) {
            A_matrix(i, j)=A[i][j];
        }
        b_matrix[i]=b[i];
    }

    Matrix<type2, size, 1> x_matrix=
        (A_matrix.transpose() * A_matrix).ldlt().solve(A_matrix.transpose() * b_matrix)
    ;

    /*todo //slow
    Matrix<type2, size, 1> x_matrix=
        A_matrix.bdcSvd(ComputeThinU | ComputeThinV).solve(b_matrix)
    ;*/

    /*Matrix<type2, Dynamic, 1> error=A_matrix*x_matrix-b_matrix;
    cerr << "Error: ";
    for (int i=0;i<size;++i) {
        cerr << error[i] << ", ";
    }
    cerr << "\n";*/

    array<type2, size> res;
    for (int i=0;i<size;++i) {
        res[i]=x_matrix[i];
    }
    return res;
}

template<class type> void solve_equations_impl(
    const vector<tuple<int, int, type>>& A,
    const vector<type>& b,
    vector<type>& x
) {
    const type error_scale=1e-5f;

    vector<Triplet<type>> triplets;
    for (tuple<int, int, type> c : A) {
        triplets.emplace_back(get<0>(c), get<1>(c), get<2>(c));
    }

    SparseMatrix<type> A_matrix(b.size(), b.size());
    Matrix<type, Dynamic, 1> b_matrix(b.size());
    Matrix<type, Dynamic, 1> x_matrix(b.size());

    for (int i=0;i<b.size();++i) {
        b_matrix[i]=b[i];
    }

    A_matrix.setFromTriplets(triplets.begin(), triplets.end());

    SparseLU<SparseMatrix<type>> solver;

    auto check_error=[&](int n) {
        if (solver.info()!=0) {
            cerr << "solve_equations_impl error: " <<  n << ", " << solver.info() << ", " << solver.lastErrorMessage() << "\n";
        }
    };

    solver.analyzePattern(A_matrix);
    solver.factorize(A_matrix); check_error(0);
    x_matrix=solver.solve(b_matrix);

    x.clear();
    for (int i=0;i<b.size();++i) {
        x.push_back(x_matrix[i]);
    }

    //
    //

    auto rand_11=[&]() -> type {
        return type(std::rand())*type(2.0/RAND_MAX)-type(1);
    };

    Matrix<type, Dynamic, 1> x_matrix_new=x_matrix;
    type error_amount=error_scale*x_matrix.norm()/sqrt(b.size());
    for (int i=0;i<b.size();++i) {
        x_matrix_new[i]+=rand_11()*error_amount;
    }
    type x_error=(x_matrix_new-x_matrix).norm()/x_matrix.norm();

    Matrix<type, Dynamic, 1> b_matrix_new=A_matrix*x_matrix_new;
    type b_error=(b_matrix_new-b_matrix).norm()/b_matrix.norm();

    cerr << "solve_equations " <<
        "x_error: " << x_error << ", b_error: " << b_error << " " <<
        "x/b: " << x_error/b_error << " " <<
    "\n";
}

namespace linear_algebra {
    void solve_equations(
        const vector<tuple<int, int, complex<double>>>& A,
        const vector<complex<double>>& b,
        vector<complex<double>>& x
    ) {
        solve_equations_impl(A, b, x);
    }

    array<complex<double>, 3> solve_least_squares(
        const vector<array<double, 3>>& A,
        const vector<complex<double>>& b
    ) {
        return solve_least_squares_impl(A, b);
    }

    array<complex<double>, 10> solve_least_squares(
        const vector<array<double, 10>>& A,
        const vector<complex<double>>& b
    ) {
        return solve_least_squares_impl(A, b);
    }

    void eigen_to_rotation_matrix(float* out, const float* q) {
        eigen_assign_matrix<3>(out, eigen_read_quaternion(q).toRotationMatrix());
    }

    void eigen_to_rotation_matrix(double* out, const double* q) {
        eigen_assign_matrix<3>(out, eigen_read_quaternion(q).toRotationMatrix());
    }

    void eigen_from_rotation_matrix(float* out, const float* m) {
        eigen_assign_quaternion(out, Quaternion<float>(eigen_read_matrix<3>(m)));
    }

    void eigen_from_rotation_matrix(double* out, const double* m) {
        eigen_assign_quaternion(out, Quaternion<double>(eigen_read_matrix<3>(m)));
    }

    void eigen_inverse_2(float* out, const float* m) {
        eigen_assign_matrix<2>(out, eigen_read_matrix<2>(m).inverse());
    }

    void eigen_inverse_2(double* out, const double* m) {
        eigen_assign_matrix<2>(out, eigen_read_matrix<2>(m).inverse());
    }

    void eigen_inverse_3(float* out, const float* m) {
        eigen_assign_matrix<3>(out, eigen_read_matrix<3>(m).inverse());
    }

    void eigen_inverse_3(double* out, const double* m) {
        eigen_assign_matrix<3>(out, eigen_read_matrix<3>(m).inverse());
    }

    void eigen_inverse_2(complex<float>* out, const complex<float>* m) {
        eigen_assign_matrix<2>(out, eigen_read_matrix<2>(m).inverse());
    }

    void eigen_inverse_2(complex<double>* out, const complex<double>* m) {
        eigen_assign_matrix<2>(out, eigen_read_matrix<2>(m).inverse());
    }

    void eigen_inverse_3(complex<float>* out, const complex<float>* m) {
        eigen_assign_matrix<3>(out, eigen_read_matrix<3>(m).inverse());
    }

    void eigen_inverse_3(complex<double>* out, const complex<double>* m) {
        eigen_assign_matrix<3>(out, eigen_read_matrix<3>(m).inverse());
    }
}


/***************************************************************************
 *  @file       lcs_with_one_table.hpp
 *  @author     alan.w
 *  @date       09  August 2014
 *  @remark     CLRS Algorithms implementation, using C++ templates.
 ***************************************************************************/

#ifndef LCS_WITH_ONE_TABLE_HPP
#define LCS_WITH_ONE_TABLE_HPP
//!
//! ex15.4-2
//! Give pseudocode to reconstruct an LCS from the completed c table and the original
//! sequences X and Y in O(m + n) time, without using the b table.
//!
#include <functional>
#include <memory>
#include "matrix.hpp"
#include "color.hpp"

namespace ch15 {

/**
 * @brief The LcsWithOneTable class
 *
 * for ex15.4-2
 */
template<typename Range>
class LcsWithOneTable
{
public:
    using SizeType  =   typename Range::size_type;
    using Pointer   =   const Range*;
    using sPointer  =   std::shared_ptr<Range>;
    using MatrixType=   ch15::Matrix<SizeType>;

    //! Ctor
    LcsWithOneTable(const Range& l, const Range& r):
        lhs(&l),
        rhs(&r),
        maze(l.size() + 1, r.size() + 1, 0)
    {
        build_maze();
    }

    /**
     * @brief print_maze
     */
    void print_maze()const
    {
        ch15::print(maze);
    }

    /**
     * @brief generate
     */
    sPointer generate() const
    {
        assert(lhs && rhs);
        sPointer lcs = std::make_shared<Range>();

        //! lambda to do the real work
        using Lambda = std::function<void(SizeType, SizeType)>;
        Lambda build_lcs = [&lcs, &build_lcs, this](SizeType l, SizeType r)
        {
            //! stop condition
            if(l == 0 || r == 0)    return;

            //! recur
            if((*lhs)[l - 1]    ==  (*rhs)[r - 1])
            {
                build_lcs(l - 1, r - 1);
                lcs->push_back((*lhs)[l - 1]);
            }
            else if (maze(l - 1, r) >= maze(l, r - 1))
                build_lcs(l - 1, r);
            else
                build_lcs(l, r - 1);
        };

        //! call the lambda
        build_lcs(lhs->size(),rhs->size());
        return lcs;
    }

private:
    Pointer lhs;
    Pointer rhs;
    MatrixType maze;

    /**
     * @brief build_maze
     *
     * @complx  O(m + n)
     * for ex15.4-2
     */
    void build_maze()
    {
        for(SizeType l = 1; l != maze.size1(); ++l)
            for(SizeType r = 1; r != maze.size2(); ++r)
            {
                if((*lhs)[l - 1]    ==  (*rhs)[r - 1])
                    maze(l,r)   =   maze(l - 1, r - 1) + 1;
                else if(maze(l - 1, r) >= maze(l, r - 1))
                    maze(l,r)   =   maze(l - 1, r);
                else
                    maze(l,r)   =   maze(l, r - 1);
            }
    }
};

}//namespace
#endif // LCS_WITH_ONE_TABLE_HPP

//! @test   for ex15.4-2
//!
//#include <iostream>
//#include <boost/numeric/ublas/io.hpp>
//#include "lcs_with_one_table.hpp"
//#include "color.hpp"

//int main()
//{
//    std::string lhs = "ABCBDAB";
//    std::string rhs = "BDCABA";

//    using LCS   =   ch15::LcsWithOneTable<std::string>;
//    LCS lcs(lhs, rhs);
//    lcs.print_maze();

//    auto sequence = lcs.generate();
//    std::cout << "The longest common sequence = ";
//    std::cout << color::yellow(*sequence) << std::endl;

//    std::cout << color::red("\nend\n");
//    return 0;
//}

//! @output:
//!
//0 0 0 0 0 0 0

//0 0 0 0 1 1 1

//0 1 1 1 1 2 2

//0 1 1 2 2 2 2

//0 1 1 2 2 3 3

//0 1 2 2 2 3 3

//0 1 2 2 3 3 4

//0 1 2 2 3 4 4

//The longest common sequence = BCBA


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2021: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  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 NON INFRINGEMENT. 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.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/geometry/vec_mat_utils.h>
#include <cinolib/clamp.h>
#include <cinolib/deg_rad.h>
#include <iostream>
#include <cmath>
#include <cassert>
#include <Eigen/Dense>

namespace cinolib
{

template<unsigned int d, typename T>
CINO_INLINE
void vec_set(T * v, const std::initializer_list<T> & il)
{
    assert(il.size()==d);
    auto it = il.begin();
    for(unsigned int i=0; i<d; ++i,++it) v[i] = *it;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void vec_set_dense(T * vec, const T val)
{
    for(unsigned int i=0; i<d; ++i)
    {
        vec[i] = val;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// v2 = v0 + v1
template<unsigned int d, typename T>
CINO_INLINE
void vec_plus(const T * v0, const T * v1, T * v2)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v2[i] = v0[i] + v1[i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// v2 = v0 - v1
template<unsigned int d, typename T>
CINO_INLINE
void vec_minus(const T * v0, const T * v1, T * v2)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v2[i] = v0[i] - v1[i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// v = -v
template<unsigned int d, typename T>
CINO_INLINE
void vec_minus(const T * v0, T * v1)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v1[i] = -v0[i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// v1 = v0 * val (element-wise)
template<unsigned int d, typename T>
CINO_INLINE
void vec_times(const T * v0, const T val, T * v1)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v1[i] = v0[i] * val;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// v1 = v0 / val (element-wise)
template<unsigned int d, typename T>
CINO_INLINE
void vec_divide(const T * v0, const T val, T * v1)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v1[i] = v0[i] / val;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if v0 == v1 (element-wise)
template<unsigned int d, typename T>
CINO_INLINE
bool vec_equals(const T * v0, const T * v1)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(v0[i] != v1[i]) return false;
    }
    return true;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if v0 < v1 (element-wise)
template<unsigned int d, typename T>
CINO_INLINE
bool vec_less(const T * v0, const T * v1)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(v0[i] < v1[i]) return true;
        if(v0[i] > v1[i]) return false;
    }
    return false;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T vec_dot(const T * v0, const T * v1)
{
    T res = 0;
    for(unsigned int i=0; i<d; ++i)
    {
        res += v0[i]*v1[i];
    }
    return res;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<typename T>
CINO_INLINE
void vec_cross(const T * v0, const T * v1, T * v2)
{
    v2[0] = v0[1] * v1[2] - v0[2] * v1[1];
    v2[1] = v0[2] * v1[0] - v0[0] * v1[2];
    v2[2] = v0[0] * v1[1] - v0[1] * v1[0];
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T vec_angle_deg(const T * v0, const T * v1, const bool normalize)
{
    return (T)to_deg((double)vec_angle_rad<d,T>(v0,v1,normalize));
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T vec_angle_rad(const T * v0, const T * v1, const bool normalize)
{
    T dot;
    if(normalize)
    {
        // normalize input vecs if they are not known to be BOTH already normal
        T tmp0[d], tmp1[d];
        vec_copy<d,T>(v0, tmp0);
        vec_copy<d,T>(v1, tmp1);
        vec_normalize<d,T>(tmp0);
        vec_normalize<d,T>(tmp1);
        if(vec_is_deg<d,T>(tmp0) || vec_is_deg<d,T>(tmp1))
        {
            return std::numeric_limits<T>::infinity();
        }
        dot = vec_dot<d,T>(tmp0,tmp1);
    }
    else
    {
        dot = vec_dot<d,T>(v0,v1);
    }
    return acos(clamp(dot,T(-1),T(1)));
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// vector distance (in the L2 sense)
template<unsigned int d, typename T>
CINO_INLINE
double vec_dist_sqrd(const T * v_0, const T * v_1)
{
    double res = 0.0;
    for(unsigned int i=0; i<d; ++i)
    {
        T tmp = v_0[i] - v_1[i];
        res += tmp*tmp;
    }
    return res;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// L2 vector norm
template<unsigned int d, typename T>
CINO_INLINE
double vec_norm_sqrd(const T * v)
{
    double res = 0.0;
    for(unsigned int i=0; i<d; ++i)
    {
        res += v[i]*v[i];
    }
    return res;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// vector distance (in the L2 sense)
template<unsigned int d, typename T>
CINO_INLINE
double vec_dist(const T * v_0, const T * v_1)
{
    return sqrt(vec_dist_sqrd<d,T>(v_0,v_1));
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// L2 vector norm
template<unsigned int d, typename T>
CINO_INLINE
double vec_norm(const T * v)
{
    return sqrt(vec_norm_sqrd<d,T>(v));
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// Lp vector norm
template<unsigned int d, typename T>
CINO_INLINE
double vec_norm_p(const T * v, const float p)
{
    double res = 0.0;
    for(unsigned int i=0; i<d; ++i)
    {
        res += std::pow(std::fabs(v[i]),p);
    }
    res = std::pow(res,1.0/p);
    return res;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
double vec_normalize(T * v)
{
    double n = vec_norm<d,T>(v);
    if(vec_is_deg<d,T>(v)) return -1;
    vec_divide<d,T>(v,n,v);
    return n;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// element-wise min
template<unsigned int d, typename T>
CINO_INLINE
void vec_min(const T * v0, const T * v1, T * v2)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v2[i] = std::min(v0[i], v1[i]);
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// element-wise max
template<unsigned int d, typename T>
CINO_INLINE
void vec_max(const T * v0, const T * v1, T * v2)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v2[i] = std::max(v0[i], v1[i]);
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T vec_min_entry(const T * v)
{
    return *std::min_element(v,v+d);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T vec_max_entry(const T * v)
{
    return *std::max_element(v,v+d);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// element-wise clamp
template<unsigned int d, typename T>
CINO_INLINE
void vec_clamp(T * v, const T min, const T max)
{
    for(unsigned int i=0; i<d; ++i)
    {
        v[i] = clamp(v[i], min, max);
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void vec_swap(T * v, const unsigned int i, const unsigned int j)
{
    assert(i<d && j<d);
    std::swap(v[i],v[j]);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if all entries are zero
template<unsigned int d, typename T>
CINO_INLINE
bool vec_is_null(const T * v)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(v[i] !=0) return false;
    }
    return true;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if at least one entry is NaN
template<unsigned int d, typename T>
CINO_INLINE
bool vec_is_nan(const T * v)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(std::isnan(v[i])) return true;
    }
    return false;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if at least one entry is inf
template<unsigned int d, typename T>
CINO_INLINE
bool vec_is_inf(const T * v)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(std::isinf(v[i])) return true;
    }
    return false;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if at least one entry is NaN or Inf, or all entries are zero
template<unsigned int d, typename T>
CINO_INLINE
bool vec_is_deg(const T * v)
{
    for(unsigned int i=0; i<d; ++i)
    {
        if(std::isinf(v[i]) || std::isnan(v[i])) return true;
    }
    return vec_is_null<d,T>(v);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// true if all entries are null, normal or subnormal
template<unsigned int d, typename T>
CINO_INLINE
bool vec_is_finite(const T* v)
{
    for (std::size_t i = 0; i < d; ++i)
    {
        if (!std::isfinite(v[i])) return false;
    }
    return true;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void vec_copy(const T * v0, T * v1)
{
    std::copy(v0, v0+d, v1);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void vec_print(const T * v)
{
    for(unsigned int i=0; i<d; ++i)
    {
        std::cout << v[i] << " ";
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_set(T m[][c], const std::initializer_list<T> & il)
{
    assert(il.size()==r*c);
    auto it = il.begin();
    for(unsigned int i=0; i<r; ++i)
    for(unsigned int j=0; j<c; ++j)
    {
        m[i][j] = *it;
        ++it;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// initialize diagonal matrix
template<unsigned int d, typename T>
CINO_INLINE
void mat_set_diag(T m[][d], const T val)
{
    for(unsigned int i=0; i<d; ++i)
    for(unsigned int j=0; j<d; ++j)
    {
        m[i][j] = (i==j) ? val : 0;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// initialize diagonal matrix
template<unsigned int d, typename T>
CINO_INLINE
void mat_set_diag(T m[][d], const T diag[])
{
    for(unsigned int i=0; i<d; ++i)
    for(unsigned int j=0; j<d; ++j)
    {
        m[i][j] = (i==j) ? diag[i] : 0;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_set_row(T m[][c], const unsigned int i, const T row[])
{
    assert(i<r);
    for(unsigned int j=0; j<c; ++j)
    {
        m[i][j] = row[j];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_set_col(T m[][c], const unsigned int i, const T col[])
{
    assert(i<c);
    for(unsigned int j=0; j<r; ++j)
    {
        m[j][i] = col[j];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// initialize 2D rotation matrix
template<unsigned int d,typename T>
CINO_INLINE
void mat_set_rot_2d(T m[][d], const T ang_rad)
{
    assert(d==2 || d==3);
    if(d==3) mat_set_diag<d,T>(m,1); // for transformations in homogeneous coordinates

    T rcos  = (T)cos(ang_rad);
    T rsin  = (T)sin(ang_rad);
    m[0][0] =  rcos;
    m[1][0] =  rsin;
    m[0][1] = -rsin;
    m[1][1] =  rcos;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// initialize 3D rotation matrix
template<unsigned int d,typename T>
CINO_INLINE
void mat_set_rot_3d(T m[][d], const T ang_rad, const T axis[])
{
    assert(d==3 || d==4);
    if(d==4) mat_set_diag<d,T>(m, 1); // for transformations in homogeneous coordinates

    T u     = axis[0];
    T v     = axis[1];
    T w     = axis[2];
    T rcos  = (T)cos(ang_rad);
    T rsin  = (T)sin(ang_rad);
    m[0][0] =      rcos + u*u*(1-rcos);
    m[1][0] =  w * rsin + v*u*(1-rcos);
    m[2][0] = -v * rsin + w*u*(1-rcos);
    m[0][1] = -w * rsin + u*v*(1-rcos);
    m[1][1] =      rcos + v*v*(1-rcos);
    m[2][1] =  u * rsin + w*v*(1-rcos);
    m[0][2] =  v * rsin + u*w*(1-rcos);
    m[1][2] = -u * rsin + v*w*(1-rcos);
    m[2][2] =      rcos + w*w*(1-rcos);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// initialize translation matrix
// NOTE: translation is a non linear operation.
// Homogeneous coordinates are assumed here, hence tx is supposed to be a (d-1) vector
template<unsigned int d, typename T>
CINO_INLINE
void mat_set_trans(T m[][d], const T tx[])
{
    mat_set_diag<d,T>(m,1);
    for(unsigned int i=0; i<d-1; ++i)
    {
        m[i][d-1] = tx[i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_swap(T m[][c], const unsigned int i, const unsigned int j, const unsigned int k, const unsigned int l)
{
    assert(i<r && j<c && k<r && l<c);
    std::swap(m[i][j], m[k][l]);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// copy ith column
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_col(const T m[][c], const unsigned int i, T col[])
{
    assert(i<c);
    for(unsigned int j=0; j<r; ++j)
    {
        col[j] = m[j][i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// copy ith row
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_row(const T m[][c], const unsigned int i, T row[])
{
    assert(i<r);
    std::copy(m[i], m[i]+c, row);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// copy diagonal
template<unsigned int d, typename T>
CINO_INLINE
void mat_diag(const T m[][d], T diag[])
{
    for(unsigned int i=0; i<d; ++i)
    {
        diag[i] = m[i][i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// vector of pointers to elements in the ith column
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_col_ptr(const T m[][c], const unsigned int i, T * col[])
{
    assert(i<c);
    for(unsigned int j=0; j<r; ++j)
    {
        col[j] = & m[j][i];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// vector of pointers to elements in the ith row
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_row_ptr(const T m[][c], const unsigned int i, T * row[])
{
    assert(i<r);
    for(unsigned int j=0; j<c; ++j)
    {
        row[j] = & m[i][j];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// vector of pointers to elements in the diagonal
template<unsigned int d, typename T>
CINO_INLINE
void mat_diag_ptr(const T m[][d], T * diag[])
{
    for(unsigned int i=0; i<d; ++i)
    for(unsigned int j=0; j<d; ++j)
    {
        diag[i] = & m[i][j];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
bool mat_is_symmetric(const T m[][d])
{
    for(unsigned int i=0;   i<d-1; ++i)
    for(unsigned int j=i+1; j<d;   ++j)
    {
        if(m[i][j]!=m[j][i]) return false;
    }
    return true;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T mat_trace(const T m[][d])
{
    T res = 0;
    for(unsigned int i=0; i<d; ++i)
    {
        res += m[i][i];
    }
    return res;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
T mat_det(const T m[][d])
{
    switch(d)
    {
        case 2: return mat_det22<T>(m[0][0], m[0][1],
                                    m[1][0], m[1][1]);

        case 3: return mat_det33<T>(m[0][0], m[0][1], m[0][2],
                                    m[1][0], m[1][1], m[1][2],
                                    m[2][0], m[2][1], m[2][2]);

        default:
        {
            typedef Eigen::Matrix<T,d,d,Eigen::RowMajor> M;
            Eigen::Map<const M> tmp(m[0]);
            return tmp.determinant();
        }
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<typename T>
CINO_INLINE
T mat_det22(const T m00, const T m01,
            const T m10, const T m11)
{
    return m00*m11 - m10*m01;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<typename T>
CINO_INLINE
T mat_det33(const T m00, const T m01, const T m02,
            const T m10, const T m11, const T m12,
            const T m20, const T m21, const T m22)
{
    return m00 * mat_det22(m11, m12, m21, m22) -
           m01 * mat_det22(m10, m12, m20, m22) +
           m02 * mat_det22(m10, m11, m20, m21);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_transpose(const T m[][c], T tr[][r])
{
    for(unsigned int i=0; i<r; ++i)
    for(unsigned int j=0; j<c; ++j)
    {
        tr[j][i] = m[i][j];
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void mat_inverse(const T m[][d], T in[][d])
{
    switch(d)
    {
        case 2:
        {
            // https://mathworld.wolfram.com/MatrixInverse.html
            // https://www.mathsisfun.com/algebra/matrix-inverse.html
            double one_over_det = 1.0 / mat_det(m);
            in[0][0] =  m[1][1] * one_over_det;
            in[0][1] = -m[0][1] * one_over_det;
            in[1][0] = -m[1][0] * one_over_det;
            in[1][1] =  m[0][0] * one_over_det;
        }

        default: // up to 4x4 matrices with Eigen
        {
            typedef Eigen::Matrix<T,d,d,Eigen::RowMajor> M;
            Eigen::Map<const M> tmp_m(m[0]);
            Eigen::Map<M> tmp_in(in[0]);
            tmp_in = tmp_m.inverse();
        }
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// matrix eigen values and eigenvectors (from lowest to highest)
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_eigendec(const T m[][c], T eval[], T evec[][c])
{
    assert(r==c);

    if(r==2)
    {
        // http://legacy-www.math.harvard.edu/archive/21b_fall_04/exhibits/2dmatrices/index.html
        T trace = mat_trace<r,T>(m);
        T det   = mat_det<r,T>(m);
        eval[0] = trace*0.5 - sqrt((trace*trace)*0.25 - det);
        eval[1] = trace*0.5 + sqrt((trace*trace)*0.25 - det);

        T e0[c];
        T e1[c];
        if(m[1][0]!=0)
        {
            vec_set<r,T>(e0, { eval[0]-m[1][1], m[1][0] });
            vec_set<r,T>(e1, { eval[1]-m[1][1], m[1][0] });
        }
        else if(m[0][1]!=0)
        {
            vec_set<r,T>(e0, { m[0][1], eval[0]-m[0][0] });
            vec_set<r,T>(e1, { m[0][1], eval[1]-m[0][0] });
        }
        else
        {
            vec_set<r,T>(e0, { 0, 1 });
            vec_set<r,T>(e1, { 1, 0 });
        }
        vec_normalize<r,T>(e0);
        vec_normalize<r,T>(e1);
        mat_set_col<r,c,T>(evec, 0, e0);
        mat_set_col<r,c,T>(evec, 1, e1);
    }
    else
    {
        if(mat_is_symmetric(m))
        {
            // eigen decomposition for self-adjoint (i.e. real valued symmetric) matrices
            //  - guaranteed real valued eigen values and vectors
            //  - faster and more precise than Eigen::EigenSolver
            typedef Eigen::Matrix<T,r,c,Eigen::RowMajor> M;
            Eigen::Map<const M> tmp(m[0]);
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix<T,r,c>> eig(tmp);
            assert(eig.info() == Eigen::Success);
            vec_copy<r*c,T>(eig.eigenvectors().data(), evec[0]);
            vec_copy<r,T>(eig.eigenvalues().data(),  eval);
            std::cout << eig.eigenvectors() << std::endl;
            std::cout << eig.eigenvalues() << std::endl;
        }
        else
        {
            // eigen decomposition for general matrices
            typedef Eigen::Matrix<T,r,c,Eigen::RowMajor> M;
            Eigen::Map<const M> tmp(m[0]);
            Eigen::EigenSolver<Eigen::Matrix<T,r,c>> eig(tmp);
            assert(eig.info() == Eigen::Success);
            for(unsigned int i=0; i<r; ++i)
            {
                for(unsigned int j=0; j<r; ++j)
                {
                    // WARNING: I am taking only the real part!
                    evec[j][i] = eig.eigenvectors()(j,i).real();
                }
                eval[i] = eig.eigenvalues()[i].real();
            }
        }
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

// matrix eigen values (from highest to lowest)
template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_eigenval(const T m[][c], T eval[])
{
    T evec[r][c];
    mat_eigendec<r,c,T>(m, eval, evec);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_eigenvec(const T m[][c], T evec[][c])
{
    T eval[2];
    mat_eigendec<r,c,T>(m, eval, evec);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_svd(const T m[][c], T U[][r], T S[], T V[][c])
{
//    // https://lucidar.me/en/mathematics/singular-value-decomposition-of-a-2x2-matrix/
//    if(r==2 && c==2)
//    {
//        // singular values (S)
//        {
//            T m00_2 = m[0][0]*m[0][0];
//            T m01_2 = m[0][1]*m[0][1];
//            T m10_2 = m[1][0]*m[1][0];
//            T m11_2 = m[1][1]*m[1][1];
//            T s1    = m00_2 + m01_2 + m10_2 + m11_2;
//            T s2    = sqrt(std::pow(m00_2 + m01_2 - m10_2 - m11_2,2) + 4*std::pow(m[0][0]*m[1][0] + m[0][1]*m[1][1],2));
//            S[0]    = sqrt((s1+s2)*0.5);
//            S[1]    = sqrt((s1-s2)*0.5);
//        }
//        // orthogonal matrices (U,V)
//        {
//            double theta     = 0.5 * atan2(2.0*m[0][0]*m[1][0]+2.0*m[0][1]*m[1][1], m[0][0]*m[0][0] + m[0][1]*m[0][1] - m[1][0]*m[1][0] - m[1][1]*m[1][1]);
//            double phi       = 0.5 * atan2(2.0*m[0][0]*m[0][1]+2.0*m[1][0]*m[1][1], m[0][0]*m[0][0] - m[0][1]*m[0][1] + m[1][0]*m[1][0] - m[1][1]*m[1][1]);
//            double cos_theta = cos(theta);
//            double sin_theta = cos(theta);
//            double cos_phi   = cos(phi);
//            double sin_phi   = cos(phi);
//            int sign1        = ((m[0][0]*cos_theta + m[1][0]*sin_theta)*cos_phi + ( m[0][1]*cos_theta + m[1][1]*sin_theta)*sin_phi > 0) ? +1 : -1;
//            int sign2        = ((m[0][0]*sin_theta - m[1][0]*cos_theta)*sin_phi + (-m[0][1]*sin_theta + m[1][1]*cos_theta)*cos_phi > 0) ? +1 : -1;
//            mat_set_rot_2d<r,T>(U, theta);
//            mat_set_rot_2d<r,T>(V, phi);
//            V[0][0] *= sign1;
//            V[1][0] *= sign1;
//            V[0][1] *= sign2;
//            V[1][1] *= sign2;
//        }
//    }
//    else
    {
        typedef Eigen::Matrix<T,r,c,Eigen::RowMajor> M;
        Eigen::Map<const M> tmp(m[0]);
        Eigen::JacobiSVD<M> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
        vec_copy<r*r,T>(svd.matrixU().data(), U[0]);
        vec_copy<c*c,T>(svd.matrixV().data(), V[0]);
        if(r<c) vec_copy<r>(svd.singularValues().data(), S);
        else    vec_copy<c>(svd.singularValues().data(), S);
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_ssvd(const T m[][c], T U[][r], T S[], T V[][c])
{
    mat_svd<r,c,T>(m,U,S,V);

    if(r==2 && c==2)
    {
        if(mat_det<c,T>(V)<0)
        {
            V[0][1] = -V[0][1];
            V[1][1] = -V[1][1];
            U[0][1] = -U[0][1];
            U[1][1] = -U[1][1];
            assert((mat_det<c,T>(V)>0));
        }

        if(mat_det<r,T>(U)<0)
        {
            U[0][1] = -U[0][1];
            U[1][1] = -U[1][1];
            S[1]    = -S[1];
            assert((mat_det<r,T>(U)>0));
        }
    }
    else if(r==3 && c==3)
    {
        if(mat_det<c,T>(V)<0)
        {
            V[0][2] = -V[0][2];
            V[1][2] = -V[1][2];
            V[2][2] = -V[2][2];
            U[0][2] = -U[0][2];
            U[1][2] = -U[1][2];
            U[2][2] = -U[2][2];
            assert((mat_det<c,T>(V)>0));
        }

        if(mat_det<r,T>(U)<0)
        {
            U[0][2] = -U[0][2];
            U[1][2] = -U[1][2];
            U[2][2] = -U[2][2];
            S[2]    = -S[2];
            assert((mat_det<c,T>(U)>0));
        }
    }
    else assert(false && "mat_ssvd: unsupported matrix size");
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int d, typename T>
CINO_INLINE
void mat_solve_Cramer(const T m[][d], const T b[], T x[])
{
    T det = mat_det<d,T>(m);
    if(det==0)
    {
        vec_set_dense<d,T>(x,0);
        return;
    }
    for(unsigned int i=0; i<d; ++i)
    {
        T m_i[d][d];
        mat_copy<d,d,T>(m, m_i);
        mat_set_col<d,d,T>(m_i, i, b);
        x[i] = mat_det<d,T>(m_i) / det;
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_copy(const T m1[][c], T m2[][c])
{
    std::copy(m1[0], m1[0]+(r*c), m2[0]);
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r, unsigned int c, typename T>
CINO_INLINE
void mat_print(const T m[][c])
{
    for(unsigned int i=0; i<r; ++i)
    for(unsigned int j=0; j<c; ++j)
    {
        if(i>0 && j%c==0) std::cout << "\n";
        std::cout << m[i][j] << " ";
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<typename T>
CINO_INLINE
vec3<T> operator*(const mat4<T>& mat, const vec3<T>& vec)
{
    vec4<T> prod(mat * vec.add_coord(1));
    prod /= prod[3];
    return prod.rem_coord();
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<unsigned int r0, unsigned int c0, unsigned int c1, typename T>
CINO_INLINE
void mat_times(const T m0[][c0], const T m1[][c1], T m2[][c1])
{
    for(unsigned int i=0; i<r0; ++i)
    for(unsigned int j=0; j<c1; ++j)
    {
        m2[i][j] = 0;
        for(unsigned int k=0; k<c0; ++k)
        {
            m2[i][j] += m0[i][k] * m1[k][j];
        }
    }
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

}


#ifndef STAN_MATH_PRIM_FUN_RISING_FACTORIAL_HPP
#define STAN_MATH_PRIM_FUN_RISING_FACTORIAL_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/boost_policy.hpp>
#include <boost/math/special_functions/factorials.hpp>

namespace stan {
namespace math {

/**
 * Return the rising factorial function evaluated
 * at the inputs.
 *
 * @tparam T type of the first argument
 * @param x first argument
 * @param n second argument
 * @return Result of rising factorial function.
 * @throw std::domain_error if x is NaN
 * @throw std::domain_error if n is negative
 *
 \f[
 \mbox{rising\_factorial}(x, n) =
 \begin{cases}
 \textrm{error} & \mbox{if } x \leq 0\\
 x^{(n)} & \mbox{if } x > 0 \textrm{ and } -\infty \leq n \leq \infty \\[6pt]
 \textrm{NaN} & \mbox{if } x = \textrm{NaN or } n = \textrm{NaN}
 \end{cases}
 \f]

 \f[
 \frac{\partial\, \mbox{rising\_factorial}(x, n)}{\partial x} =
 \begin{cases}
 \textrm{error} & \mbox{if } x \leq 0\\
 \frac{\partial\, x^{(n)}}{\partial x} & \mbox{if } x > 0 \textrm{ and } -\infty
 \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } n =
 \textrm{NaN} \end{cases} \f]

 \f[
 \frac{\partial\, \mbox{rising\_factorial}(x, n)}{\partial n} =
 \begin{cases}
 \textrm{error} & \mbox{if } x \leq 0\\
 \frac{\partial\, x^{(n)}}{\partial n} & \mbox{if } x > 0 \textrm{ and } -\infty
 \leq n \leq \infty \\[6pt] \textrm{NaN} & \mbox{if } x = \textrm{NaN or } n =
 \textrm{NaN} \end{cases} \f]

 \f[
 x^{(n)}=\frac{\Gamma(x+n)}{\Gamma(x)}
 \f]

 \f[
 \frac{\partial \, x^{(n)}}{\partial x} = x^{(n)}(\Psi(x+n)-\Psi(x))
 \f]

 \f[
 \frac{\partial \, x^{(n)}}{\partial n} = (x)_n\Psi(x+n)
 \f]
 *
 */
template <typename T>
inline return_type_t<T> rising_factorial(const T& x, int n) {
  static const char* function = "rising_factorial";
  check_not_nan(function, "first argument", x);
  check_nonnegative(function, "second argument", n);
  return boost::math::rising_factorial(x, n, boost_policy_t());
}

}  // namespace math
}  // namespace stan
#endif


#include <iostream>
#include <vector>
#include <set>
#include <map>
#include <algorithm>

using namespace std;

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/find_flow_cost.hpp>

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor,
                boost::property <boost::edge_weight_t, long>>>>> graph;

typedef boost::graph_traits<graph>::edge_descriptor             edge_desc;
typedef boost::graph_traits<graph>::out_edge_iterator           out_edge_it;

class edge_adder {
 graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}
  void add_edge(int from, int to, long capacity, long cost) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    auto w_map = boost::get(boost::edge_weight, G); // new!
    const edge_desc e = boost::add_edge(from, to, G).first;
    const edge_desc rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
    w_map[e] = cost;   // new assign cost
    w_map[rev_e] = -cost;   // new negative cost
  }
};

// Strategy:
// - Build a space-time graph, i.e. a vertex is described by a tuple (i, t) where i is the rental station and t is the time
//   - We add edges for every booking and for parking (stay at the same station until the next time step)
//   - Use Maxflow-Mincost to solve the problem
void solve() {

  // Read input
  int n, m;
  cin >> n >> m;

  vector<int> l(m); // initially available cars
  int sumCars = 0; // How many cars are there in total?
  for (int j = 0; j < m; ++j) {
    cin >> l[j];
    sumCars += l[j];
  }

  // Save the relevant timesteps for each station (a timestep is only relevant if a booking starts or ends at the given station)
  vector<set<int>> timePoints(m);

  vector<int> s(n); // source
  vector<int> t(n); // target
  vector<int> d(n); // departure time
  vector<int> a(n); // arrival time
  vector<int> p(n); // profit
  int tMax = 0;
  for (int i = 0; i < n; ++i) {

    // Read input
    cin >> s[i] >> t[i] >> d[i] >> a[i] >> p[i];
    --s[i]; --t[i]; // convert 1-based to 0-based indices

    // Save relevant time points
    timePoints[s[i]].insert(d[i]);
    timePoints[t[i]].insert(a[i]);
    tMax = max(tMax, a[i]);
  }

  // Time 0 and tMax are relevant for connecting to source and target
  for (int j = 0; j < m; ++j) {
    timePoints[j].insert(0);
    timePoints[j].insert(tMax);
  }

  // For every station:
  // - Assign a vertex id to every time point
  // - Sort relevant time points
  vector<map<int, int>> timeToVertex(m);
  vector<vector<int>> sortedTimePoints(m);
  int id = 0;
  for (int j = 0; j < m; ++j) {
    for (int t : timePoints[j]) {
      timeToVertex[j][t] = id++;
      sortedTimePoints[j].push_back(t);
    }
    sort(sortedTimePoints[j].begin(), sortedTimePoints[j].end());
  }

  // Create graph
  int N = id;
  graph G(N);
  edge_adder adder(G);

  // Connect consecutive time points
  for (int j = 0; j < m; ++j) {
    for (int i = 0; i < sortedTimePoints[j].size() - 1; ++i) {
      int t0 = sortedTimePoints[j][i];
      int t1 = sortedTimePoints[j][i + 1];
      adder.add_edge(timeToVertex[j][t0], timeToVertex[j][t1], sumCars, 100 * (t1 - t0));
    }
  }

  // Add edges for booking requests
  for (int i = 0; i < n; ++i) {
    adder.add_edge(timeToVertex[s[i]][d[i]], timeToVertex[t[i]][a[i]], 1, 100 * (a[i] - d[i]) - p[i]);
  }

  // Connect source and sink
  int source = boost::add_vertex(G);
  int target = boost::add_vertex(G);
  for (int j = 0; j < m; ++j) {
    adder.add_edge(source, timeToVertex[j][0], l[j], 0);
    adder.add_edge(timeToVertex[j][tMax], target, sumCars, 0);
  }

  boost::successive_shortest_path_nonnegative_weights(G, source, target);
  int cost = boost::find_flow_cost(G);
  cost = 100 * sumCars * tMax - cost;

  cout << cost << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t;
  cin >> t;
  while (t--) {
    solve();
  }
  return 0;
}


#include <string>
#include <iostream>
#include <fstream>
#include <stdexcept>
#include <stdio.h>
#include <vector>
#include <utility>
#include <algorithm>
#include <sys/times.h>

//includes from boost librabry
#include <boost/config.hpp>
#include <boost/utility.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/property_map/property_map.hpp>

using namespace std;
using namespace boost;

/** ex5 with timing.
 *
 *  @author Michael Brand
 *  @date 20.06.2017
 *
 *  @version 2.0
 *  Merged ex5_old and ex5_boost
 */

int maxEdgeWeight = 2e9; //*SETUP VARIABLES

/**
*   TIME MEASUREMENT FUNCTIONS
*/
clock_t times(struct tms *buffer);

void start_clock(void);
void end_clock(void);

static clock_t st_time;
static clock_t en_time;
static struct tms st_cpu;
static struct tms en_cpu;

/** Function for time measurement
*   Measuring the time
*/
void
start_clock()
{
    st_time = times(&st_cpu);
}

void
end_clock()
{
    en_time = times(&en_cpu);

    printf("\nREAL TIME: %jd ms,\nUSER TIME %jd ms,\nSYSTEM TIME %jd ms.\n",
        (intmax_t)(en_time - st_time)*10,
        (intmax_t)(en_cpu.tms_utime - st_cpu.tms_utime)*10,
        (intmax_t)(en_cpu.tms_stime - st_cpu.tms_stime))*10;
}

/**
* STRUCTURE FOR OWN IMPLEMENTATION
*/

//* a structure to represent a weighted edge in graph
struct Edge
{
    int src, dest, weight;
};
//* a structure to represent a connected, directed and weighted graph
struct Graph
{
    // V-> Number of vertices, E-> Number of edges
    int V, E;
    // graph is represented as an array of edges.
    struct Edge* edge;
};

//* Creates a graph with V vertices and E edges
struct Graph* createGraph(int V, int E)
{
    struct Graph* graph = (struct Graph*) malloc( sizeof(struct Graph) );
    graph->V = V;
    graph->E = E;

    graph->edge =
       (struct Edge*) malloc( graph->E * sizeof( struct Edge ) );

    return graph;
}

//ALGOS

/** gets longest path value.
   *  @param[in] dist[] int array of edges.
   *  @param[in] n no. vertices.
   *  @return weight of longest path
   */
int getLongestPathValue(int dist[], int n){

  int high = 0;

  for(int i=0; i<n; i++){
    if(dist[i]>high && dist[i]<=maxEdgeWeight) high = dist[i];
  }

  return high;
}

/** gets vertices with longest distance to source.
   *  @param[in] dist[] int array of edges.
   *  @param[in] n no. vertices.
   *  @param[in] val longest path value in graph
   *  @return vector of vertices which have given path value in common
   */
std::vector<int> getLongestPathVertices(int dist[], int n, int val){
  std::vector<int> v;
  for(int i=0; i<n; i++){
    if(dist[i] == val)
      v.push_back(i);
  }
  return v;
}

/** gets vertex of longest way.
   *  @param[in] vertices possible vertices
   *  @param[in] ecounter no. of edges to source
   *  @return vertice with more edges or smaller index if even
   */
int chooseWinner(std::vector<int> vertices,int ecounter[]){

  int winner = vertices[0];

  for(int i=1; i<vertices.size(); i++){
    if(ecounter[vertices[i]] > ecounter[winner]){
      winner = vertices[i];
    }
  }
  return winner;
}

/** BellmanFord main algo
   *  @param[in] graph graph on which BF is executed
   *  @param[in] src source node
   */
void BellmanFord(struct Graph* graph, int src)
{
    int V = graph->V;
    int E = graph->E;
    int dist[V];
    int ecount[V];

    // Initialize distances from src to all other vertices as INFINITE
    for(int i = 0; i < V; i++){
        dist[i]   = INT_MAX;
        ecount[i] = 0;
    }

    dist[src] = 0;

    // Relax all edges |V| - 1 times. A simple shortest
    // path from src to any other vertex can have at-most |V| - 1
    // edges
    for(int i = 1; i <= V-1; i++)
    {
        for (int j = 0; j < E; j++)
        {
            int u = graph->edge[j].src;
            int v = graph->edge[j].dest;
            int weight = graph->edge[j].weight;
            if (dist[u] != INT_MAX && dist[u] + weight < dist[v]){
                dist[v] = dist[u] + weight;
                ecount[v] = ecount[u]+1;
            }

        }
    }

    // check for negative-weight cycles.  The above step
    // guarantees shortest distances if graph doesn't contain
    // negative weight cycle.  If we get a shorter path, then there
    // is a cycle.
    for (int i = 0; i < E; i++)
    {
        int u = graph->edge[i].src;
        int v = graph->edge[i].dest;
        int weight = graph->edge[i].weight;
        if (dist[u] != INT_MAX && dist[u] + weight < dist[v])
            fprintf(stderr, "%s", "Graph contains negative weight cycle");
    }

  int lPathVal = getLongestPathValue(dist,V);

  std::vector<int> vert = getLongestPathVertices(dist, V, lPathVal);

  int win = chooseWinner(vert, ecount);

  fprintf(stdout, "NON-BOOST mode\nRESULT VERTEX %i \nRESULT DIST %i \n", win+1, dist[win]);

    return;
}

/** Main Method
*   runrunrunrunrun
*
*/
int main (int argc, char* argv[]) {

  start_clock(); //*start timer

  //check for right number of arguments
  if( argc != 3){
      fprintf(stderr, "%s", "Call programm as ex5 mode filename.gph, where mode is integer 1(boost) or 2(non-boost)\n");
      exit(EXIT_FAILURE);
  }

  int mode = stoi(argv[1]);

  if(mode!=1 && mode!=2){
    fprintf(stderr, "%s", "mode argument needs to be 1(for boost) or 2(non-boost)\n");
    exit(EXIT_FAILURE);
  }

  if(mode==1){
  //Boost
  ifstream    file(argv[2]);
  string      line;

  if(!file){
    fprintf(stderr, "%s", "Could not open file.\n");
    exit(EXIT_FAILURE);

  }

  //property weighted graph
  typedef property <edge_weight_t, int> EdgeWeightProperty;
  //typedef for graph type: undirected, weighted graph
  typedef adjacency_list <listS, vecS, undirectedS, no_property, EdgeWeightProperty> Graph;
  //typedef to describe vertecies in boost library
  typedef graph_traits <Graph>::vertex_descriptor vertex_descriptor;
  //typedef for edges in the graph
  typedef pair<int, int> Edge;

  //read number of vertices and edges
  getline(file, line, ' ');
  const int numV = stoi(line);
  getline(file, line, '\n');
  const int numE = stoi(line);

  Edge* edges;
  int* weights;

//  Edge edges[numE];     //array containig edge-pairs
//  int  weights[numE];   //array containing corresponding weights
  int  edgecount = 0;

  edges = new Edge[numE];
  weights = new int[numE];

    //loop over all lines in the file
  while( getline(file, line) ){

    stringstream linestream(line);
    string       vertex1, vertex2, weight;

    try{
      getline(linestream, vertex1, ' ');
      getline(linestream, vertex2, ' ');
      edges[edgecount] = Edge(stoi(vertex1), stoi(vertex2));     //store edges as pairs of vertices

      getline(linestream, weight, '\n');
      weights[edgecount] = stoi(weight); //store corresponding weights

    } catch (std::invalid_argument& ia){
      //when data is not a digit,
      //std::stoi throws an invalid argument exception
    } catch ( ... ){}

    edgecount++;

  }//while
  file.close();

  //*create graph containig edges and weights
  Graph g (edges, edges + numE, weights, numV);
  //*setup for shortest path solver
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  std::vector<vertex_descriptor> pre(num_vertices(g));
  //*vector to store distances to source vertex
  std::vector<int> dist(num_vertices(g));
  //*define source vertex to which shortest path shall be
  //*computed from all other vertecies in graph g
  vertex_descriptor source = vertex(edges[0].first, g);

  //*use dijkstra algorithm, since all edges are positive
  dijkstra_shortest_paths(g, source,
                          predecessor_map(make_iterator_property_map(pre.begin(), get(vertex_index, g))).
                          distance_map(make_iterator_property_map(dist.begin(), get(vertex_index, g))));
  int maxdist = 0;
  int vertex  = 0;

  //*iterate over all vertecies and update
  //*maximum disntance, if needed
  graph_traits < Graph >::vertex_iterator vi, vend;
  tie(vi, vend) = vertices(g);
  vi++;
  for (; vi != vend; ++vi) {

    if(dist[*vi] > maxdist){
      maxdist = dist[*vi];
      vertex  = *vi;
    }
  }

  fprintf(stdout, "BOOST mode\nRESULT VERTEX %i \nRESULT DIST %i \n", vertex, maxdist);

  end_clock();
  return 0;

  }

  //OWN BELLMANFORD FROM COMA
  else if(mode==2){
    //nonboost

  //Find shortest path to vertex of index...
  int src=1;

  string line;

  int err=0;

  int lcout = 0;
  int ecount = 0;
  int edgesTot;
  int vertTot;

  struct Graph* graph;

    ifstream f (argv[2]);

    //* Read input file and create graph

  while(getline(f, line)) {

    lcout++;

    stringstream buffer(line);

    int start = 0;
    int end = 0;
    int weight = 0;

    buffer >> start >> end >> weight;

    //FROM EX1
    if(lcout==1){
      //std::cout << "Graph setup" << std::endl;
        graph = createGraph(start, 2*end);
        //fprintf(stdout, "%s %d %s %d %s\n", "Creating undirected Graph with ",start," nodes, ",end," edges.");
        edgesTot = end;
        vertTot = start;
    }
    else{

      if(start < 1 || start>vertTot || end < 1 || end > vertTot){
        fprintf(stderr, "%s \t %d\n", "errorline: " , lcout);
        err++;
        continue;
      }

      if((weight <= 0 || weight>=2000000000 ) && lcout>1 ){
        fprintf(stderr, "%s \t %d\n", "errorline: " , lcout);
        err++;
        continue;
    }

      if(ecount>=2*edgesTot){
        cout << "too many edges " << buffer.str() << endl;
        err++;
        continue;
      }
    // INDEX SWITCH 1->0,...
      graph->edge[ecount].src = int(start-1);
      graph->edge[ecount].dest = int(end-1);
      graph->edge[ecount].weight = weight;

      ecount++;

      graph->edge[ecount].src = int(end-1);
      graph->edge[ecount].dest = int(start-1);
      graph->edge[ecount].weight = weight;

      ecount++;
    }

  }

  f.close();

  if(err>0) fprintf(stderr, "%s \t\t %d\n", "ERRORS" , err);

  if(ecount < 2*edgesTot){
     fprintf(stderr, "%s \t\t %s\n", "INFO", "NOT ENOUGH EDGES DEFINED");
     return -1;
  }

// BELLMAN FORD FROM COMA

  BellmanFord(graph, src-1);

  end_clock(); //* end timing and print stats

  return 0;
  }

}