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

};


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_REM_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SCALAR_REM_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/rem.hpp>
#include <boost/simd/include/functions/scalar/idivfix.hpp>
/////////////////////////////////////////////////////////////////////////////
//  The rem function computes the floating-point remainder of dividing x by y.
//  The return value is x - n * y, where n is the quotient of x / y, rounded
//  toward zero to an integer.
//  The fmod function is just an alias for the same thing.
/////////////////////////////////////////////////////////////////////////////

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::rem_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)(scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1) return a0%a1; else return a0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::rem_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)(scalar_< floating_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1) return a0-a1*idivfix(a0,a1);  else return a0;
    }
  };
} } }

#endif


//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());
    }
}


#include <gtest/gtest.h>
#include <gmock/gmock.h>

#include <waypoint_control/waypoint_controller.h>
#include <boost/math/constants/constants.hpp>

using boost::math::double_constants::pi;
using std::abs;
using std::sqrt;
using std::pow;

using namespace waypoint_control;

TEST(FeedbackTests, invalidOrientation)
{
  tf2::Transform robot;
  Waypoint goal;

  // Throw error if robot quaternion is invalid
  ASSERT_THROW(Feedback(robot, goal), std::runtime_error);

  // Set default orientation if waypoint quaternion is invalid
  robot.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
  Feedback feedback(robot, goal);
  EXPECT_NEAR(feedback.theta(), 0.0, 1.0e-6);
}

TEST(FeedbackTests, goalPoses)
{
  tf2::Transform robot;
  robot.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
  Waypoint goal;
  goal.pose.orientation.w = 1.0;
  Feedback feedback;

  goal.pose.position.x =  1.0;
  goal.pose.position.y =  0.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),     1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),     0.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(), 0.0, 1e-6);

  goal.pose.position.x =  1.0;
  goal.pose.position.y =  1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),      pi/4, 1e-6);

  goal.pose.position.x =  0.0;
  goal.pose.position.y =  1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           0.0, 1e-6);
  EXPECT_NEAR(feedback.y(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),      pi/2, 1e-6);

  goal.pose.position.x = -1.0;
  goal.pose.position.y =  1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),    3*pi/4, 1e-6);

  goal.pose.position.x = -1.0;
  goal.pose.position.y =  0.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),           0.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),        pi, 1e-6);

  goal.pose.position.x = -1.0;
  goal.pose.position.y = -1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),   -3*pi/4, 1e-6);

  goal.pose.position.x =  0.0;
  goal.pose.position.y = -1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           0.0, 1e-6);
  EXPECT_NEAR(feedback.y(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),     -pi/2, 1e-6);

  goal.pose.position.x =  1.0;
  goal.pose.position.y = -1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),     -pi/4, 1e-6);
}

TEST(Feedback, robotPoses)
{
  tf2::Transform robot;
  robot.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
  Waypoint goal;
  goal.pose.position.x =  1.0;
  goal.pose.position.y =  0.0;
  goal.pose.orientation.w = 1.0;
  Feedback feedback;

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi/2));
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),         0.0, 1e-6);
  EXPECT_NEAR(feedback.y(),        -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),         1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(),   -pi/2, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), -pi/2));
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),         0.0, 1e-6);
  EXPECT_NEAR(feedback.y(),         1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),         1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(),    pi/2, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), 3*pi/4));
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(), cos(-3*pi/4), 1e-6);
  EXPECT_NEAR(feedback.y(), sin(-3*pi/4), 1e-6);
  EXPECT_NEAR(feedback.r(),          1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(),  -3*pi/4, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), -3*pi/4));
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),  cos(3*pi/4), 1e-6);
  EXPECT_NEAR(feedback.y(),  sin(3*pi/4), 1e-6);
  EXPECT_NEAR(feedback.r(),         1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(),  3*pi/4, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi));
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),    -1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),     0.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     1.0, 1e-6);
  EXPECT_NEAR(feedback.theta(), -pi, 1e-6);
}

TEST(Feedback, robotAndGoalPoses)
{
  tf2::Transform robot;
  robot.setRotation(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
  Waypoint goal;
  goal.pose.orientation.w = 1.0;
  Feedback feedback;

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi/4));
  goal.pose.position.x =  1.0;
  goal.pose.position.y =  1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.y(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),       0.0, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi/2));
  goal.pose.position.x =  0.0;
  goal.pose.position.y =  1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.y(),     sqrt(0.0), 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),       0.0, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), -pi/2));
  goal.pose.position.x =   0.0;
  goal.pose.position.y =  -1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),           0.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),       0.0, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi));
  goal.pose.position.x =  -1.0;
  goal.pose.position.y =   0.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.y(),     sqrt(0.0), 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),       0.0, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi));
  goal.pose.position.x =  -1.0;
  goal.pose.position.y =  -1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.y(),     sqrt(1.0), 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),      pi/4, 1e-6);

  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi));
  goal.pose.position.x =  -1.0;
  goal.pose.position.y =   1.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),           1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),          -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),     -pi/4, 1e-6);

  robot.setOrigin(tf2::Vector3(2.0, 3.0, 0.0));
  robot.setRotation(tf2::Quaternion(tf2::Vector3(0, 0, 1), pi));
  goal.pose.position.x =  1.0;
  goal.pose.position.y =  2.0;
  feedback = Feedback(robot, goal);
  EXPECT_NEAR(feedback.x(),         -1.0, 1e-6);
  EXPECT_NEAR(feedback.y(),         -1.0, 1e-6);
  EXPECT_NEAR(feedback.r(),     sqrt(2.0), 1e-6);
  EXPECT_NEAR(feedback.theta(),      pi/4, 1e-6);
}

// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


// Filename: matrix_indirect.cpp (part of MTL4)
#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main(int, char**)
{
    typedef mtl::dense2D<double> matrix;
    matrix A(5, 3);
    hessian_setup(A, 1.0);

    mtl::iset rows, cols;
    rows= 2, 0, 3; cols= 2, 1;

    cout << "rows = " << rows << ", cols = " << cols << "\n"
	 << "The sub-matrix A[{2, 0, 3}][{2, 1}] is\n" << A[rows][cols];

    mtl::mat::indirect<matrix> B(A[rows][cols]);
    cout << "B is\n" << B;

    return 0;
}


/***************************************************************************
 *  @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];
        }
    }
}

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

}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

#include <NTL/ZZ.h>

#include <helib/helib.h>
#include <helib/permutations.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {

struct Parameters
{

  long test;
  long m;
  long p;
  long r;
  long depth;
  long L;
  long ord1;
  long ord2;
  long ord3;
  long ord4;
  long good1;
  long good2;
  long good3;
  long good4;

  Parameters(long test,
             long m,
             long p,
             long r,
             long depth,
             long L,
             long ord1,
             long ord2,
             long ord3,
             long ord4,
             long good1,
             long good2,
             long good3,
             long good4) :
      test(test),
      m(m),
      p(p),
      r(r),
      depth(depth),
      L(L),
      ord1(ord1),
      ord2(ord2),
      ord3(ord3),
      ord4(ord4),
      good1(good1),
      good2(good2),
      good3(good3),
      good4(good4){};

  // Let googletest know how to print the Parameters
  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "test=" << params.test << ","
              << "p=" << params.p << ","
              << "r=" << params.r << ","
              << "m=" << params.m << ","
              << "depth=" << params.depth << ","
              << "L=" << params.L << ","
              << "ord1=" << params.ord1 << ","
              << "ord2=" << params.ord2 << ","
              << "ord3=" << params.ord3 << ","
              << "ord4=" << params.ord4 << ","
              << "good1=" << params.good1 << ","
              << "good2=" << params.good2 << ","
              << "good3=" << params.good3 << ","
              << "good4=" << params.good4 << "}";
  };
};

class GTestPermutations : public ::testing::TestWithParam<Parameters>
{
protected:
  GTestPermutations() :
      test(GetParam().test),
      m(GetParam().m),
      p(GetParam().p),
      r(GetParam().r),
      depth(GetParam().depth),
      L(GetParam().L),
      ord1(GetParam().ord1),
      ord2(GetParam().ord2),
      ord3(GetParam().ord3),
      ord4(GetParam().ord4),
      good1(GetParam().good1),
      good2(GetParam().good2),
      good3(GetParam().good3),
      good4(GetParam().good4){};

  long test;
  long m;
  long p;
  long r;
  long depth;
  long L;
  long ord1;
  long ord2;
  long ord3;
  long ord4;
  long good1;
  long good2;
  long good3;
  long good4;

  virtual void SetUp() override { helib::setDryRun(helib_test::dry); };

  virtual void TearDown() override { helib::cleanupGlobals(); }
};

void testCube(NTL::Vec<helib::GenDescriptor>& vec, long widthBound)
{
  helib::GeneratorTrees trees;
  long cost = trees.buildOptimalTrees(vec, widthBound);
  if (!helib_test::noPrint) {
    std::cout << "@TestCube: trees=" << trees << std::endl;
    std::cout << " cost =" << cost << std::endl;
  }
  NTL::Vec<long> dims;
  trees.getCubeDims(dims);
  helib::CubeSignature sig(dims);

  for (long cnt = 0; cnt < 3; cnt++) {
    helib::Permut pi;
    helib::randomPerm(pi, trees.getSize());

    helib::PermNetwork net;
    net.buildNetwork(pi, trees);

    helib::HyperCube<long> cube1(sig), cube2(sig);
    for (long i = 0; i < cube1.getSize(); i++)
      cube1[i] = i;
    helib::HyperCube<long> cube3 = cube1;
    helib::applyPermToVec(
        cube2.getData(), cube1.getData(), pi); // direct application
    net.applyToCube(cube3);                    // applying permutation netwrok

    const auto getErrorMessage = [&cube1, &cube2, &cube3]() {
      std::ostringstream os;
      if (cube1.getSize() < 100 && !helib_test::noPrint)
        os << "in=" << cube1.getData() << std::endl
           << "out1=" << cube2.getData() << ", out2=" << cube3.getData()
           << std::endl
           << std::endl;
      return os;
    };

    ASSERT_EQ(cube2, cube3) << getErrorMessage().str();
  }
}

void testCtxt(long m, long p, long widthBound = 0, long L = 0, long r = 1);

void testCtxt(long m, long p, long widthBound, long L, long r)
{
  if (!helib_test::noPrint)
    std::cout << "@testCtxt(m=" << m << ",p=" << p << ",depth=" << widthBound
              << ",r=" << r << ")";

  helib::Context context(m, p, r);
  helib::EncryptedArray ea(context); // Use G(X)=X for this ea object

  // Some arbitrary initial plaintext array
  std::vector<long> in(ea.size());
  for (long i = 0; i < ea.size(); i++)
    in[i] = i % p;

  // Setup generator-descriptors for the PAlgebra generators
  NTL::Vec<helib::GenDescriptor> vec(NTL::INIT_SIZE, ea.dimension());
  for (long i = 0; i < ea.dimension(); i++)
    vec[i] = helib::GenDescriptor(/*order=*/ea.sizeOfDimension(i),
                                  /*good=*/ea.nativeDimension(i),
                                  /*genIdx=*/i);

  // Some default for the width-bound, if not provided
  if (widthBound <= 0)
    widthBound = 1 + log2((double)ea.size());

  // Get the generator-tree structures and the corresponding hypercube
  helib::GeneratorTrees trees;
  long cost = trees.buildOptimalTrees(vec, widthBound);
  if (!helib_test::noPrint) {
    context.zMStar.printout();
    std::cout << ": trees=" << trees << std::endl;
    std::cout << " cost =" << cost << std::endl;
  }
  //  NTL::Vec<long> dims;
  //  trees.getCubeDims(dims);
  //  helib::CubeSignature sig(dims);

  // 1/2 prime per level should be more or less enough, here we use 1 per layer
  if (L <= 0)
    L = (1 + trees.numLayers()) * context.BPL();
  helib::buildModChain(context, /*nLevels=*/L, /*nDigits=*/3);
  if (!helib_test::noPrint)
    std::cout << "**Using " << L << " and " << context.ctxtPrimes.card()
              << " Ctxt-primes)\n";

  // Generate a sk/pk pair
  helib::SecKey secretKey(context);
  const helib::PubKey& publicKey = secretKey;
  secretKey.GenSecKey(); // A +-1/0 secret key
  helib::Ctxt ctxt(publicKey);

  for (long cnt = 0; cnt < 3; cnt++) {
    helib::resetAllTimers();
    // Choose a random permutation
    helib::Permut pi;
    helib::randomPerm(pi, trees.getSize());

    // Build a permutation network for pi
    helib::PermNetwork net;
    net.buildNetwork(pi, trees);

    // make sure we have the key-switching matrices needed for this network
    helib::addMatrices4Network(secretKey, net);

    // Apply the permutation pi to the plaintext
    std::vector<long> out1(ea.size());
    std::vector<long> out2(ea.size());
    helib::applyPermToVec(out1, in, pi); // direct application

    // Encrypt plaintext array, then apply permutation network to ciphertext
    ea.encrypt(ctxt, publicKey, in);
    if (!helib_test::noPrint)
      std::cout << "  ** applying permutation network to ciphertext... "
                << std::flush;
    double t = NTL::GetTime();
    net.applyToCtxt(ctxt, ea); // applying permutation netwrok
    t = NTL::GetTime() - t;
    if (!helib_test::noPrint)
      std::cout << "done in " << t << " seconds" << std::endl;
    ea.decrypt(ctxt, secretKey, out2);

    ASSERT_EQ(out1, out2);
    // printAllTimers();
  }
}

/* m = 31, p = 2, phi(m) = 30
  ord(p)=5
  generator 6 has order (== Z_m^*) of 6
  T = [1 6 5 30 25 26 ]

  m = 61, p = 3, phi(m) = 60
  ord(p)=10
  generator 13 has order (== Z_m^*) of 3
  generator 2 has order (!= Z_m^*) of 2
  T = [1 2 13 26 47 33 ]

  m = 683, p = 2, phi(m) = 682
  ord(p)=22
  generator 3 has order (== Z_m^*) of 31

  m = 47127, p = 2, phi(m) = 30008
  ord(p)=22
  generator 5 has order (== Z_m^*) of 682
  generator 13661 has order (== Z_m^*) of 2
*/

TEST_P(GTestPermutations, ciphertextPermutations)
{
  if (test == 0 || helib_test::dry != 0) {
    NTL::Vec<helib::GenDescriptor> vec;
    long nGens;
    if (ord2 <= 1)
      nGens = 1;
    else if (ord3 <= 1)
      nGens = 2;
    else if (ord4 <= 1)
      nGens = 3;
    else
      nGens = 4;
    vec.SetLength(nGens);

    switch (nGens) {
    case 4:
      vec[3] = helib::GenDescriptor(ord4, good4, /*genIdx=*/3);
    case 3:
      vec[2] = helib::GenDescriptor(ord3, good3, /*genIdx=*/2);
    case 2:
      vec[1] = helib::GenDescriptor(ord2, good2, /*genIdx=*/1);
    default:
      vec[0] = helib::GenDescriptor(ord1, good1, /*genIdx=*/0);
    }
    if (!helib_test::noPrint) {
      std::cout << "***Testing ";
      if (helib::isDryRun())
        std::cout << "(dry run) ";
      for (long i = 0; i < vec.length(); i++)
        std::cout << "(" << vec[i].order << "," << vec[i].good << ")";
      std::cout << ", depth=" << depth << "\n";
    }
    ASSERT_NO_FATAL_FAILURE(testCube(vec, depth));
  } else {
    helib::setTimersOn();
    if (!helib_test::noPrint) {
      std::cout << "***Testing m=" << m << ", p=" << p << ", depth=" << depth
                << std::endl;
    }
    ASSERT_NO_FATAL_FAILURE(testCtxt(m, p, depth, L, r));
  }
};

INSTANTIATE_TEST_SUITE_P(
    defaultParameters,
    GTestPermutations,
    ::testing::Values(
        // FAST
        // Parameters(1, 91, 2, 1, 5, 0, 30, 0, 0, 0, 1, 1, 1, 1)
        // SLOW
        Parameters(1, 4369, 2, 1, 5, 0, 30, 0, 0, 0, 1, 1, 1, 1)));

} // namespace

#if 0
  std::cout << "***Testing m=31, p=2, width=3\n"; // (6 good)
  testCtxt(/*m=*/31, /*p=*/2, /*width=*/3);

  std::cout << "\n***Testing m=61, p=3, width=3\n"; // (3 good), (2, bad)
  testCtxt(/*m=*/61, /*p=*/3, /*width=*/3);

  std::cout << "\n***Testing m=683, p=2, width=5\n"; // (31, good)
  testCtxt(/*m=*/683, /*p=*/2, /*width=*/5);

  //  std::cout << "\n***Testing m=47127, p=2, width=11\n"; // (682,good),(2,good)
  //  testCtxt(/*m=*/47127, /*p=*/2, /*width=*/11);

  // Test 1: a single good small prime-order generator (3)
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 1);
  vec[0] = helib::GenDescriptor(/*order=*/3, /*good=*/true, /*genIdx=*/0);
  std::cout << "***Testing (3,good), width=1\n";
  testCube(vec, /*width=*/1);
  }

  // Test 2: a single bad larger prime-order generator (31)
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 1);
  vec[0] = helib::GenDescriptor(/*order=*/31, /*good=*/false, /*genIdx=*/0);
  std::cout << "\n***Testing (31,bad), width=5\n";
  testCube(vec, /*width=*/5);
  }

  // Test 3: two generators with small prime orders (2,3), both bad
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 2);
  vec[0] = helib::GenDescriptor(/*order=*/2, /*good=*/false, /*genIdx=*/0);
  vec[1] = helib::GenDescriptor(/*order=*/3, /*good=*/false, /*genIdx=*/1);
  std::cout << "\n***Testing [(2,bad),(3,bad)], width=3\n";
  testCube(vec, /*width=*/3);
  }

  // Test 4: two generators with small prime orders (2,3), one good
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 2);
  vec[0] = helib::GenDescriptor(/*order=*/3, /*good=*/true, /*genIdx=*/0);
  vec[1] = helib::GenDescriptor(/*order=*/2, /*good=*/false, /*genIdx=*/1);
  std::cout << "\n***Testing [(3,good),(2,bad)], width=3\n";
  testCube(vec, /*width=*/3);
  }

  // Test 5: a single good composite-order generator (6)
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 1);
  vec[0] = helib::GenDescriptor(/*order=*/6, /*good=*/true, /*genIdx=*/0);
  std::cout << "\n***Testing (6,good), width=3\n";
  testCube(vec, /*width=*/3);
  }

  // Test 6: (6,good),(2,bad)
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 2);
  vec[0] = helib::GenDescriptor(/*order=*/6,/*good=*/true, /*genIdx=*/0);
  vec[1] = helib::GenDescriptor(/*order=*/ 2, /*good=*/false,/*genIdx=*/1);
  std::cout << "\n**Testing [(6,good),(2,bad)], width=5\n";
  testCube(vec, /*width=*/5);
  }

  // Test 7: the "general case", (682,good),(2,bad)
  {
  NTL::Vec<helib::GenDescriptor> vec(INIT_SIZE, 2);
  vec[0] = helib::GenDescriptor(/*order=*/682,/*good=*/true, /*genIdx=*/0);
  vec[1] = helib::GenDescriptor(/*order=*/ 2, /*good=*/false,/*genIdx=*/1);
  std::cout << "\n**Testing [(682,good),(2,bad)], width=11\n";
  testCube(vec, /*width=*/11);
  }
#endif


/*******************************************************************************
Copyright (c) 2011, Dr. D. Studios
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

Redistributions of source code must retain the above copyright notice, this list
of conditions and the following disclaimer.
Redistributions in binary form must reproduce the above copyright notice, this
list of conditions and the following disclaimer in the documentation and/or other
materials provided with the distribution.
Neither the name of the Dr. D. Studios nor the names of its contributors may be
used to endorse or promote products derived from this software without specific
prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

#ifndef _PIMATH_FRAME__H_
#define _PIMATH_FRAME__H_

#include <boost/python.hpp>
#include <ImathFrame.h>
#include "util.h"

namespace pimath
{
	namespace bp = boost::python;

	template<typename T>
	struct FrameBind
	{
		typedef Imath::Vec3<T> 		vec3_type;
		typedef Imath::Matrix44<T> 	mat44_type;

		FrameBind()
		{
			bp::def("firstFrame", &Imath::firstFrame<T>);
			bp::def("nextFrame", &Imath::nextFrame<T>);
			bp::def("lastFrame", &Imath::lastFrame<T>);
		}
	};
}

#endif


#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;
}


// Copyright 2022 Haruki Uchiito
//
// 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 NDT_2D_SLAM__DATA_TYPES_HPP_
#define NDT_2D_SLAM__DATA_TYPES_HPP_

#include <Eigen/Core>
#include <algorithm>
#include <iostream>
#include <utility>
#include <vector>

namespace NDT2DSLAM {

/**
 * @brief Transform2D represensts homogeneous transformation in 2D space
 */
struct Transform2D {
    // parameters
    double tx, ty;  // translation [m]
    double theta;   // rotation [rad]

    // homogeneous transformation matrix type
    typedef Eigen::Matrix<double, 3, 3> TransformMat;
    TransformMat matrix;

    void setMatrix() {
        double ct = cos(theta);
        double st = sin(theta);
        matrix << ct, -st, tx, st, ct, ty, 0, 0, 1;
    }

    Transform2D()
        : tx(0.0), ty(0.0), theta(0.0), matrix(TransformMat::Identity()) {}
    Transform2D(double tx_, double ty_, double theta_)
        : tx(tx_), ty(ty_), theta(theta_) {
        setMatrix();
    }
    Transform2D(const Transform2D& another)
        : tx(another.tx),
          ty(another.ty),
          theta(another.theta),
          matrix(another.matrix) {}

    Transform2D& operator+=(const Transform2D& another) {
        tx += another.tx;
        ty += another.ty;
        theta += another.theta;
        setMatrix();
        return *this;
    }
    Transform2D& operator=(const Transform2D& another) {
        tx = another.tx;
        ty = another.ty;
        theta = another.theta;
        setMatrix();
        return *this;
    }
    friend std::ostream& operator<<(std::ostream& os,
                                    const Transform2D& transform) {
        os << "Transform2D tx: " << transform.tx << ", ty: " << transform.ty
           << ", theta: " << transform.theta;
        return os;
    }
};

/**
 * @brief Scan2D represensts pointcloud in 2D space
 */
struct Scan2D {
    // scan points container
    // each row contains positions of each points
    // < x1 x2 ...
    //   y1 y2 ...
    //    1  1 ... >
    // last lines is required for homogeneous transformation of points.
    // data is stored in row-major order for ease of entire row access.
    typedef Eigen::Matrix<double, 3, Eigen::Dynamic, Eigen::RowMajor> ScanMat;

    ScanMat points;

    Scan2D() {}
    explicit Scan2D(size_t n) : points(ScanMat::Constant(3, n, 1.0)) {}
    Scan2D(const Scan2D& scan) : points(scan.points) {}
    /**
     * @brief Construct a new Scan2D object from std::vector containing
     * positions
     * @param[in] x, y position of scan points
     */
    Scan2D(const std::vector<double>& x, const std::vector<double>& y) {
        int pNum = static_cast<int>(x.size());
        if (pNum == 0)
            throw std::invalid_argument("input size must be over zero");
        if (pNum != static_cast<int>(y.size()))
            throw std::invalid_argument("size of vector x and y must be same");

        points = ScanMat::Constant(3, pNum, 1.0);
        for (int i = 0; i < pNum; ++i) {
            points(0, i) = x[i];
            points(1, i) = y[i];
        }
    }

    Scan2D transformBy(const Transform2D& transform) const {
        Scan2D ret(*this);
        ret.points = transform.matrix * points;
        return ret;
    }

    Scan2D& operator+=(const Scan2D& another) {
        size_t pcol = points.cols(), acol = another.points.cols();
        size_t ncol = pcol + acol;
        ScanMat npoints = ScanMat::Constant(3, ncol, 1.0);
        std::copy(points.data(), points.data() + pcol, npoints.data());
        std::copy(points.data() + pcol, points.data() + pcol + pcol,
                  npoints.data() + ncol);
        // copy another
        std::copy(another.points.data(), another.points.data() + acol,
                  npoints.data() + pcol);  // x
        std::copy(another.points.data() + acol,
                  another.points.data() + acol + acol,
                  npoints.data() + ncol + pcol);  // y

        // npoints << points, another.points;
        points = npoints;
        return *this;
    }

    Scan2D& operator=(const Scan2D& another) {
        points = another.points;
        return *this;
    }

    std::vector<double> getRow(int i) const {
        auto start = points.data() + points.cols() * i;
        std::vector<double> ret(start, start + points.cols());
        return ret;
    }
    std::vector<double> getXVec() const { return getRow(0); }
    std::vector<double> getYVec() const { return getRow(1); }
    double getX(int index) const { return points(0, index); }
    double getY(int index) const { return points(1, index); }
    size_t size() const { return points.cols(); }
};

}  // namespace NDT2DSLAM

#endif  // NDT_2D_SLAM__DATA_TYPES_HPP_


#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;
  }

}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <iostream>
#include <fstream>

#include <boost/graph/connected_components.hpp>

typedef CGAL::Simple_cartesian<double>                       Kernel;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Surface_mesh<Point>                            Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;

int main(int argc, char* argv[])
{
  const std::string filename = (argc > 1) ? argv[1] : CGAL::data_file_path("meshes/prim.off");

  Mesh sm;
  if(!CGAL::IO::read_polygon_mesh(filename, sm))
  {
    std::cerr << "Invalid input." << std::endl;
    return 1;
  }

  Mesh::Property_map<vertex_descriptor,int> ccmap;
  ccmap = sm.add_property_map<vertex_descriptor,int>("v:CC").first;

  int num = connected_components(sm, ccmap);
  std::cout  << num << " connected components" << std::endl;
  for(vertex_descriptor v : vertices(sm)){
    std::cout  << v << " is in component " << ccmap[v] << std::endl;
  }

  return 0;
}