//Code developed and used to produce the results of
//
//  Rhoads, D., Solé-Ribalta, A., González, M. C., & Borge-Holthoefer, J. (2020).
//  Planning for sustainable Open Streets in pandemic cities.
//  arXiv preprint arXiv:2009.12548.
//
//Please cite the above paper in any works using or derived
//from this software

#define TRUE 1
#define FALSE 0
#define DEBUG 0
#include "mex.h"
#define MAX_COLA 65001
#define MAX_NODES 65000
typedef unsigned long int node_id_type;
#include "predQueueCircStatic.h"
#include <string.h>
#include <math.h>
#include "matrix.h"
#include <boost/heap/fibonacci_heap.hpp>

char outputString[150];

//returns the value of a half-normal PDF with sigma=855 at x
double halfNormalPDF(double x){

    //855 is the sigma that corresponds to a situation where
    //95% of trips fall within a distance of 1667 meters

    return (sqrt(2)/(855*(sqrt(M_PI)))) * exp(-pow(x,2)/(2*pow(855,2)));

}

//returns one row of the estimated OD matrix,
//corresponding to the number of trips from s
//to all other nodes j
void getOdRow(
    node_id_type s,
    double * vStores,
    double * vPop,
    double * vCoords,
    node_id_type sizeSupra,
    double * attraction_vectPr,
    double * OD_rowPr
){

    double xi, yi, xj, yj, dist, mass, rowSum;

    memset( attraction_vectPr, 0, sizeof(double)*sizeSupra );
    memset( OD_rowPr, 0, sizeof(double)*sizeSupra );

    //x and y coordinates of node s
    xi = vCoords[s];
    yi = vCoords[sizeSupra + s];

    //keep track of a sum of the row, to normalize later
    rowSum = 0;
    //for all nodes j in the network...
    for (node_id_type j=0; j<sizeSupra; j++){
        if (s != j ){

            //x and y coordinates of node j
            xj = vCoords[j];
            yj = vCoords[sizeSupra + j];

            //Calculate distance between s and j
            dist = sqrt(pow(xi-xj,2) + pow(yi-yj,2));

            //mass = # stores at j * halfnorm(distance_sj)
            mass = vStores[j] * halfNormalPDF(dist);
            attraction_vectPr[j] = mass;
            rowSum = rowSum + mass;

        }
        else {
            attraction_vectPr[j] = 0;
        }
    }

    for (node_id_type j=0; j<sizeSupra; j++){
        //OD_row = population at source * normalized mass of j
        OD_rowPr[j] = vPop[s]*(attraction_vectPr[j] / rowSum);
    }
}

void printMatrix(double * P, int rows, int cols);
void printMatrixInverted(double * P, int rows, int cols);
void printSparseMatrix(int n, double * pr, size_t * ir, size_t * jc);
//to use the heaps
struct dk_Node{
    node_id_type nodeId;
    double distance;
    dk_Node(node_id_type id, double dist) : nodeId(id),distance(dist) { } //<-- this is a constructor
};
struct compare_dk_Node{
    bool operator()(const dk_Node& n1, const dk_Node& n2) const{
        return n1.distance > n2.distance;
    }
};

//prhs[0] -> Adjacency Matrix in time
//prhs[1] -> N nodes
//prhs[2] -> N layers = 1
//prhs[3] -> vector s of services per node
//prhs[4] -> vector p of population per node
//prhs[5] -> vector c of node coordinates x,y

//plhs[0] -> Node betweenness
//plhs[1] -> Directed edge betweenness
//plhs[2] -> Num at start node CHECK
//plhs[3] -> Num at end node CHECK

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ){

    //output matrices
    double tolerance = 0.000000001;
    node_id_type numNodes = 0, numLayers = 0;
    node_id_type sizeSupra = 0;
    mwSignedIndex dimsP[3];
    mxArray *rhs[1], *lhs[1];

    // get and check inputs
    if (nrhs != 6) mexErrMsgTxt( "Only 6 input arguments allowed." );
    if (nlhs > 2) mexErrMsgTxt( "Only 2 output argument allowed." );
    sizeSupra = mxGetN( prhs[0] );
    if (mxGetM( prhs[0] ) != sizeSupra) mexErrMsgTxt( "Input matrix G needs to be square." );
    if(mxIsSparse(prhs[0])==0) mexErrMsgTxt( "Input matrix must be sparse" );

    numNodes = mxGetPr(prhs[1])[0];
    numLayers = mxGetPr(prhs[2])[0];

    double * vStores = mxGetPr(prhs[3]);
    double * vPop = mxGetPr(prhs[4]);
    double * vCoords = mxGetPr(prhs[5]);

    mxArray * attraction_vect = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    mxArray * OD_row = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    double * attraction_vectPr = mxGetPr(attraction_vect);
    double * OD_rowPr = mxGetPr(OD_row);

    if(numLayers != 1){
        mexErrMsgTxt("This code is not prepared to work in multiplex networks" );
    }
    if((numNodes*numLayers)!=sizeSupra) mexErrMsgTxt( "Incorrect size of the input matrix" );

    rhs[0] = mxDuplicateArray(prhs[0]);
    mexCallMATLAB(1, lhs, 1, rhs, "'");
    mxDestroyArray(rhs[0]);

    double *Gpr = mxGetPr(lhs[0]);
    size_t *Gir = mxGetIr(lhs[0]);
    size_t *Gjc = mxGetJc(lhs[0]);

    double edgeWeight = 0;
    double min_dist = -1;

    long int sIndEdges;
    long int eIndEdges;
    long int nInd;
    node_id_type v,s,w,min_v;
    double numPacketReachDest;
    double * C_B = 0;
    double * spEBW = 0;
    staticQueue * P = NULL;
    double * sigma = 0;
    double * sigmaOrig = 0;
    double * d = 0;
    double * delta = 0;
    double * visited = 0;
    staticQueue S;

    //to store the nodes and the distance to them
    boost::heap::fibonacci_heap< dk_Node, boost::heap::compare<compare_dk_Node> > dk_heap;
    boost::heap::fibonacci_heap< dk_Node, boost::heap::compare<compare_dk_Node> >::handle_type heapHandles[MAX_NODES];

    plhs[0] = mxCreateDoubleMatrix( sizeSupra, 1, mxREAL );
    C_B = mxGetPr(plhs[0]);
    plhs[1] = mxCreateDoubleMatrix( numLayers*numNodes, numLayers*numNodes, mxREAL );
    spEBW = mxGetPr(plhs[1]);

    visited = (double *)mxMalloc(sizeof(double)*sizeSupra);
    sigma = (double *)mxMalloc(sizeof(double)*sizeSupra);
    sigmaOrig = (double *)mxMalloc(sizeof(double)*sizeSupra);
    d = (double *)mxMalloc(sizeof(double)*sizeSupra);
    P = (staticQueue *)mxMalloc(sizeof(staticQueue)*sizeSupra);
    delta = (double *)mxMalloc(sizeof(double)*sizeSupra);

    memset( C_B, 0, sizeof(double)*sizeSupra );
    memset( spEBW, 0, sizeof(double)*sizeSupra );

    for( s = 0 ; s<numNodes; s++){

        sprintf(outputString,"fprintf('Source = %lu\\n');",s+1);

        mexEvalString(outputString);

        getOdRow(s, vStores, vPop, vCoords, sizeSupra, attraction_vectPr, OD_rowPr);

        //initializations
        create(&S);

        for( w = 0 ; w<sizeSupra ; w++){
            create(&P[w]);
            sigma[w] = 0;
            d[w] = mxGetInf();
            visited[w] = 0;

            //store the handle in a table then use the table to access the handle
            heapHandles[w] = dk_heap.push(dk_Node(w,d[w]));
        }
        d[s] = 0;
        (*heapHandles[s]).distance = 0; dk_heap.decrease(heapHandles[s]);
        sigma[s] = 1;

        //end initializations

        min_v = dk_heap.top().nodeId;
        min_dist = dk_heap.top().distance;
        dk_heap.pop();

        while( min_dist < mxGetInf()){
        //Min_V := S;
        //Min_Dist := 0.0;
        //while Min_Dist /= Plus_Infinity loop

            if(DEBUG){
                sprintf(outputString,"fprintf('min_v %lu, min_dist = %f\\n');",min_v+1,min_dist);
                mexEvalString(outputString);
            }

            v = min_v;

            visited[v] = 1;
            add(&S,v);

            if(DEBUG){
                sprintf(outputString,"fprintf('Exploring %lu\\n');",v+1);
                mexEvalString(outputString);
            }

            //for each neigbor of v
            sIndEdges = Gjc[v];
            eIndEdges = Gjc[v+1] - 1;

            if(DEBUG){
                sprintf(outputString,"fprintf('\\tmin_v = %lu, min_dist = %f\\n');",min_v+1, min_dist);
                mexEvalString(outputString);
            }

            for( nInd = sIndEdges ; nInd<=eIndEdges ; nInd++ ){
            //while Has_Next(El) loop
                w = Gir[nInd];
                edgeWeight = Gpr[nInd];

                if(DEBUG){
                    sprintf(outputString,"fprintf('\tEdge from w=%lu to w=%lu and edgeWeight = %f\\n');",v+1, w+1, edgeWeight);
                    mexEvalString(outputString);
                }

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\tedges out %lu\\n');",w);
                    mexEvalString(outputString);
                }

                if (v != w){
                //if V /= W then
                    if(d[w] > d[v] + edgeWeight){ // new path to the vertex
                        d[w] = d[v] + edgeWeight;

                        (*heapHandles[w]).distance = d[w]; dk_heap.decrease(heapHandles[w]);

                        sigma[w] = sigma[v];
                        create(&P[w]);
                        add(&P[w],v);

                        if(DEBUG){
                            sprintf(outputString,"fprintf('\\t\\tnew path to vertex %lu with dist  = %f\\n');",w+1, d[w]);
                            mexEvalString(outputString);
                        }
                    }else if( fabs(d[w] - d[v] - edgeWeight) < tolerance){ // the current path is of equal length
                        sigma[w] = sigma[w] + sigma[v];
                        add(&P[w],v);

                        if(DEBUG){
                            sprintf(outputString,"fprintf('\\t\\tpath to equal length to %lu , path difference  =%f\\n');",w+1, fabs(d[w] - d[v] - edgeWeight));
                            mexEvalString(outputString);
                            sprintf(outputString,"fprintf('\\t\\told length = %f, dist to %lu = %f and edgeweight = %f\\n');",d[w],v+1,d[v],edgeWeight);
                            mexEvalString(outputString);
                        }
                    }
                 }
                 //end if;
            }
            //end loop;

            if(!dk_heap.empty()){
                min_v = dk_heap.top().nodeId;
                min_dist = dk_heap.top().distance;
                dk_heap.pop();
            }else{
                min_v = -1;
                min_dist = mxGetInf();
            }
        }

        //keep an original copy of the sigma
        //memcpy(sigmaOrig,sigma,sizeof(double)*sizeSupra);
        for( w = 0 ; w<sizeSupra ; w++){
            delta[w] = 0;

        }

        while(!empty(&S)){

            w = pop(&S);

            if(DEBUG){
                sprintf(outputString,"fprintf('explore vertex w = %lu\\n');",w+1);
                mexEvalString(outputString);
            }

            if(DEBUG){
                sprintf(outputString,"fprintf('Distance from (s=%lu) to (w=%lu) = %f\\n');",s+1,w+1,d[w]);
                mexEvalString(outputString);
            }

            while(!empty(&P[w])){

                v = pop(&P[w]);

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\tvertex %lu has predecessor %lu\\n');",w+1,v+1);
                    mexEvalString(outputString);
                }

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\t\\tPred: (v=%lu)\\n');",v);
                    mexEvalString(outputString);
                    sprintf(outputString,"fprintf('\\t\\tsigma[v=%lu]=%i/sigma[w=%lu]=%i,delta[w=%lu]=%f\\n');",v,(int)sigma[v],w,(int)sigma[w],w,delta[w]);
                    mexEvalString(outputString);
                }

                //--> Aquí la OD matrix

                numPacketReachDest = OD_rowPr[w];

                delta[v] = delta[v] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                if(v != s){

                    spEBW[v + sizeSupra*w] = spEBW[v + sizeSupra*w] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                }
                if(v == s){

                    spEBW[v + sizeSupra*w] = spEBW[v + sizeSupra*w] + (sigma[v]/sigma[w])*(numPacketReachDest + delta[w]);

                }

            }

            if(w!=s){

                C_B[w] = C_B[w] + delta[w];  /// aqui la delta d'ha de normalizar no es poden sumar les betweenness
                                                               // de les differents capes simplement

                if(DEBUG){
                    sprintf(outputString,"fprintf('\\t\\tBetweenness[w(%lu)]=%f\\n');",w,C_B[w]);
                    mexEvalString(outputString);
                }

            }

        }

    }

    mxFree(sigma);
    mxFree(sigmaOrig);
    mxFree(d);
    mxFree(P);
    mxFree(delta);
    mxFree(visited);
}

void printMatrix(double * P, int rows, int cols){
    int rowIndex = 0, columnIndex = 0;

    //print the matrix
    for(rowIndex = 0 ; rowIndex < rows; rowIndex++){
        for(columnIndex = 0 ; columnIndex < cols; columnIndex++){
            mexPrintf("%1.4f ",P[rowIndex*cols + columnIndex]);
        }
        mexPrintf("\n");
    }
}

void printMatrixInverted(double * P, int rows, int cols){
    int rowIndex = 0, columnIndex = 0;

    //print the matrix
    for(rowIndex = 0 ; rowIndex < rows; rowIndex++){
        for(columnIndex = 0 ; columnIndex < cols; columnIndex++){
            mexPrintf("%1.4f ",P[columnIndex*rows + rowIndex]);
        }
        mexPrintf("\n");
    }
}

void printSparseMatrix(int n, double * pr, size_t * ir, size_t * jc){
    size_t nrow;

    mexPrintf("This is from the .cpp");

    for (int y=0; y<n; y++){
        nrow = jc[y+1] - jc[y];
        for (int x = 0; x<nrow; x++){
            mexPrintf("      (%d,%d)    %g\n", (*ir++)+1,y+1,*pr++);
        }
    }
}


//------------------------------------------------------------------------------
// \file Rings_tests.cpp
// \ref https://www.cs.utexas.edu/users/fussell/courses/cs429h/lectures/Lecture_2-429h.pdf
//------------------------------------------------------------------------------
#include "Algebra/Rings/Matrices2x2.h"

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <sstream>

using Algebra::Rings::Matrix2x2;

BOOST_AUTO_TEST_SUITE(Algebra)
BOOST_AUTO_TEST_SUITE(Rings)
BOOST_AUTO_TEST_SUITE(Rings_tests)

BOOST_AUTO_TEST_SUITE(Matrices2x2)

BOOST_AUTO_TEST_SUITE(Interface)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(ConstructsFrom4RealParameters)
{
	{
		const Matrix2x2<int> mat {1, 2, 3, 4};

		const auto result {mat.data()};

		for (int i {0}; i < 4; ++i)
		{
			BOOST_TEST(result.at(i) == (i + 1));
		}
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(OperatorInsertsIntoStringStream)
{
	{
		std::ostringstream oss;

		const Matrix2x2<double> mat {-5.6, 6.2, 7.1, -8.0};

		oss << mat;

		BOOST_TEST(oss.str() == "-5.6 6.2\n7.1 -8");

		oss.flush();
	}
}

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(AdditiveIdentityReturnsZero)
{
  BOOST_TEST(true);
}

BOOST_AUTO_TEST_SUITE_END() // Interface

BOOST_AUTO_TEST_SUITE(Operations)

BOOST_AUTO_TEST_SUITE_END() // Operations

BOOST_AUTO_TEST_SUITE_END() // Matrices2x2

BOOST_AUTO_TEST_SUITE_END() // Rings_tests
BOOST_AUTO_TEST_SUITE_END() // Rings
BOOST_AUTO_TEST_SUITE_END() // Algebra


/**
 * \file dcs/math/detail/float.hpp
 *
 * \brief Utilities for floating-point comparison.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2012 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef DCS_MATH_DETAIL_FLOAT_HPP
#define DCS_MATH_DETAIL_FLOAT_HPP

#include <algorithm>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/utility/enable_if.hpp>
//#include <cfloat>
#include <cmath>

namespace dcs { namespace math { namespace detail {

/// See also:
/// - http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm
/// - http://www.petebecker.com/js/js200012.html
/// - http://code.google.com/p/googletest/source/browse/trunk/include/gtest/internal/gtest-internal.h
/// - http://www.parashift.com/c++-faq-lite/newbie.html#faq-29.16
/// - http://adtmag.com/articles/2000/03/16/comparing-floats-how-to-determine-if-floating-quantities-are-close-enough-once-a-tolerance-has-been.aspx
/// - http://www.boost.org/doc/libs/1_47_0/libs/test/doc/html/utf/testing-tools/floating_point_comparison.html
/// - http://learningcppisfun.blogspot.com/2010/04/comparing-floating-point-numbers.html
/// - http://floating-point-gui.de/errors/comparison/
/// - https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
/// .

/**
 * \brief x is approximately equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \approx y\,\text{ if and only if } |y-x|\le\epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type approximately_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero)
	if (x == y)
	{
		return true;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isinf(y))
	{
		// According to IEEE, Infinite operands of the same sign shall compare equal
		return true;
	}
	if ((::std::isinf(x) && ::std::isfinite(y)) || (::std::isfinite(x) && ::std::isinf(y)))
	{
		// Infinity vs non-infinite operands are different
		return false;
	}

	return ::std::abs(x-y) <= (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely equal to y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \sim y\,\text{ if and only if } |y-x|\le\epsilon\min(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type essentially_equal(T x, T y, T tol)
{
	// Try first with standard comparison (handles the case when both x and y are zero)
	if (x == y)
	{
		return true;
	}
	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isinf(y))
	{
		// According to IEEE, Infinite operands of the same sign shall compare equal
		return true;
	}
	if ((::std::isinf(x) && ::std::isfinite(y)) || (::std::isfinite(x) && ::std::isinf(y)))
	{
		// Infinity vs non-infinite operands are different
		return false;
	}

	return ::std::abs(x-y) <= (::std::min(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely greater than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \succ y\,\text{ if and only if } x-y > \epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_greater(T x, T y, T tol)
{
	// Try first with standard comparison
	if (x <= y)
	{
		return false;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isfinite(y))
	{
		return true;
	}
	if (::std::isfinite(x) && ::std::isinf(y))
	{
		return false;
	}

	return (x-y) > (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

/**
 * \brief x is definitely less than y.
 *
 * Inspired by [1]:
 * \f[
 *  $x \prec y\,\text{ if and only if } y-x > \epsilon\max(|x|,|y|)
 * \f]
 *
 * References:
 * -# Knuth, "The Art of Computer Programming: Vol.2" 3rd Ed, 1998, Sec. 4.2.2.
 * .
 */
template <typename T>
inline
typename ::boost::enable_if<
	::boost::is_floating_point<T>,
	bool
>::type definitely_less(T x, T y, T tol)
{
	// Try first with standard comparison
	if (x >= y)
	{
		return false;
	}

	// Handle degenerate cases
	if (::std::isnan(x) || ::std::isnan(y))
	{
		// According to IEEE, NaN are different even by itself
		return false;
	}
	if (::std::isinf(x) && ::std::isfinite(y))
	{
		return false;
	}
	if (::std::isfinite(x) && ::std::isinf(y))
	{
		return true;
	}

	return (y-x) > (::std::max(::std::abs(x), ::std::abs(y))*tol);
}

}}} // Namespace dcs::math::detail

#endif // DCS_MATH_DETAIL_FLOAT_HPP


#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;

int main() {
  MatrixXd m(2, 2);
  m(0, 0) = 3;
  m(1, 0) = 2.5;
  m(0, 1) = -1;
  m(1, 1) = m(1, 0) + m(0, 1);
  std::cout << "Here is the matrix m:\n" << m << std::endl;
  VectorXd v(2);
  v(0) = 4;
  v(1) = v(0) - 1;
  std::cout << "Here is the vector v:\n" << v << std::endl;
}


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>
#include <vector>
#include <omp.h>

#include "dbscan.h"

namespace clustering {
DBSCAN::ClusterData DBSCAN::gen_cluster_data( size_t features_num, size_t elements_num )
{
    DBSCAN::ClusterData cl_d( elements_num, features_num );

    for ( size_t i = 0; i < elements_num; ++i ) {
        for ( size_t j = 0; j < features_num; ++j ) {
            cl_d( i, j ) = ( -1.0 + rand() * ( 2.0 ) / RAND_MAX );
        }
    }

    return cl_d;
}

DBSCAN::FeaturesWeights DBSCAN::std_weights( size_t s )
{
    // num cols
    DBSCAN::FeaturesWeights ws( s );

    for ( size_t i = 0; i < s; ++i ) {
        ws( i ) = 1.0;
    }

    return ws;
}

DBSCAN::DBSCAN()
{
}

static int num_threads_or_default( int nt )
{
    if ( !nt ) {
        return omp_get_max_threads();
    }
    return nt;
}

void DBSCAN::init( double eps, size_t min_elems, int num_threads )
{
    m_eps = eps;
    m_min_elems = min_elems;
    m_num_threads = num_threads_or_default( num_threads );
}

DBSCAN::DBSCAN( double eps, size_t min_elems, int num_threads )
    : m_eps( eps )
    , m_min_elems( min_elems )
    , m_num_threads( num_threads_or_default( num_threads ) )
    , m_dmin( 0.0 )
    , m_dmax( 0.0 )
{
    reset();
}

DBSCAN::~DBSCAN()
{
}

void DBSCAN::reset()
{
    m_labels.clear();
}

void DBSCAN::prepare_labels( size_t s )
{
    m_labels.resize( s );

    for ( auto& l : m_labels ) {
        l = -1;
    }
}

const DBSCAN::DistanceMatrix DBSCAN::calc_dist_matrix( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    DBSCAN::ClusterData cl_d = C;

    omp_set_dynamic( 0 );
    omp_set_num_threads( m_num_threads );
#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size2(); ++i ) {
        ublas::matrix_column< DBSCAN::ClusterData > col( cl_d, i );

        const auto r = minmax_element( col.begin(), col.end() );

        double data_min = *r.first;
        double data_range = *r.second - *r.first;

        if ( data_range == 0.0 ) {
            data_range = 1.0;
        }

        const double scale = 1 / data_range;
        const double min = -1.0 * data_min * scale;

        col *= scale;
        col.plus_assign( ublas::scalar_vector< typename ublas::matrix_column< DBSCAN::ClusterData >::value_type >( col.size(), min ) );
    }

    // rows x rows
    DBSCAN::DistanceMatrix d_m( cl_d.size1(), cl_d.size1() );
    ublas::vector< double > d_max( cl_d.size1() );
    ublas::vector< double > d_min( cl_d.size1() );

    omp_set_dynamic( 0 );
    omp_set_num_threads( m_num_threads );
#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size1(); ++i ) {
        for ( size_t j = i; j < cl_d.size1(); ++j ) {
            d_m( i, j ) = 0.0;

            if ( i != j ) {
                ublas::matrix_row< DBSCAN::ClusterData > U( cl_d, i );
                ublas::matrix_row< DBSCAN::ClusterData > V( cl_d, j );

                int k = 0;
                for ( const auto e : ( U - V ) ) {
                    d_m( i, j ) += fabs( e ) * W[k++];
                }

                d_m( j, i ) = d_m( i, j );
            }
        }

        const auto cur_row = ublas::matrix_row< DBSCAN::DistanceMatrix >( d_m, i );
        const auto mm = minmax_element( cur_row.begin(), cur_row.end() );

        d_max( i ) = *mm.second;
        d_min( i ) = *mm.first;
    }

    m_dmin = *( min_element( d_min.begin(), d_min.end() ) );
    m_dmax = *( max_element( d_max.begin(), d_max.end() ) );

    m_eps = ( m_dmax - m_dmin ) * m_eps + m_dmin;

    return d_m;
}

DBSCAN::Neighbors DBSCAN::find_neighbors( const DBSCAN::DistanceMatrix& D, uint32_t pid )
{
    Neighbors ne;

    for ( uint32_t j = 0; j < D.size1(); ++j ) {
        if ( D( pid, j ) <= m_eps ) {
            ne.push_back( j );
        }
    }
    return ne;
}

void DBSCAN::dbscan( const DBSCAN::DistanceMatrix& dm )
{
    std::vector< uint8_t > visited( dm.size1() );

    uint32_t cluster_id = 0;

    for ( uint32_t pid = 0; pid < dm.size1(); ++pid ) {
        if ( !visited[pid] ) {
            visited[pid] = 1;

            Neighbors ne = find_neighbors( dm, pid );

            if ( ne.size() >= m_min_elems ) {
                m_labels[pid] = cluster_id;

                for ( uint32_t i = 0; i < ne.size(); ++i ) {
                    uint32_t nPid = ne[i];

                    if ( !visited[nPid] ) {
                        visited[nPid] = 1;

                        Neighbors ne1 = find_neighbors( dm, nPid );

                        if ( ne1.size() >= m_min_elems ) {
                            for ( const auto& n1 : ne1 ) {
                                ne.push_back( n1 );
                            }
                        }
                    }

                    if ( m_labels[nPid] == -1 ) {
                        m_labels[nPid] = cluster_id;
                    }
                }

                ++cluster_id;
            }
        }
    }
}

void DBSCAN::fit( const DBSCAN::ClusterData& C )
{
    const DBSCAN::FeaturesWeights W = DBSCAN::std_weights( C.size2() );
    wfit( C, W );
}
void DBSCAN::fit_precomputed( const DBSCAN::DistanceMatrix& D )
{
    prepare_labels( D.size1() );
    dbscan( D );
}

void DBSCAN::wfit( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    prepare_labels( C.size1() );
    const DBSCAN::DistanceMatrix D = calc_dist_matrix( C, W );
    dbscan( D );
}

const DBSCAN::Labels& DBSCAN::get_labels() const
{
    return m_labels;
}

std::ostream& operator<<( std::ostream& o, DBSCAN& d )
{
    o << "[ ";
    for ( const auto& l : d.get_labels() ) {
        o << " " << l;
    }
    o << " ] " << std::endl;

    return o;
}
}


#include <iostream>
#include "ArchivoTrain.h"
#include "Imagen.h"
#include "Trabajador.h"
#include "config.h"
#include "ArchivoNeighbors.h"
#include <set>
#include "ActiveSet.h"
#include <fstream>
#include <Eigen/Eigenvalues>
#include <algorithm>
#include <iterator>
/**
 * Necesito que el archivo venga en
 * "exactamente" el mismo formato que
 * el train de kaggle, se detecta la
 * cantidad de dimensiones
 * */
//weinberger 09 - páginas 33 a 35
using namespace std;

void filtrarNegativos(Matriz& m){
	for(int i = 0; i < m.rows(); i++){
		if(m(i,i)<0){
			m(i,i)=0;
		}
	}
}

void obtenerSalieron(	ConjuntoActiveSets& salieron,
						ConjuntoActiveSets& activos,
						ConjuntoActiveSets& anterior){
	//salieron = anterior - activos
	set_difference(	anterior.begin(),anterior.end(),
					activos.begin(),activos.end(),
					inserter(salieron,salieron.end()),
					CompararActiveSet()
	);
}
void obtenerEntraron(	ConjuntoActiveSets& entraron,
						ConjuntoActiveSets& activos,
						ConjuntoActiveSets& anterior){
	//entraron = activos - anterior
	set_difference(	activos.begin(),activos.end(),
					anterior.begin(),anterior.end(),
					inserter(entraron,entraron.end()),
					CompararActiveSet()
	);
	//notese que los argumentos están invertidos
}

int main(int argc, char* argv[]){
	if(argc==1){
		cout<<"Y los argumentos?"<<endl;
	}else{
		if(argv[1][0]=='n'){
			cout<<"Recalculando target neighbours"<<endl;
			ArchivoTrain archivo;
			archivo.conectarTargetNeighbors(5,0,1000);
			archivo.guardarTargetNeighbours(0,1000);
		}else if(argv[1][0]=='m'){
			ofstream off("M.dat");
			cout<<"Voy a calcular la matriz M"<<endl;
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);

			Matriz m,g;
			m.setIdentity();//initialize with the identity matrix
			ConjuntoActiveSets posibles_activos, activos, anteriores;//initialize active sets
			//neighbors.calcularG0(g);//initialize gradient

			for(int i = 0; i<2; i++){//while not converged do
				cout<<"Iniciando nueva iteracion:"<<i<<endl;
				if(i==0){//if mod(i,10)==0 || casi convergencia then
					posibles_activos.clear();
					neighbors.agregarActiveSets(posibles_activos);//compute Nt+1 exactly
					//(es inviable hacer la búsqueda de active sets con la distancia Mahalanobis)
					activos = posibles_activos;//update active set
				}else{
					anteriores = activos;
					neighbors.filtrarActivos(posibles_activos, activos,m);
					//compute Nt+1 only search active set
				}
				cout<<"Posibles activos:"<<activos.size()<<endl;

				cout<<"Voy a calcular la G nueva"<<endl;

				ConjuntoActiveSets salieron, entraron;
				obtenerSalieron(salieron,activos,anteriores);
				obtenerEntraron(entraron,activos,anteriores);
				cout<<"Ya tengo los que salientraron"<<endl;

				Matriz gResta,gSuma;
				neighbors.calcularSegundoTerminoGradiente(gResta,salieron);
				neighbors.calcularSegundoTerminoGradiente(gSuma,entraron);
				g+=g-gResta+gSuma;//sumaresta de gradientes

				cout<<"Descmponiendo las matrices"<<endl;

				Matriz m_def = m-0.1*g;//Mt-alfa*Gt+1
				Eigen::EigenSolver<Matriz> solver(m_def);
				Matriz delta = solver.pseudoEigenvalueMatrix();
				Matriz v = solver.pseudoEigenvectors();
				filtrarNegativos(delta);//proyección sobre semidefinidas positivas
				m=v*delta*v.transpose();//take gradient step

			}

			off<<m;//output Mt

		}else if(argv[1][0]=='g'){
			ofstream off("G0.dat");
			cout<<"Voy a calcular la matriz G0"<<endl;
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);
			Matriz g0;
			neighbors.calcularG0(g0);
			cout<<"G0 es "<<g0<<endl;
			off<<g0;
		}else if(argv[1][0]=='a'){
			ofstream off("G0.dat");
			cout<<"Voy a calcular los ActiveSets"<<endl;
			Matriz m,g0;
			m.setIdentity();
			ArchivoTrain archivo;
			ArchivoNeighbors neighbors(archivo);

			ConjuntoActiveSets caca;
			neighbors.agregarActiveSets(caca);
			cout<<"Activesets: "<<caca.size()<<endl;
		}
	}
	return 0;
}


#include "average_case_error.hpp"

#include <cudd_helpers.hpp>
#include <boost/multiprecision/cpp_int.hpp>

namespace abo::error_metrics {

    boost::multiprecision::cpp_dec_float_100 average_value(const std::vector<BDD> &f) {

        boost::multiprecision::uint256_t sum = 0;
        const boost::multiprecision::uint256_t one = 1;

        int max_support_size = 0;
        for (const auto &bdd : f) {
            max_support_size = std::max(max_support_size, bdd.SupportSize());
        }

        for (unsigned int i = 0;i<f.size();i++) {
            // TODO: CountMinterms returns a double, might loose precision
            double minterms = f[i].CountMinterm(max_support_size);
            sum += (one << i) * boost::multiprecision::uint256_t(minterms);
        }

        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(one << max_support_size);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_value(const std::vector<BDD> &f) {

        boost::multiprecision::uint256_t sum = 0;
        const boost::multiprecision::uint256_t one = 1;

        int max_support_size = 0;
        for (const auto &bdd : f) {
            max_support_size = std::max(max_support_size, bdd.SupportSize());
        }

        for (unsigned int i = 0;i<f.size();i++) {
            for (unsigned int b = i;b<f.size();b++) {
                // cudd handles the case i = b efficiently
                BDD both = f[i] & f[b];
                double minterms = both.CountMinterm(max_support_size);
                // most terms (except for i == b), appear twice when factoring the square
                boost::multiprecision::uint256_t doubling_factor = i == b ? 1 : 2;
                sum += (one << i) * (one << b) * boost::multiprecision::uint256_t(minterms) * doubling_factor;
            }
        }

        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(one << max_support_size);
    }

    boost::multiprecision::cpp_dec_float_100 average_case_error(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return average_value(absolute_difference);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_error(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                const util::NumberRepresentation num_rep) {

        std::vector<BDD> absolute_difference = abo::util::bdd_absolute_difference(mgr, f, f_hat, num_rep);
        return mean_squared_value(absolute_difference);
    }

    boost::multiprecision::cpp_dec_float_100 average_case_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                    const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(diff);

        boost::multiprecision::uint256_t sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            sum += boost::multiprecision::uint256_t(p.first) * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

    boost::multiprecision::cpp_dec_float_100 mean_squared_error_add(const Cudd &mgr, const std::vector<BDD> &f, const std::vector<BDD> &f_hat,
                                                                    const NumberRepresentation num_rep) {

        ADD diff = abo::util::absolute_difference_add(mgr, f, f_hat, num_rep);
        std::vector<std::pair<double, unsigned long>> terminal_values = abo::util::add_terminal_values(diff);

        boost::multiprecision::uint256_t sum = 0;
        boost::multiprecision::uint256_t path_sum = 0;
        for (auto p : terminal_values) {
            boost::multiprecision::uint256_t value(p.first);
            sum += value * value * p.second;
            path_sum += p.second;
        }
        return boost::multiprecision::cpp_dec_float_100(sum) /
                boost::multiprecision::cpp_dec_float_100(path_sum);
    }

}


#include "Gamma.h"
#include <boost/math/distributions/gamma.hpp>

BaseDistributionGamma::BaseDistributionGamma(const Options& iOptions, const Data& iData) : BaseDistribution(iOptions, iData) {

}
float BaseDistributionGamma::getCdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   boost::math::gamma_distribution<> dist(shape, scale);
   return boost::math::cdf(dist, iX);
}
float BaseDistributionGamma::getPdf(float iX, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   //std::cout << "Scale, shape = " << scale << " " << shape << std::endl;
   boost::math::gamma_distribution<> dist(shape, scale);
   //std::cout << scale << " " << shape << std::endl;
   return boost::math::pdf(dist, iX);
}
float BaseDistributionGamma::getInv(float iCdf, const std::vector<float>& iMoments) const {
   assert(iMoments.size() == 2);
   float mean     = iMoments[0];
   float variance = iMoments[1];
   if(variance == 0) {
      return Global::MV;
   }
   float scale    = getScale(mean, variance);
   float shape    = getShape(mean, variance);
   boost::math::gamma_distribution<> dist(shape, scale);
   return boost::math::quantile(dist, iCdf);
}
int BaseDistributionGamma::getNumMoments() const {
   return 2;
}
float BaseDistributionGamma::getShape(float iMean, float iVariance) {
   // alpha (wiki: k)
   return iMean*iMean / iVariance;
}
float BaseDistributionGamma::getScale(float iMean, float iVariance) {
   // beta (wiki: theta)
   return iVariance / iMean;
}


#include <limits>
#include <iostream>
#include <iomanip>
#include <string>
#include <cmath>
#include <algorithm>
#include <vector>
// Includes
// ========
#include <iostream>
// BGL includes
#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>

// Graph Type with nested interior edge properties for Cost Flow Algorithms
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; // new! weightmap corresponds to costs

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

// Custom edge adder class
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
  }
};

int ind(int i, int j, int n) {
    return  i * n + j;
}

void testcase() {
    int n; std::cin >> n;
    int nn = n * n;
    int sum_a = 0;
    std::vector<std::vector<int>> a(n, std::vector<int>(n));
    for(int i = 0; i < n; i++) {
        for(int j = 0; j < n; j++) {
            std::cin >> a[i][j];
            sum_a += a[i][j];
        }
    }

    graph G(2 * nn);
    edge_adder adder(G);
    int max_a = 100;
    const int v_source = nn;
    const int v_sink = nn - 1;
    for(int i = 0; i < n; i++) {
        for(int j = 0; j < n; j++) {
            if(i < n - 1) adder.add_edge(nn + ind(i,j,n), ind(i+1,j,n), 1, 0);
            if(j < n - 1) adder.add_edge(nn + ind(i,j,n), ind(i,j+1,n), 1, 0);
            if(!( (i == 0 && j == 0) || (i == n - 1 && j == n - 1))) {
                adder.add_edge(ind(i,j,n), nn + ind(i,j,n), 1, -a[i][j] + max_a);
            }
        }
    }

    boost::successive_shortest_path_nonnegative_weights(G, v_source, v_sink);
    int cost1 = boost::find_flow_cost(G);
    // std::cerr << cost1 << " " << (4 * n - 6) * max_a << std::endl;
    std::cout << (4 * n - 6) * max_a - cost1 + a[0][0] + a[n-1][n-1] << std::endl;
    return;
}

int main() {
    std::ios_base::sync_with_stdio(false);

    int t;
    std::cin >> t;
    for (int i = 0; i < t; ++i)
        testcase();
}


//===------------------------------------------------------------*- C++ -*-===//
///
/// \brief 仿射变换。
/// \details 即线性变换和位移变换：输出向量 v' = 线性变换矩阵 M * 输入向量 v + 位移向量 k。
///
/// \sa <https://zh.wikipedia.org/wiki/仿射变换>
///
/// \version 2021-11-08
/// \since 2021-11-08
/// \authors zhengrr
/// \copyright Unlicense
///
//===----------------------------------------------------------------------===//

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <gtest/gtest.h>

TEST(AffineTransformation, Case)
{
    using Scalar = float;
    constexpr Eigen::Index dimension = 2;
    using Point = Eigen::Vector<Scalar, dimension>;
    using LinearTransformationMatrix = Eigen::Matrix<Scalar, dimension, dimension>;
    using AffineTransformationMatrix = Eigen::Transform<Scalar, dimension, Eigen::Affine>;
    //                       [x ]
    //                       [y ]
    //                       [1 ]
    //
    // [M(0,0) M(0,1) k(x)]  [x'] = M(0,0)x + M(0,1)y + k(x)
    // [M(1,0) M(1,1) k(y)]  [y'] = M(1,0)x + M(1,0)y + k(y)
    // [0      0      1   ]  [1 ] = 1

    // 单位矩阵
    const auto i = AffineTransformationMatrix::Identity();

    const Eigen::Matrix<Scalar, dimension + 1, dimension + 1> matrix {
        {1, 0, 0},
        {0, 1, 0},
        {0, 0, 1}
    };
    const Eigen::Matrix<Scalar, dimension, dimension> rotation {
        {1, 0},
        {0, 1}
    };
    const Eigen::Matrix<Scalar, dimension, 1> translation {
        {0},
        {0}
    };
    EXPECT_EQ(i.matrix(), matrix);
    EXPECT_EQ(i.rotation(), rotation);
    EXPECT_EQ(i.translation(), translation);
}


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__INTERNAL__LMPAR_HPP_
#define SMOOTH__INTERNAL__LMPAR_HPP_

#include <Eigen/Core>
#include <Eigen/Jacobi>
#include <Eigen/QR>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "utils.hpp"

namespace smooth::detail {

/**
 * @brief Solve structured least-squares probelm
 *
 *   min_x \| [J ; D] x + [r; 0] \|^2
 *
 * Where a is a size N vector and J is M x N
 * and it must hold that J^T J + D^T D is positive semi-definite
 * where D = diag(d) is a diagonal matrix
 *
 * Let J P = Q R0  be a QR decomposition of J where P is a permutation matrix
 *
 * @param[in, out] R top left NxN corner of R0 (NxN upper triangular) (dense or sparse supported)
 * @param[in] Qt_r N-vector containing product Q' * r
 * @param[in] P NxN permutation matrix in QR decomposition
 * @param[in] d N-vector representing diagonal of D
 *
 * The function modifies R to be an upper triangular matrix Rt in the QR decomposition
 * of [R; P' D P] which satisfies
 *
 *    Rt' Rt = P' (J' J + D' D) P
 *
 * NOTE For systems with M < N, R and Qt_r should be filled with zeros at the bottom
 *
 * NOTE To maximize performance in the sparse case R should be Row-Major
 */
template<int N, typename MatrixType, typename PermIndex>
Eigen::Matrix<double, N, 1> solve_ls(MatrixType & R,
  const Eigen::Matrix<double, N, 1> & Qt_r,
  const Eigen::PermutationMatrix<N, N, PermIndex> & P,
  const Eigen::Matrix<double, N, 1> & d)
{
  // true if R is sparse
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<MatrixType>, MatrixType>;

  const auto n                  = R.cols();
  Eigen::Matrix<double, N, 1> a = Qt_r;

  // We operate on B and b row-wise, so just need to allocate one row
  Eigen::Matrix<double, N, 1> Bj(n);
  double bj;

  // QR decomposition of [R; P' D P] with Givens rotations;
  // where it is known that A is upper triangular and B is diagonal
  // algorithm:
  //   R = A, Q = I
  //   for each nonzero in B part:
  //      find rotation G that eliminates nonzero without introducing new zeros
  //      Q = Q G
  //      R = G' R
  Eigen::JacobiRotation<double> G;
  for (auto j = 0u; j != n; ++j) {  // for each diagonal element
    // find permuted diagonal index
    const auto permidx = P.indices()(j);
    // initialize row j of B and b
    Bj.tail(n - 1 - j).setZero();
    Bj(j) = d(permidx);  // Bj(k) represents B(j, k)
    bj    = 0;           // bj represents b(j)

    for (auto col = j; col != n; ++col) {  // for each column right of diagonal
      if (R.coeff(col, col) >= 0 && Bj(col) == 0) { continue; }

      double r;
      G.makeGivens(R.coeff(col, col), Bj(col), &r);  // eliminates B(j, col)

      // perform matrix multiplication R = G' R
      // affects row 'col' of R and row 'j' of B
      R.coeffRef(col, col) = r;
      for (auto k = col + 1; k != n; ++k) {
        const double tmp = G.c() * R.coeff(col, k) - G.s() * Bj(k);
        Bj(k)            = G.s() * R.coeff(col, k) + G.c() * Bj(k);
        if constexpr (is_sparse) {
          if (R.coeff(col, k) != 0 || tmp != 0) { R.coeffRef(col, k) = tmp; }
        } else {
          R.coeffRef(col, k) = tmp;
        }
      }

      // At the end we need Q matrix to multiply rhs as
      // Q' * rhs = (I * G0 * ... * Gk)' * rhs = Gk' * ... * G0' * rhs
      //
      // We can therefore do it as we go, here we set
      // rhs = G * rhs
      const double tmp = G.c() * a(col) - G.s() * bj;
      bj               = G.s() * a(col) + G.c() * bj;
      a(col)           = tmp;
    }
  }

  // solve triangular system R z = a to obtain z = R^-1 z
  // first check rank of upper-diagonal R (may happen if d not full-rank)
  int rank = 0;
  for (; rank != n && R.coeff(rank, rank) >= Eigen::NumTraits<double>::dummy_precision(); ++rank) {}

  Eigen::Matrix<double, N, 1> sol(n);
  sol.tail(n - rank).setZero();
  sol.head(rank) =
    R.topLeftCorner(rank, rank).template triangularView<Eigen::Upper>().solve(a.head(rank));

  // solution is now equal to -P z
  return -(P * sol);
}

/**
 * @brief Solve structured least-squares probelm
 *
 *   min_x \| [J ; D] x + [r; 0] \|^2
 *
 * Where a is a size N vector and J is M x N
 * and it must hold that J^T J + D^T D is positive semi-definite
 * where D = diag(d) is a diagonal matrix
 *
 * @param J_qr QR decomposition J P = Q R of J with column pivoting
 * @param d vector of length N representing diagonal matrix
 * @param r vector with same number of elements as there are rows in J
 */
template<int N, int M, typename QrType>
Eigen::Matrix<double, N, 1> solve_ls(
  const QrType & J_qr, const Eigen::Matrix<double, N, 1> & d, const Eigen::Matrix<double, M, 1> & r)
{
  // true if it's a sparse decomposition
  static constexpr bool is_sparse =
    std::is_base_of_v<Eigen::SparseMatrixBase<typename QrType::MatrixType>,
      typename QrType::MatrixType>;

  // figure type to use for A matrix
  using AType = std::conditional_t<is_sparse,
    Eigen::SparseMatrix<double, Eigen::RowMajor>,
    Eigen::Matrix<double, N, N>>;

  static constexpr int NM_min  = std::min(N, M);
  static constexpr int NM_rest = NM_min == -1 ? -1 : N - NM_min;

  // dynamic sizes
  const auto n       = J_qr.cols();
  const auto m       = J_qr.rows();
  const auto nm_min  = std::min(n, m);
  const auto nm_rest = n - nm_min;

  AType R(n, n);
  if constexpr (is_sparse) {
    // allocate upper triangular pattern
    Eigen::Matrix<Eigen::Index, N, 1> pattern(n);
    for (auto i = 0u; i != n; ++i) { pattern(i) = i + 1; }
    R.reserve(pattern);
  } else {
    R.template bottomRows<NM_rest>(nm_rest).setZero();
  }
  R.template topRows<NM_min>(nm_min) = J_qr.matrixR().template topRows<NM_min>(nm_min);

  Eigen::Matrix<double, N, 1> Qt_r(n);
  Qt_r.template head<NM_min>(nm_min) =
    (J_qr.matrixQ().transpose() * r).template head<NM_min>(nm_min);
  Qt_r.template bottomRows<NM_rest>(nm_rest).setZero();

  return solve_ls<N>(R, Qt_r, J_qr.colsPermutation(), d);
}

/**
 * @brief Approximate a Levenberg-Marquardt parameter lambda s.t. if x solves
 *
 *   \| [J; sqrt(lambda) * diag(d)] x  +  [r ; 0] \|^2
 *
 * then either
 *  * lambda = 0 AND \|diag(d) * x\| <= 1.1 Delta
 *    OR
 *  * lambda > 0 AND 0.9 Delta <= \|diag(d) * x\| <= 1.1 Delta
 *
 * @param J matrix MxN (static/dynamic/sparse sizes supported)
 * @param d vector size Nx1 (static/dynamic sizes supported)
 * @param r vector size Mx1 (static/dynamic sizes supported)
 * @param Delta scalar
 * @return pair(lambda, x) where x solves the least-squares problem for lambda
 *
 * @note The sparse QR decomposition is numerically unstable for ill-conditioned matrices.
 * Consider using lmpar_sparse instead.
 */
template<int N, int M, typename MatrixT>
std::pair<double, Eigen::Matrix<double, N, 1>> lmpar(const MatrixT & J,
  const Eigen::Matrix<double, N, 1> & d,
  const Eigen::Matrix<double, M, 1> & r,
  double Delta)
{
  static constexpr bool is_sparse = std::is_base_of_v<Eigen::SparseMatrixBase<MatrixT>, MatrixT>;

  static constexpr int NM_min  = std::min(N, M);
  static constexpr int NM_rest = NM_min == -1 ? -1 : N - NM_min;

  // dynamic sizes
  const auto m       = J.rows();
  const auto n       = J.cols();
  const auto nm_min  = std::min(n, m);
  const auto nm_rest = n - nm_min;

  // calculate qr decomposition of J
  std::conditional_t<is_sparse,
    Eigen::SparseQR<MatrixT, Eigen::COLAMDOrdering<int>>,
    Eigen::ColPivHouseholderQR<MatrixT>> J_qr;

  if constexpr (is_sparse) {
    // sparse solver is not very good for close-to-singular matrices
    // J_qr.setPivotThreshold(1e-1);
  }

  J_qr.compute(J);

  // calculate size n Qt_r
  Eigen::Matrix<double, N, 1> Qt_r(n);
  Qt_r.template head<NM_min>(nm_min) =
    (J_qr.matrixQ().transpose() * r).template head<NM_min>(nm_min);
  Qt_r.template bottomRows<NM_rest>(nm_rest).setZero();

  // calculate phi(0) by solving J x = -r as x = P R^-1 (-Q' r)
  Eigen::Matrix<double, N, 1> x(n);
  int rank     = J_qr.rank();
  x.head(rank) = J_qr.matrixR()
                   .topLeftCorner(rank, rank)
                   .template triangularView<Eigen::Upper>()
                   .solve(-Qt_r.head(rank));
  x.tail(n - rank).setZero();
  x.applyOnTheLeft(J_qr.colsPermutation());

  Eigen::Matrix<double, N, 1> D_x_iter = d.cwiseProduct(x);
  double D_x_iter_norm                 = D_x_iter.stableNorm();

  double alpha = 0;
  double phi   = D_x_iter_norm - Delta;
  double dphi;

  if (phi <= 0.1 * Delta) {
    // alpha = 0 solution fulfills condition
    return std::make_pair(alpha, std::move(x));
  }

  // lower bound
  double l = 0;
  if (J_qr.rank() == n) {
    // full rank means we can calculate dphi(0)
    // as - \| D x \| * \| Rinv (P' D' D x) / \| Dx \| \|^2
    Eigen::Matrix<double, N, 1> y =
      J_qr.colsPermutation().inverse() * (d.cwiseProduct(D_x_iter) / D_x_iter_norm);
    J_qr.matrixR()
      .template topLeftCorner<N, N>(n, n)
      .template triangularView<Eigen::Upper>()
      .transpose()
      .solveInPlace(y);
    dphi = -D_x_iter_norm * y.squaredNorm();
    l    = std::max(l, -phi / dphi);
  }

  // upper bound
  double u = (d.cwiseInverse().cwiseProduct(J.transpose() * r)).stableNorm() / Delta;

  // it typically converges in 2 or 3 iterations
  for (auto i = 0u; i != 20; ++i) {
    // ensure alpha stays within bounds (and not equal to zero)
    if (!(l < alpha && alpha < u)) { alpha = std::max<double>(0.001 * u, sqrt(l * u)); }

    // solve least-squares problem
    using RType = std::conditional_t<is_sparse,
      Eigen::SparseMatrix<double, Eigen::RowMajor>,
      Eigen::Matrix<double, N, N>>;
    RType R(n, n);
    R.template topRows<NM_min>(nm_min) = J_qr.matrixR().template topRows<NM_min>(nm_min);
    if constexpr (!is_sparse) { R.template bottomRows<NM_rest>(nm_rest).setZero(); }
    x = solve_ls<N>(R, Qt_r, J_qr.colsPermutation(), sqrt(alpha) * d);
    if constexpr (is_sparse) { R.makeCompressed(); }

    // calculate phi
    D_x_iter      = d.cwiseProduct(x);
    D_x_iter_norm = D_x_iter.stableNorm();
    phi           = D_x_iter_norm - Delta;

    if (std::abs(phi) <= 0.1 * Delta) {
      break;  // condition fulfilled
    }

    // calculate derivative of phi wrt alpha
    Eigen::Matrix<double, N, 1> y =
      J_qr.colsPermutation().inverse() * (d.cwiseProduct(D_x_iter) / D_x_iter_norm);
    R.template triangularView<Eigen::Upper>().transpose().solveInPlace(y);
    dphi = -D_x_iter_norm * y.squaredNorm();

    // update bounds
    l = std::max<double>(l, alpha - phi / dphi);
    if (phi < 0) { u = alpha; }

    // update alpha
    alpha = alpha - ((phi + Delta) / Delta) * (phi / dphi);
  }

  return std::make_pair(alpha, std::move(x));
}

}  // namespace smooth::detail

#endif  // SMOOTH__INTERNAL__LMPAR_HPP_


#include <iostream>
#include <iomanip>

#include <Eigen/Dense>

#include "LSLOpt/BFGS.hpp"

#include "ModelSystem.hpp"

int main(int argc, char* argv[])
{
  int mode;
  if (argc == 0) {
    mode = 0;
  }
  else {
    try {
      mode = std::stoi(argv[1]);
    }
    catch (...) {
      mode = 0;
    }
  }

  ModelSystem modelSystem;

  std::cerr << std::setprecision(16);

  double angle_eps = 0.1 * M_PI / 180.0;

  unsigned n_steps = static_cast<unsigned>((2 * M_PI) / angle_eps) + 1;

  for (unsigned i = 0; i < n_steps; ++i) {
    double angle = angle_eps * i;
    double x = modelSystem.x0.norm() * std::sin(angle);
    double y = modelSystem.x0.norm() * std::cos(angle);
    Eigen::VectorXd x0 = Eigen::VectorXd::Zero(1);
    x0[0] = angle;

    LSLOpt::OptimizationParameters<double> params
        = LSLOpt::getOptimizationParameters<double>();
    LSLOpt::OstreamOutput output{LSLOpt::OutputLevel::Status, std::cout};

    LSLOpt::OptimizationResult<double> result;
    if (mode == 0) {
      result = LSLOpt::lsl_bfgs(modelSystem, x0, params, output);
    }
    else {
      result = LSLOpt::bfgs(modelSystem, x0, params, output);
    }

    double dist = sqrt(pow(modelSystem.to_cartesian(result.x)[0] - modelSystem.to_cartesian(x0)[0], 2)
                     + pow(modelSystem.to_cartesian(result.x)[1] - modelSystem.to_cartesian(x0)[1], 2));
    std::cerr << angle << ";" << x << ";" << y << ";" << result.function_value << ";" << dist << std::endl;
  }

  return 0;
}


// Original source code from:
// https://github.com/stegua/MyBlogEntries/tree/master/Dijkstra

/// My typedefs
#include <boost/cstdint.hpp>
#include <boost/integer_traits.hpp>
#include <inttypes.h>

typedef int32_t node_t;
typedef int32_t edge_t;
typedef int64_t cost_t;

/// From STL library
#include <fstream>

#include <vector>
using std::vector;

#include <string>

using std::make_pair;
using std::pair;

/// Lemon Graph Library
#include <lemon/smart_graph.h>
using lemon::SmartDigraph;

#include <lemon/adaptors.h>
#include <lemon/concepts/maps.h>
#include <lemon/dijkstra.h>
#include <lemon/path.h>
typedef SmartDigraph::Arc Arc;
typedef SmartDigraph::Node Node;
typedef SmartDigraph::ArcMap<cost_t> LengthMap;

#include <lemon/fib_heap.h>
typedef SmartDigraph::NodeMap<int> NodeMap;
typedef lemon::FibHeap<cost_t, NodeMap> FibonacciHeap;

/// Boost Timer
#include <boost/progress.hpp>
using boost::timer;

using namespace boost;

/// Read input data, build graph, and run Dijkstra
cost_t runDijkstra(char *argv[]) {
  /// Read instance from the OR-lib
  std::ifstream infile(argv[1]);
  if (!infile)
    exit(EXIT_FAILURE);

  int n; /// Number of variables
  int m; /// Number of constraints

  // reads file of the form
  // #nodes #edges
  // e_1 = v_i v_j cost[e_m]
  // ..
  // e_m = v_i v_j cost[e_m]

  /// Read the first line
  infile >> n >> m;
  fprintf(stdout, "n %d, m %d\t", n, m);
  /// Build the graph
  SmartDigraph G;
  G.reserveNode(n);
  G.reserveArc(m);
  vector<Node> vs;
  vs.reserve(n);
  for (int i = 0; i < n; ++i)
    vs.push_back(G.addNode());

  int v, w;
  cost_t c;
  cost_t T_dist;
  LengthMap C(G);
  for (int i = 0; i < m; i++) {
    infile >> v >> w >> c;
    Arc a;
    a = G.addArc(vs[v - 1], vs[w - 1]);
    C[a] = c;
  }

  timer TIMER;
  for (int i = 0; i < 50; ++i) {
    double t0 = TIMER.elapsed();
    Node S = vs[i];
    Node T = vs[n - 1 - i];
    // NodeMap heap_cross_ref(G);
    // FibonacciHeap heap(heap_cross_ref);
    // lemon::Dijkstra<SmartDigraph, LengthMap>::SetHeap<FibonacciHeap,
    // NodeMap>::Create spp(G, C); spp.heap( heap, heap_cross_ref );
    lemon::Dijkstra<SmartDigraph, LengthMap> spp(G, C);
    spp.run(S, T);
    T_dist = spp.dist(T);
    fprintf(stdout, "Time %.4f Cost %" PRId64 "\n", TIMER.elapsed() - t0,
            T_dist);
  }
  fprintf(stdout, "Tot %.4f\n", TIMER.elapsed());

  return T_dist;
}

/// Main function
int main(int argc, char **argv) {
  if (argc != 2) {
    fprintf(stdout, "usage: ./dijkstra <filename>\n");
    exit(EXIT_FAILURE);
  }
  /// Measure overall time
  timer TIMER;
  /// Invoke the different Dijkstra algorithm implementations
  cost_t T_dist = runDijkstra(argv);
  /// Print basic figures
  fprintf(stdout, "Cost %" PRId64 " - Time %.3f\n", T_dist, TIMER.elapsed());

  return 0;
}


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

#include "ellipsoid.h"

#include <Eigen/SVD>

namespace manipulability_metrics
{
Ellipsoid ellipsoidFromJacobian(const Eigen::Matrix<double, 6, Eigen::Dynamic>& jacobian)
{
  auto jac_svd = Eigen::JacobiSVD<Eigen::Matrix<double, 6, Eigen::Dynamic>>{ jacobian, Eigen::ComputeFullU };

  return { { { jac_svd.matrixU().col(0), jac_svd.singularValues()(0) },
             { jac_svd.matrixU().col(1), jac_svd.singularValues()(1) },
             { jac_svd.matrixU().col(2), jac_svd.singularValues()(2) },
             { jac_svd.matrixU().col(3), jac_svd.singularValues()(3) },
             { jac_svd.matrixU().col(4), jac_svd.singularValues()(4) },
             { jac_svd.matrixU().col(5), jac_svd.singularValues()(5) } } };
}
}  // namespace manipulability_metrics


#define DEBUG 1
/**
 * File    : F.cpp
 * Author  : Kazune Takahashi
 * Created : 5/19/2020, 3:57:45 PM
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator++() { return *this += 1; }
  Mint operator++(int)
  {
    Mint tmp{*this};
    ++*this;
    return tmp;
  }
  Mint &operator--() { return *this -= 1; }
  Mint operator--(int)
  {
    Mint tmp{*this};
    --*this;
    return tmp;
  }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
// ----- for C++17 -----
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL}; // or
// constexpr int infty{1'000'000'010};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

// ----- LCA -----

class LCA
{
  // helper classes
  struct Edge
  {
    // Initialized by initializer list. Take care fore the order of the field.
    int src, dst;
    ll cost;
  };

  struct Vertex
  {
    int depth;
    ll length;
  };

  // fields
  int N, root, L;
  vector<vector<Edge>> E;
  vector<Vertex> V;
  vector<vector<int>> ancestors;

  // methods
public:
  LCA(int N, int root = 0);
  void add_edge(int a, int b, ll c = 0);
  void init();
  void init(int root); // after adding all edges.

  // LCA
  int operator()(int a, int b);

  int depth(int a, int b) { return V[a].depth + V[b].depth - 2 * V[(*this)(a, b)].depth; }
  int depth(int v) { return depth(v, root); }
  ll length(int a, int b) { return V[a].length + V[b].length - 2 * V[(*this)(a, b)].length; }
  ll length(int v) { return length(v, root); }
  int parent(int v) { return ancestors[v][0]; }

private:
  void dfs(int v, int d = 0, ll l = 0, int p = -1);
};

// LCA: implement

LCA::LCA(int N, int root) : N{N}, root{root}, L{0}, E(N), V(N)
{
  while ((1 << L) < N)
  {
    ++L;
  }
  ancestors = vector<vector<int>>(N + 1, vector<int>(L, N));
}

void LCA::add_edge(int a, int b, ll c)
{
  E[a].push_back(LCA::Edge{a, b, c});
  E[b].push_back(LCA::Edge{b, a, c});
}

void LCA::init(int root)
{
  LCA::root = root;
  init();
}

void LCA::init()
{
  dfs(root);
  for (auto i = 0; i < L - 1; i++)
  {
    for (auto v = 0; v < N; v++)
    {
      if (ancestors[v][i] != -1)
      {
        ancestors[v][i + 1] = ancestors[ancestors[v][i]][i];
      }
    }
  }
}

int LCA::operator()(int a, int b)
{
  if (V[a].depth > V[b].depth)
  {
    swap(a, b);
  }
  int gap = V[b].depth - V[a].depth;
  for (auto i = L - 1; i >= 0; i--)
  {
    int len{1 << i};
    if (gap >= len)
    {
      gap -= len;
      b = ancestors[b][i];
    }
  }
  if (a == b)
  {
    return a;
  }
  for (auto i = L - 1; i >= 0; i--)
  {
    int na{ancestors[a][i]};
    int nb{ancestors[b][i]};
    if (na != nb)
    {
      a = na;
      b = nb;
    }
  }
  return ancestors[a][0];
}

void LCA::dfs(int v, int d, ll l, int p)
{
  if (p != -1)
  {
    ancestors[v][0] = p;
  }
  V[v].depth = d;
  V[v].length = l;
  for (auto const &e : E[v])
  {
    int u{e.dst};
    if (u == p)
    {
      continue;
    }
    dfs(u, d + 1, l + e.cost, v);
  }
}

// ----- main() -----

struct Query
{
  int id, color, y, coefficient;
};

struct Edge
{
  int src, dst, color, cost;
};

class Solve
{
  int N, Q;
  vector<vector<Edge>> V;
  vector<vector<Query>> W;
  LCA lca;
  vector<int> ans;
  vector<int> sum, cnt;

public:
  Solve(int N, int Q) : N{N}, Q{Q}, V(N), W(N), lca{N}, ans(Q), sum(N - 1, 0), cnt(N - 1, 0)
  {
    for (auto i = 0; i < N - 1; ++i)
    {
      int a, b, c, d;
      cin >> a >> b >> c >> d;
      --a;
      --b;
      --c;
      V[a].push_back({a, b, c, d});
      V[b].push_back({b, a, c, d});
      lca.add_edge(a, b, d);
    }
    lca.init(0);
    for (auto i = 0; i < Q; ++i)
    {
      int x, y, u, v;
      cin >> x >> y >> u >> v;
      --x;
      --u;
      --v;
      W[u].push_back({i, x, y, 1});
      W[v].push_back({i, x, y, 1});
      W[lca(u, v)].push_back({i, x, y, -2});
      ans[i] = lca.length(u, v);
    }
  }

  void flush()
  {
    dfs();
    for (auto i = 0; i < Q; ++i)
    {
      cout << ans[i] << endl;
    }
  }

private:
  void dfs(int u = 0, int p = -1)
  {
#if DEBUG == 1
    cerr << "u = " << u << ", p = " << p << endl;
#endif
    for (auto const &q : W[u])
    {
      ans[q.id] += q.coefficient * (-sum[q.color] + cnt[q.color] * q.y);
#if DEBUG == 1
      cerr << "ans[" << q.id << "] += " << q.coefficient << " * (-" << sum[q.color] << " + " << cnt[q.color] << " * " << q.y << ")" << endl;
#endif
    }
    for (auto const &e : V[u])
    {
      if (e.dst == p)
      {
        continue;
      }
      sum[e.color] += e.cost;
      cnt[e.color]++;
      dfs(e.dst, u);
      sum[e.color] -= e.cost;
      cnt[e.color]--;
    }
  }
};

int main()
{
  int N, Q;
  cin >> N >> Q;
  Solve solve(N, Q);
  solve.flush();
}


// -------------------------------------------------------------------------------------------------
//                              Copyright 2016 - NumScale SAS
//
//                   Distributed under the Boost Software License, Version 1.0.
//                        See accompanying file LICENSE.txt or copy at
//                            http://www.boost.org/LICENSE_1_0.txt
// -------------------------------------------------------------------------------------------------

#include <simd_bench.hpp>
#include <boost/simd/function/simd/ilog2.hpp>
#include <boost/simd/pack.hpp>

namespace nsb = ns::bench;
namespace bs =  boost::simd;

DEFINE_SIMD_BENCH(simd_ilog2, bs::ilog2);

DEFINE_BENCH_MAIN()
{
  nsb::for_each<simd_ilog2, NS_BENCH_IEEE_TYPES>(-10, 10);
}


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file linear_transition_test.hpp
 * \date October 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>
#include "../../typecast.hpp"

#include <Eigen/Dense>

#include <cmath>
#include <iostream>

#include <fl/util/types.hpp>
#include <fl/model/transition/linear_transition.hpp>

template <typename TestType>
class LinearTransitionTest:
    public testing::Test
{
public:
    enum: signed int
    {
        StateDim = TestType::Parameter::StateDim,
        InputDim = TestType::Parameter::InputDim,

        StateSize = fl::TestSize<StateDim, TestType>::Value,
        InputSize = fl::TestSize<InputDim, TestType>::Value
    };

    typedef Eigen::Matrix<fl::Real, StateDim, 1> State;
    typedef Eigen::Matrix<fl::Real, InputDim, 1> Input;
    typedef fl::LinearTransition<State, State, Input> LinearModel;

    typedef typename LinearModel::Noise Noise;

    LinearTransitionTest()
        : model()
    { }

    void init_dimension_test()
    {
        EXPECT_EQ(model.state_dimension(), StateDim);
        EXPECT_EQ(model.noise_dimension(), StateDim);
        EXPECT_EQ(model.input_dimension(), InputDim);

        EXPECT_EQ(model.dynamics_matrix().rows(), StateDim);
        EXPECT_EQ(model.dynamics_matrix().cols(), StateDim);

        EXPECT_EQ(model.noise_matrix().rows(), StateDim);
        EXPECT_EQ(model.noise_matrix().cols(), StateDim);
    }

    void init_dynamics_matrix_value_test()
    {
        EXPECT_TRUE(model.dynamics_matrix().isIdentity());
    }

    void init_noise_matrix_value_test()
    {
        EXPECT_TRUE(model.noise_matrix().isIdentity());
    }

    void dynamics_matrix_value_test()
    {
        auto dynamics_matrix = model.dynamics_matrix();
        dynamics_matrix.setRandom();
        model.dynamics_matrix(dynamics_matrix);
        EXPECT_TRUE(fl::are_similar(model.dynamics_matrix(), dynamics_matrix));
    }

    void noise_matrix_value_test()
    {
        auto noise_matrix = model.noise_matrix();
        noise_matrix.setRandom();
        model.noise_matrix(noise_matrix);
        EXPECT_TRUE(fl::are_similar(model.noise_matrix(), noise_matrix));
    }

    void expected_state_test()
    {
        auto x = State(model.state_dimension());
        auto u = Input(model.input_dimension());

        x.setRandom();
        u.setRandom();

        EXPECT_TRUE(
            fl::are_similar(
                model.expected_state(x, u),
                x + model.input_matrix() * u));
    }

    void state_with_zero_noise_test()
    {
        auto x = State(model.state_dimension());
        auto u = Input(model.input_dimension());
        auto w = Noise(model.noise_dimension());

        x.setRandom();
        u.setRandom();
        w.setZero();

        EXPECT_TRUE(
            fl::are_similar(
                model.state(x, w, u),
                x + model.input_matrix() * u));
    }

    void state_test()
    {
        auto x = State(model.state_dimension());
        auto u = Input(model.input_dimension());
        auto w = Noise(model.noise_dimension());

        x.setRandom();
        u.setRandom();
        w.setRandom();

        EXPECT_TRUE(
            fl::are_similar(
                model.state(x, w, u),
                x + model.input_matrix() * u + w));
    }

protected:
    LinearModel model;
};

template <int StateDimension, int InputDimension>
struct Dimensions
{
    enum: signed int
    {
        StateDim = StateDimension,
        InputDim = InputDimension
    };
};

typedef ::testing::Types<
            fl::StaticTest<Dimensions<2, 1>>,
            fl::StaticTest<Dimensions<2, 2>>,
            fl::StaticTest<Dimensions<3, 3>>,
            fl::StaticTest<Dimensions<10, 10>>,
            fl::StaticTest<Dimensions<10, 20>>,
            fl::StaticTest<Dimensions<100, 10>>,
            fl::StaticTest<Dimensions<3, 100>>,
            fl::StaticTest<Dimensions<100, 100>>,
            fl::DynamicTest<Dimensions<2, 1>>,
            fl::DynamicTest<Dimensions<2, 2>>,
            fl::DynamicTest<Dimensions<3, 3>>,
            fl::DynamicTest<Dimensions<10, 10>>,
            fl::DynamicTest<Dimensions<10, 20>>,
            fl::DynamicTest<Dimensions<100, 10>>,
            fl::DynamicTest<Dimensions<3, 100>>,
            fl::DynamicTest<Dimensions<100, 100>>
        > TestTypes;

TYPED_TEST_CASE(LinearTransitionTest, TestTypes);

TYPED_TEST(LinearTransitionTest, init_dimension)
{
    TestFixture::init_dimension_test();
}

TYPED_TEST(LinearTransitionTest, init_dynamics_matrix_value)
{
    TestFixture::init_dynamics_matrix_value_test();
}
TYPED_TEST(LinearTransitionTest, init_noise_matrix_value)
{
    TestFixture::init_noise_matrix_value_test();
}

TYPED_TEST(LinearTransitionTest, dynamics_matrix)
{
    TestFixture::dynamics_matrix_value_test();
}

TYPED_TEST(LinearTransitionTest, noise_matrix)
{
    TestFixture::noise_matrix_value_test();
}

TYPED_TEST(LinearTransitionTest, expected_state)
{
    TestFixture::expected_state_test();
}

TYPED_TEST(LinearTransitionTest, state_with_zero_noise)
{
    TestFixture::state_with_zero_noise_test();
}

TYPED_TEST(LinearTransitionTest, state)
{
    TestFixture::state_test();
}

/// \todo missing probability and log_probability tests


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/morton_dense.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
#include <boost/numeric/mtl/matrix/laplacian_setup.hpp>
#include <boost/numeric/mtl/recursion/predefined_masks.hpp>
#include <boost/numeric/mtl/operation/print.hpp>

using namespace std;

template <typename Matrix>
void test(Matrix& matrix, unsigned dim1, unsigned dim2, const char* name)
{
    cout << "\n" << name << "\n";
    mtl::mat::laplacian_setup(matrix, dim1, dim2);
    cout << "Laplacian matrix:\n" << matrix << "\n";

    if (dim1 > 1 && dim2 > 1) {
	typename mtl::Collection<Matrix>::value_type four(4.0), minus_one(-1.0), zero(0.0);
	MTL_THROW_IF(matrix[0][0] != four, mtl::runtime_error("wrong diagonal"));
	MTL_THROW_IF(matrix[0][1] != minus_one, mtl::runtime_error("wrong east neighbor"));
	MTL_THROW_IF(matrix[0][dim2] != minus_one, mtl::runtime_error("wrong south neighbor"));
	MTL_THROW_IF(dim2 > 2 && matrix[0][2] != zero, mtl::runtime_error("wrong zero-element"));
	MTL_THROW_IF(matrix[1][0] != minus_one, mtl::runtime_error("wrong west neighbor"));
	MTL_THROW_IF(matrix[dim2][0] != minus_one, mtl::runtime_error("wrong north neighbor"));
	MTL_THROW_IF(dim2 > 2 && matrix[2][0] != zero, mtl::runtime_error("wrong zero-element"));
    }
}

int main(int argc, char* argv[])
{
    using namespace mtl;

    unsigned dim1= 3, dim2= 4;

    if (argc > 2) {dim1= atoi(argv[1]);dim2= atoi(argv[2]);}
    unsigned size= dim1 * dim2;

    dense2D<double>                                      dr(size, size);
    dense2D<double, mat::parameters<col_major> >      dc(size, size);
    morton_dense<double, recursion::morton_z_mask>       mzd(size, size);
    morton_dense<double, recursion::doppled_2_row_mask>  d2r(size, size);
    compressed2D<double>                                 cr(size, size);
    compressed2D<double, mat::parameters<col_major> > cc(size, size);

    dense2D<complex<double> >                            drc(size, size);
    compressed2D<complex<double> >                       crc(size, size);

    test(dr, dim1, dim2, "Dense row major");
    test(dc, dim1, dim2, "Dense column major");
    test(mzd, dim1, dim2, "Morton Z-order");
    test(d2r, dim1, dim2, "Hybrid 2 row-major");
    test(cr, dim1, dim2, "Compressed row major");
    test(cc, dim1, dim2, "Compressed column major");
    test(drc, dim1, dim2, "Dense row major complex");
    test(crc, dim1, dim2, "Compressed row major complex");

    return 0;
}


// Copyright (C) 2012  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/svm.h>

#include "tester.h"

namespace
{

    using namespace test;
    using namespace dlib;
    using namespace std;

    logger dlog("test.active_learning");

// ----------------------------------------------------------------------------------------

    typedef matrix<double, 0, 1> sample_type;
    typedef radial_basis_kernel<sample_type> kernel_type;

// ----------------------------------------------------------------------------------------

    void make_dataset (
        std::vector<sample_type>& samples,
        std::vector<double>& labels
    )
    {
        for (int r = -10; r <= 10; ++r)
        {
            for (int c = -10; c <= 10; ++c)
            {
                sample_type samp(2);
                samp(0) = r;
                samp(1) = c;
                samples.push_back(samp);

                // if this point is less than 10 from the origin
                if (sqrt((double)r*r + c*c) <= 8)
                    labels.push_back(+1);
                else
                    labels.push_back(-1);

            }
        }

        vector_normalizer<sample_type> normalizer;
        normalizer.train(samples);
        for (unsigned long i = 0; i < samples.size(); ++i)
            samples[i] = normalizer(samples[i]);

        randomize_samples(samples, labels);

        /*
        cout << "samples.size(): " << samples.size() << endl;
        cout << "num +1 samples: "<< sum(mat(labels) > 0) << endl;
        cout << "num -1 samples: "<< sum(mat(labels) < 0) << endl;
        */

        empirical_kernel_map<kernel_type> ekm;
        ekm.load(kernel_type(0.15), samples);
        for (unsigned long i = 0; i < samples.size(); ++i)
            samples[i] = ekm.project(samples[i]);

        //cout << "dims: "<< ekm.out_vector_size() << endl;
    }

// ----------------------------------------------------------------------------------------

    double test_rank_unlabeled_training_samples (
        const std::vector<sample_type>& samples,
        const std::vector<double>& labels,
        active_learning_mode mode,
        int iterations,
        bool pick_front
    )
    {
        matrix<double,2,1> s;
        s = sum(mat(labels) > 0), sum(mat(labels) < 0);
        s /= labels.size();

        svm_c_linear_dcd_trainer<linear_kernel<sample_type> > trainer;
        trainer.set_c(25);

        const unsigned long initial_size = 1;
        std::vector<sample_type> tsamples(samples.begin(), samples.begin()+initial_size);
        std::vector<double> tlabels(labels.begin(), labels.begin()+initial_size);

        decision_function<linear_kernel<sample_type> > df;

        double random_score = 0;
        double active_learning_score = 0;
        for (int i = 0; i < iterations; ++i)
        {
            print_spinner();
            random_subset_selector<sample_type> sss = randomly_subsample(samples,50,i);
            random_subset_selector<double> ssl = randomly_subsample(labels,50,i);
            std::vector<unsigned long> results;

            results = rank_unlabeled_training_samples(trainer, tsamples, tlabels, sss, mode);

            const unsigned long idx = pick_front ? results.front() : results.back();
            tsamples.push_back(sss[idx]);
            tlabels.push_back(ssl[idx]);

            df = trainer.train(tsamples, tlabels);
            //cout << "tsamples.size(): " << tsamples.size() << endl;
            const unsigned long num = tsamples.size();
            const double active = test_binary_decision_function(df, samples, labels)*s;
            //cout << "test: "<< active;
            df = trainer.train(randomly_subsample(samples,num,i), randomly_subsample(labels,num,i));
            const double random = test_binary_decision_function(df, samples, labels)*s;
            //cout << "test: "<< random << endl;

            active_learning_score += active;
            random_score += random;

            //cout << "\n\n***********\n\n" << flush;
        }

        dlog << LINFO << "pick_front: " << pick_front << "   mode: "<< mode;
        dlog << LINFO << "active_learning_score: "<< active_learning_score;
        dlog << LINFO << "random_score:          "<< random_score;
        return active_learning_score / random_score;
    }

// ----------------------------------------------------------------------------------------

    class test_active_learning : public tester
    {
    public:
        test_active_learning (
        ) :
            tester ("test_active_learning",
                "Runs tests on the active learning components.")
        {}

        void perform_test (
        )
        {
            std::vector<sample_type> samples;
            std::vector<double> labels;
            print_spinner();
            make_dataset(samples, labels);
            dlog << LINFO << "samples.size(): "<< samples.size();

            // When we pick the best/front ranked element then the active learning method
            // shouldn't do much worse than random selection (and often much better).
            DLIB_TEST(test_rank_unlabeled_training_samples(samples, labels, max_min_margin, 25, true) >= 0.97);
            DLIB_TEST(test_rank_unlabeled_training_samples(samples, labels, ratio_margin, 25, true) >= 0.96);
            // However, picking the worst ranked element should do way worse than random
            // selection.
            DLIB_TEST(test_rank_unlabeled_training_samples(samples, labels, max_min_margin, 25, false) < 0.8);
            DLIB_TEST(test_rank_unlabeled_training_samples(samples, labels, ratio_margin, 25, false) < 0.8);
        }
    } a;

}


#include <Rcpp.h>
// [[Rcpp::plugins(cpp11)]]

// [[Rcpp::depends(BH)]]
#include <boost/numeric/odeint.hpp>
#include <boost/array.hpp>

using namespace boost::numeric::odeint;

unsigned int n;
Rcpp::NumericMatrix g_a;
Rcpp::NumericVector g_r;
std::vector<double> out_t;
std::vector< std::vector<double> > out_x;

// the model
void LV (const std::vector<double> &x,
         std::vector<double> &dxdt,
         double t) {
  for (int row = 0; row < g_a.nrow(); ++row) {
    double c = 0;
    for (int col = 0; col < g_a.ncol(); ++col) {
      // Rcpp::numericMatrix is indexed like this
      // matrixName(row, column)
      c =  c + g_a(row, col) * x[col];
    }
    dxdt[row] = x[row] * (g_r[row] - c);
  }
}

// the observer
void write_LV (const std::vector<double> &x , const double t) {
  // debugging output
  // for (unsigned int i = 0; i < x.size(); ++i) {
  //   std::cout << x[i] << " ";
  // }
  // std::cout << std::endl;

  out_t.push_back(t);

  for (unsigned int i = 0; i < x.size(); ++i) {
    out_x[i].push_back(x[i]);
  }
}

// creates the output object
Rcpp::List createOutput() {

  Rcpp::List out;
  out("Time") = Rcpp::wrap(out_t);
  for (unsigned int i = 0; i != n; ++i)
  {
    auto cnam = std::string("X") + std::to_string(i + 1);
    out(cnam) = Rcpp::wrap(out_x[i]);
  }
  out.attr("class") = "data.frame";
  int rows_out = out_t.size();
  auto rn = Rcpp::IntegerVector::create(NA_INTEGER, -rows_out);
  out.attr("row.names") = rn;
  return out;
}

// function that will be called from R
// [[Rcpp::export]]
Rcpp::List integrateModel(Rcpp::NumericVector init,
                                   Rcpp::NumericVector r,
                                   Rcpp::NumericMatrix a,
                                   double t0,
                                   double t1,
                                   double dt) {
  n = r.size();

  // initialize out_x
  out_x = std::vector< std::vector<double> >(n);
  for (unsigned int i = 0; i < n; ++i) {
    out_x[i] = std::vector<double>();
  }

  // initialize/reset out_t
  out_t = std::vector<double>();

  // assign global variables, needed for function LV
  g_a = a;
  g_r = r;

  // init must be casted to std::vector for integrate function
  std::vector<double> v_init = Rcpp::as< std::vector<double> >(init);

  // for debugging: is the order of row/col iteration right?
  // for (int i = 0; i < n; ++i) {
  //   std::cout << r[i] << " ";
  // }
  // std::cout << std::endl;
  //
  // for (int row = 0; row < a.nrow(); ++row) {
  //   for (int col = 0; col < a.ncol(); ++col) {
  //     std::cout << a(row, col) << " ";
  //   }
  // }
  // std::cout << std::endl;

  // this is the integration step
  // the function LV does the work
  // the observer function write_LV catches the state of the
  // stepper and pushes it to the global output variables
  integrate(LV, v_init, t0, t1, dt, write_LV);

  return createOutput();
}